def test_contact_applet_hydroelastic(self):
"""Check that _ContactApplet doesn't crash when receiving hydroelastic
messages.
"""
# Create the device under test.
dut = mut.Meldis()
meshcat = dut.meshcat
lcm = dut._lcm
# Enqueue a hydroelastic contact message.
sdf_file = FindResourceOrThrow(
"drake/multibody/meshcat/test/hydroelastic.sdf")
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.001)
parser = Parser(plant=plant)
parser.AddModelFromFile(sdf_file)
body1 = plant.GetBodyByName("body1")
body2 = plant.GetBodyByName("body2")
plant.AddJoint(
PrismaticJoint(name="body1",
frame_on_parent=plant.world_body().body_frame(),
frame_on_child=body1.body_frame(),
axis=[0, 0, 1]))
plant.AddJoint(
PrismaticJoint(name="body2",
frame_on_parent=plant.world_body().body_frame(),
frame_on_child=body2.body_frame(),
axis=[1, 0, 0]))
plant.Finalize()
ConnectContactResultsToDrakeVisualizer(builder=builder,
plant=plant,
scene_graph=scene_graph,
lcm=lcm)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant.SetPositions(plant.GetMyMutableContextFromRoot(context),
[0.1, 0.3])
diagram.Publish(context)
# The geometry isn't registered until the load is processed.
hydro_path = "/CONTACT_RESULTS/hydroelastic/" + \
"body1.two_bodies::body1_collision+body2"
hydro_path2 = "/CONTACT_RESULTS/hydroelastic/" + \
"body1.two_bodies::body1_collision2+body2"
self.assertEqual(meshcat.HasPath(hydro_path), False)
self.assertEqual(meshcat.HasPath(hydro_path2), False)
# Process the load + draw; contact results should now exist.
lcm.HandleSubscriptions(timeout_millis=1)
dut._invoke_subscriptions()
self.assertEqual(meshcat.HasPath("/CONTACT_RESULTS/hydroelastic"),
True)
self.assertEqual(meshcat.HasPath(hydro_path), True)
self.assertEqual(meshcat.HasPath(hydro_path2), True)
def test_contact_applet_point_pair(self):
"""Check that _ContactApplet doesn't crash when receiving point
contact messages.
"""
# Create the device under test.
dut = mut.Meldis()
meshcat = dut.meshcat
lcm = dut._lcm
# Enqueue a point contact result message.
sdf_file = FindResourceOrThrow(
"drake/examples/manipulation_station/models/sphere.sdf")
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.001)
parser = Parser(plant=plant)
sphere1_model = parser.AddModelFromFile(sdf_file, "sphere1")
sphere2_model = parser.AddModelFromFile(sdf_file, "sphere2")
body1 = plant.GetBodyByName("base_link", sphere1_model)
body2 = plant.GetBodyByName("base_link", sphere2_model)
plant.AddJoint(
PrismaticJoint(name="sphere1_x",
frame_on_parent=plant.world_body().body_frame(),
frame_on_child=body1.body_frame(),
axis=[1, 0, 0]))
plant.AddJoint(
PrismaticJoint(name="sphere2_x",
frame_on_parent=plant.world_body().body_frame(),
frame_on_child=body2.body_frame(),
axis=[1, 0, 0]))
plant.Finalize()
ConnectContactResultsToDrakeVisualizer(builder=builder,
plant=plant,
scene_graph=scene_graph,
lcm=lcm)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant.SetPositions(plant.GetMyMutableContextFromRoot(context),
[-0.03, 0.03])
diagram.Publish(context)
# The geometry isn't registered until the load is processed.
pair_path = "/CONTACT_RESULTS/point/base_link(2)+base_link(3)"
self.assertEqual(meshcat.HasPath(pair_path), False)
# Process the load + draw; make sure the geometry exists now.
lcm.HandleSubscriptions(timeout_millis=0)
dut._invoke_subscriptions()
self.assertEqual(meshcat.HasPath(pair_path), True)
def add_box_at_location(mbp,
name,
color,
pose,
mass=0.25,
inertia=UnitInertia(3E-3, 3E-3, 3E-3)):
''' Adds a 5cm cube at the specified pose. Uses a planar floating base
in the x-z plane. '''
no_mass_no_inertia = SpatialInertia(mass=0.,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia(0., 0., 0.))
body_mass_and_inertia = SpatialInertia(mass=mass,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=inertia)
shape = Box(0.05, 0.05, 0.05)
model_instance = mbp.AddModelInstance(name)
body = mbp.AddRigidBody(name, model_instance, body_mass_and_inertia)
RegisterVisualAndCollisionGeometry(mbp, body, RigidTransform(), shape,
name, color, CoulombFriction(0.9, 0.8))
body_pre_z = mbp.AddRigidBody("{}_pre_z".format(name), model_instance,
no_mass_no_inertia)
body_pre_theta = mbp.AddRigidBody("{}_pre_theta".format(name),
model_instance, no_mass_no_inertia)
world_carrot_origin = mbp.AddFrame(frame=FixedOffsetFrame(
name="world_{}_origin".format(name), P=mbp.world_frame(), X_PF=pose))
body_joint_x = PrismaticJoint(name="{}_x".format(name),
frame_on_parent=world_carrot_origin,
frame_on_child=body_pre_z.body_frame(),
axis=[1, 0, 0],
damping=0.)
mbp.AddJoint(body_joint_x)
body_joint_z = PrismaticJoint(name="{}_z".format(name),
frame_on_parent=body_pre_z.body_frame(),
frame_on_child=body_pre_theta.body_frame(),
axis=[0, 0, 1],
damping=0.)
mbp.AddJoint(body_joint_z)
body_joint_theta = RevoluteJoint(
name="{}_theta".format(name),
frame_on_parent=body_pre_theta.body_frame(),
frame_on_child=body.body_frame(),
axis=[0, 1, 0],
damping=0.)
mbp.AddJoint(body_joint_theta)
def copy_joint(self, joint_src):
"""Copies a joint to be added to the destination plant."""
assert isinstance(joint_src, Joint)
plant_dest = self.plant_dest
frame_on_parent_dest = self.frames[joint_src.frame_on_parent()]
frame_on_child_dest = self.frames[joint_src.frame_on_child()]
# N.B. We use `type(x) == cls`, not `isinstance(x, cls)`, so that we
# know we recreate the exact types.
if type(joint_src) == BallRpyJoint:
joint_dest = BallRpyJoint(
name=joint_src.name(),
frame_on_parent=frame_on_parent_dest,
frame_on_child=frame_on_child_dest,
# TODO(eric.cousineau): Bind this?
# damping=joint_src.damping(),
)
elif type(joint_src) == PrismaticJoint:
joint_dest = PrismaticJoint(
name=joint_src.name(),
frame_on_parent=frame_on_parent_dest,
frame_on_child=frame_on_child_dest,
axis=joint_src.translation_axis(),
damping=joint_src.damping(),
)
elif type(joint_src) == RevoluteJoint:
joint_dest = RevoluteJoint(
name=joint_src.name(),
frame_on_parent=frame_on_parent_dest,
frame_on_child=frame_on_child_dest,
axis=joint_src.revolute_axis(),
damping=joint_src.damping(),
)
elif type(joint_src) == UniversalJoint:
joint_dest = UniversalJoint(
name=joint_src.name(),
frame_on_parent=frame_on_parent_dest,
frame_on_child=frame_on_child_dest,
damping=joint_src.damping(),
)
elif type(joint_src) == WeldJoint:
joint_dest = WeldJoint(
name=joint_src.name(),
parent_frame_P=frame_on_parent_dest,
child_frame_C=frame_on_child_dest,
X_PC=joint_src.X_PC(),
)
else:
assert False, f"Cannot clone: {type(joint_src)}"
joint_dest.set_position_limits(joint_src.position_lower_limits(),
joint_src.position_upper_limits())
joint_dest.set_velocity_limits(joint_src.velocity_lower_limits(),
joint_src.velocity_upper_limits())
joint_dest.set_acceleration_limits(
joint_src.acceleration_lower_limits(),
joint_src.acceleration_upper_limits())
joint_dest.set_default_positions(joint_src.default_positions())
plant_dest.AddJoint(joint_dest)
_add_item(self.joints, joint_src, joint_dest)
def random_joint(parent, child):
# Returns a random joint, but with an incrementing name. Note that we
# use a separate index so that we ensure we can loop through all
# joints.
i = i_next("joint")
name = f"joint_{i}"
joint_cls = JOINT_CLS_LIST[i % len(JOINT_CLS_LIST)]
frame_on_parent = random_frame(parent.body_frame())
frame_on_child = random_frame(child.body_frame())
axis = np.zeros(3)
axis[i_next() % 3] = 1
damping = random.random()
if joint_cls == BallRpyJoint:
joint = BallRpyJoint(
name,
frame_on_parent=frame_on_parent,
frame_on_child=frame_on_child,
damping=damping,
)
elif joint_cls == PrismaticJoint:
joint = PrismaticJoint(
name,
frame_on_parent=frame_on_parent,
frame_on_child=frame_on_child,
axis=axis,
damping=damping,
)
elif joint_cls == RevoluteJoint:
joint = RevoluteJoint(
name,
frame_on_parent=frame_on_parent,
frame_on_child=frame_on_child,
axis=axis,
damping=damping,
)
elif joint_cls == UniversalJoint:
joint = UniversalJoint(
name,
frame_on_parent=frame_on_parent,
frame_on_child=frame_on_child,
damping=damping,
)
elif joint_cls == WeldJoint:
joint = WeldJoint(
name,
frame_on_parent_P=frame_on_parent,
frame_on_child_C=frame_on_child,
X_PC=random_X(),
)
else:
assert False
return plant.AddJoint(joint)
def test_solve(self):
plant = MultibodyPlant(0)
M_AAo_A = SpatialInertia(1, np.zeros(3), UnitInertia(1, 1, 1))
body = plant.AddRigidBody("body", M_AAo_A)
plant.AddJoint(
PrismaticJoint("joint", plant.world_frame(), body.body_frame(),
[1, 0, 0]))
plant.Finalize()
path = PiecewisePolynomial.CubicWithContinuousSecondDerivatives(
[0., 1.], np.array([[0., 1.]]), [0.], [0.])
gridpoints = Toppra.CalcGridPoints(path, CalcGridPointsOptions())
toppra = Toppra(path=path, plant=plant, gridpoints=gridpoints)
toppra.AddJointAccelerationLimit([-1.], [1.])
trajectory = toppra.SolvePathParameterization()
self.assertIsInstance(trajectory, PiecewisePolynomial)