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()