def test_distance_constraint(self, variables):
def get_sphere_geometry_id(frame):
id_, = variables.plant.GetCollisionGeometriesForBody(frame.body())
return id_
constraint = ik.DistanceConstraint(
plant=variables.plant,
geometry_pair=(get_sphere_geometry_id(variables.body1_frame),
get_sphere_geometry_id(variables.body2_frame)),
plant_context=variables.plant_context,
distance_lower=0.1,
distance_upper=2)
self.assertIsInstance(constraint, mp.Constraint)
def test_distance_constraint(self, variables):
inspector = self.scene_graph_f.model_inspector()
frame_id1 = inspector.GetGeometryIdByName(
self.plant_f.GetBodyFrameIdOrThrow(
self.plant_f.GetBodyByName("body1").index()),
pydrake.geometry.Role.kProximity, "two_bodies::body1_collision")
frame_id2 = inspector.GetGeometryIdByName(
self.plant_f.GetBodyFrameIdOrThrow(
self.plant_f.GetBodyByName("body2").index()),
pydrake.geometry.Role.kProximity, "two_bodies::body2_collision")
constraint = ik.DistanceConstraint(
plant=variables.plant, geometry_pair=(frame_id1, frame_id2),
distance_lower=0.1, distance_upper=0.2,
plant_context=variables.plant_context)
self.assertIsInstance(constraint, mp.Constraint)
