def test_contact(self):
# PenetrationAsContactPair
point_pair = PenetrationAsPointPair()
self.assertTrue(isinstance(point_pair.id_A, GeometryId))
self.assertTrue(isinstance(point_pair.id_B, GeometryId))
self.assertTrue(point_pair.p_WCa.shape == (3, ))
self.assertTrue(point_pair.p_WCb.shape == (3, ))
self.assertTrue(isinstance(point_pair.depth, float))
# PointPairContactInfo
id_A = BodyIndex(0)
id_B = BodyIndex(1)
contact_info = PointPairContactInfo(bodyA_index=id_A,
bodyB_index=id_B,
f_Bc_W=np.array([0, 0, 1]),
p_WC=np.array([0, 0, 0]),
separation_speed=0,
slip_speed=0,
point_pair=point_pair)
self.assertTrue(isinstance(contact_info.bodyA_index(), BodyIndex))
self.assertTrue(isinstance(contact_info.bodyB_index(), BodyIndex))
self.assertTrue(contact_info.contact_force().shape == (3, ))
self.assertTrue(contact_info.contact_point().shape == (3, ))
self.assertTrue(isinstance(contact_info.slip_speed(), float))
# ContactResults
contact_results = ContactResults()
contact_results.AddContactInfo(contact_info)
self.assertTrue(contact_results.num_contacts() == 1)
self.assertTrue(
isinstance(contact_results.contact_info(0), PointPairContactInfo))
def test_contact(self):
# PenetrationAsContactPair
point_pair = PenetrationAsPointPair()
self.assertTrue(isinstance(point_pair.id_A, GeometryId))
self.assertTrue(isinstance(point_pair.id_B, GeometryId))
self.assertTrue(point_pair.p_WCa.shape == (3, ))
self.assertTrue(point_pair.p_WCb.shape == (3, ))
self.assertTrue(isinstance(point_pair.depth, float))
# PointPairContactInfo
id_A = BodyIndex(0)
id_B = BodyIndex(1)
contact_info = PointPairContactInfo(bodyA_index=id_A,
bodyB_index=id_B,
f_Bc_W=np.array([0, 0, 1]),
p_WC=np.array([0, 0, 0]),
separation_speed=0,
slip_speed=0,
point_pair=point_pair)
self.assertTrue(isinstance(contact_info.bodyA_index(), BodyIndex))
self.assertTrue(isinstance(contact_info.bodyB_index(), BodyIndex))
self.assertTrue(contact_info.contact_force().shape == (3, ))
self.assertTrue(contact_info.contact_point().shape == (3, ))
self.assertTrue(isinstance(contact_info.slip_speed(), float))
self.assertIsInstance(contact_info.point_pair(),
PenetrationAsPointPair)
# ContactResults
contact_results = ContactResults()
contact_results.AddContactInfo(contact_info)
self.assertTrue(contact_results.num_contacts() == 1)
self.assertTrue(
isinstance(contact_results.contact_info(0), PointPairContactInfo))
# ContactResultsToLcmSystem
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
contact_results_to_lcm = ContactResultsToLcmSystem(plant)
context = contact_results_to_lcm.CreateDefaultContext()
context.FixInputPort(0, AbstractValue.Make(contact_results))
output = contact_results_to_lcm.AllocateOutput()
contact_results_to_lcm.CalcOutput(context, output)
result = output.get_data(0)
self.assertIsInstance(result, AbstractValue)
def test_contact(self):
# PenetrationAsContactPair
point_pair = PenetrationAsPointPair()
self.assertTrue(isinstance(point_pair.id_A, GeometryId))
self.assertTrue(isinstance(point_pair.id_B, GeometryId))
self.assertTrue(point_pair.p_WCa.shape == (3,))
self.assertTrue(point_pair.p_WCb.shape == (3,))
self.assertTrue(isinstance(point_pair.depth, float))
# PointPairContactInfo
id_A = BodyIndex(0)
id_B = BodyIndex(1)
contact_info = PointPairContactInfo(
bodyA_index=id_A, bodyB_index=id_B,
f_Bc_W=np.array([0, 0, 1]), p_WC=np.array([0, 0, 0]),
separation_speed=0, slip_speed=0, point_pair=point_pair)
self.assertTrue(
isinstance(contact_info.bodyA_index(), BodyIndex))
self.assertTrue(
isinstance(contact_info.bodyB_index(), BodyIndex))
self.assertTrue(contact_info.contact_force().shape == (3,))
self.assertTrue(contact_info.contact_point().shape == (3,))
self.assertTrue(isinstance(contact_info.slip_speed(), float))
self.assertIsInstance(
contact_info.point_pair(), PenetrationAsPointPair)
# ContactResults
contact_results = ContactResults()
contact_results.AddContactInfo(contact_info)
self.assertTrue(contact_results.num_contacts() == 1)
self.assertTrue(
isinstance(contact_results.contact_info(0), PointPairContactInfo))
# ContactResultsToLcmSystem
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
contact_results_to_lcm = ContactResultsToLcmSystem(plant)
context = contact_results_to_lcm.CreateDefaultContext()
context.FixInputPort(0, AbstractValue.Make(contact_results))
output = contact_results_to_lcm.AllocateOutput()
contact_results_to_lcm.CalcOutput(context, output)
result = output.get_data(0)
self.assertIsInstance(result, AbstractValue)
def __init__(self,
meshcat_viz,
force_threshold=1e-2,
contact_force_scale=10,
plant=None,
contact_force_radius=0.01):
"""
Args:
meshcat_viz: A pydrake MeshcatVisualizer instance.
force_threshold: contact forces whose norms are smaller than
force_threshold are not displayed.
contact_force_scale: a contact force with norm F (in Newtons) is
displayed as a cylinder with length F/contact_force_scale
(in meters).
plant: the MultibodyPlant associated with meshcat_viz.scene_graph.
contact_force_radius: sets the constant radius of the cylinder used
to visualize the forces.
"""
LeafSystem.__init__(self)
assert plant is not None
self._meshcat_viz = meshcat_viz
self._force_threshold = force_threshold
self._contact_force_scale = contact_force_scale
self._plant = plant
self._radius = contact_force_radius
self.set_name('meshcat_contact_visualizer')
self.DeclarePeriodicPublish(self._meshcat_viz.draw_period, 0.0)
# Pose bundle (from SceneGraph) input port.
self.DeclareAbstractInputPort("pose_bundle",
AbstractValue.Make(PoseBundle(0)))
# Contact results input port from MultibodyPlant
self.DeclareAbstractInputPort("contact_results",
AbstractValue.Make(ContactResults()))
# Make force cylinders smaller at initialization.
self._force_cylinder_radial_scale = 1.
self._force_cylinder_longitudinal_scale = 100.
# This system has undeclared states, see #4330.
# - All contacts (previous and current), of type `_ContactState`.
self._contacts = []
# - Unique key for contacts in meshcat.
self._contact_key_counter = 0
# - Previous time at which contact was published.
self._t_previous = 0.
def __init__(self,
meshcat_viz,
force_threshold=1e-2,
contact_force_scale=10,
plant=None,
contact_force_radius=0.01):
"""
Args:
meshcat_viz: A pydrake MeshcatVisualizer instance.
force_threshold: contact forces whose norms are smaller than
force_threshold are not displayed.
contact_force_scale: a contact force with norm F (in Newtons) is
displayed as a cylinder with length F/contact_force_scale
(in meters).
plant: the MultibodyPlant associated with meshcat_viz.scene_graph.
contact_force_radius: sets the constant radius of the cylinder used
to visualize the forces.
"""
LeafSystem.__init__(self)
assert plant is not None
self._meshcat_viz = meshcat_viz
self._force_threshold = force_threshold
self._contact_force_scale = contact_force_scale
self._plant = plant
self._radius = contact_force_radius
self.set_name('meshcat_contact_visualizer')
self.DeclarePeriodicPublish(self._meshcat_viz.draw_period, 0.0)
# Pose bundle (from SceneGraph) input port.
self.DeclareAbstractInputPort("pose_bundle",
AbstractValue.Make(PoseBundle(0)))
# Contact results input port from MultibodyPlant
self.DeclareAbstractInputPort("contact_results",
AbstractValue.Make(ContactResults()))
# This system has undeclared states, see #4330.
self._warned_pose_bundle_input_port_connected = False
self._published_contacts = []
# Zap any previous contact forces on this prefix
vis = self._meshcat_viz.vis[self._meshcat_viz.prefix]["contact_forces"]
vis.delete()