I have tried to make a simple ball bouncing problem in drake, but the ball is not bouncing it hits the ground and keeps moving forward. I guess there is no restitution behavior of any sort. the ball's vertical velocity seems to be lost. what might be the problem.
import numpy as np
import matplotlib.pyplot as plt
from pydrake.all import (
MultibodyPlant, DiagramBuilder, RigidTransform,
SpatialInertia, UnitInertia, CoulombFriction,
AddMultibodyPlantSceneGraph, Simulator
)
from pydrake.geometry import Box, Sphere
from pydrake.multibody.math import SpatialVelocity
def create_bouncing_ball_sim():
builder = DiagramBuilder()
# Discrete plant with small time step
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0001)
# Ground
ground_shape = Box(2.0, 2.0, 0.1)
ground_pose = RigidTransform([0, 0, -0.05])
plant.RegisterCollisionGeometry(
plant.world_body(), ground_pose,
ground_shape, "ground_collision",
CoulombFriction(0.9, 0.8)
)
plant.RegisterVisualGeometry(
plant.world_body(), ground_pose,
ground_shape, "ground_visual",
[0.5, 0.5, 0.5, 1.0]
)
# Ball
radius = 0.05
mass = 0.1
inertia = UnitInertia.SolidSphere(radius)
spatial_inertia = SpatialInertia(mass=mass, p_PScm_E=np.zeros(3), G_SP_E=inertia)
ball_body = plant.AddRigidBody("ball", spatial_inertia)
plant.RegisterCollisionGeometry(
ball_body, RigidTransform(),
Sphere(radius), "ball_collision",
CoulombFriction(0.8, 0.6)
)
plant.RegisterVisualGeometry(
ball_body, RigidTransform(),
Sphere(radius), "ball_visual",
[0.8, 0.1, 0.1, 1.0]
)
# Gravity
plant.mutable_gravity_field().set_gravity_vector([0, 0, -9.81])
plant.Finalize()
diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant_context = plant.GetMyMutableContextFromRoot(context)
plant.SetFreeBodyPose(plant_context, ball_body, RigidTransform([0, 0, 1.0]))
plant.SetFreeBodySpatialVelocity(ball_body, SpatialVelocity(np.zeros(3), np.zeros(3)), plant_context)
simulator = Simulator(diagram, context)
simulator.set_target_realtime_rate(1.0)
return simulator, plant, ball_body
# Run simulation
simulator, plant, ball_body = create_bouncing_ball_sim()
# Simulate and collect data
time_steps = []
z_positions = []
sim_time = 2.0
dt = 0.001
simulator.Initialize()
while simulator.get_context().get_time() < sim_time:
context = simulator.get_context()
plant_context = plant.GetMyContextFromRoot(context)
pose = plant.EvalBodyPoseInWorld(plant_context, ball_body)
z = pose.translation()[2]
t = context.get_time()
z_positions.append(z)
time_steps.append(t)
simulator.AdvanceTo(t + dt)
# Plotting
plt.figure(figsize=(8, 4))
plt.plot(time_steps, z_positions, label="Ball height (z)")
plt.xlabel("Time [s]")
plt.ylabel("Height [m]")
plt.title("Bouncing Ball Simulation in Drake")
plt.grid(True)
plt.legend()
plt.show()
I expected a bouncy behavior but the motion got a flat horizontal path after hitting ground
The simplest way to get things to bounce is to tweak your contact material properties. Specifically: stiffness and dissipation.
You haven't assigned anything to those values. In the absence of explicit assignment the default values (stiffness = 1e6 and dissipation = 50) get applied. If, for example, I set those values to 1e3 and 0.1, respectively, I get a bounce.
from pydrake.geometry import ProximityProperties, AddContactMaterial
...
props = ProximityProperties()
AddContactMaterial(dissipation=0.1, point_stiffness=1e3,
friction=CoulombFriction(0.8, 0.6), properties=props)
plant.RegisterCollisionGeometry(
ball_body, RigidTransform(),
Sphere(radius), "ball_collision",
props
)
There are some other issues that I don't have answers to:
Are these parameters "physical"? Probably not.
Why doesn't it bounce forever if dissipation goes to zero? Most likely there are other, systemic sources of dissipation.