def weld_gripper(mbp, robot_index, gripper_index):
X_EeGripper = create_transform([0, 0, 0.081], [np.pi / 2, 0, np.pi / 2])
robot_body = get_model_bodies(mbp, robot_index)[-1]
gripper_body = get_model_bodies(mbp, gripper_index)[0]
mbp.AddJoint(
WeldJoint(name="weld_gripper_to_robot_ee",
parent_frame_P=robot_body.body_frame(),
child_frame_C=gripper_body.body_frame(),
X_PC=X_EeGripper))
def movable_bodies(self):
movable = {self.mbp.tree().get_body(index) for index in self.doors}
for model in [self.robot, self.gripper] + list(self.movable):
movable.update(get_model_bodies(self.mbp, model))
#for body in get_model_bodies(self.mbp, model):
# #print(self.mbp.tree().get_body(body.index()) in {body}) # True
# #print(body.index() in {body.index()}) # False
# movable.add(body.index())
return movable
def gen(obj_name, surface):
obj = mbp.GetModelInstanceByName(obj_name)
surface_body = mbp.GetBodyByName(surface.body_name, surface.model_index)
surface_pose = get_body_pose(context, surface_body)
collision_pairs = set(product(get_model_bodies(mbp, obj), fixed)) # + [surface]
#object_aabb, object_local = AABBs[obj_name], Isometry3.Identity()
#surface_aabb, surface_local = AABBs[surface_name], Isometry3.Identity()
object_aabb, object_local, _ = box_from_geom[int(obj), get_base_body(mbp, obj).name(), 0]
surface_aabb, surface_local, _ = box_from_geom[int(surface.model_index), surface.body_name, surface.visual_index]
for surface_from_object in sample_aabb_placement(object_aabb, surface_aabb):
world_pose = surface_pose.multiply(surface_local).multiply(
surface_from_object).multiply(object_local.inverse())
pose = Pose(mbp, world, obj, world_pose)
pose.assign(context)
if not exists_colliding_pair(task.diagram, task.diagram_context, task.mbp, task.scene_graph, collision_pairs):
yield pose,
def gen(obj_name, surface):
obj = mbp.GetModelInstanceByName(obj_name)
obj_aabb, obj_from_box, _ = box_from_geom[
int(obj), get_base_body(mbp, obj).name(), 0]
collision_pairs = set(product(get_model_bodies(mbp, obj), fixed))
#object_radius = min(object_aabb[:2])
surface_body = mbp.GetBodyByName(surface.body_name,
surface.model_index)
surface_pose = get_body_pose(context, surface_body)
surface_aabb, surface_from_box, _ = box_from_geom[
int(surface.model_index), surface.body_name, surface.visual_index]
for surface_box_from_obj_box in sample_aabb_placement(obj_aabb,
surface_aabb,
shrink=shrink):
world_pose = surface_pose.multiply(surface_from_box).multiply(
surface_box_from_obj_box).multiply(obj_from_box.inverse())
pose = Pose(mbp, world, obj, world_pose)
pose.assign(context)
if not exists_colliding_pair(task.diagram, task.diagram_context,
task.mbp, task.scene_graph,
collision_pairs):
yield (pose, )
def bodies(self):
return get_model_bodies(self.mbp, self.child)
def bodies_from_models(mbp, models):
return {body for model in models for body in get_model_bodies(mbp, model)}