def test_multibody_add_frame(self):
plant = MultibodyPlant()
frame = plant.AddFrame(frame=FixedOffsetFrame(
name="frame", P=plant.world_frame(),
X_PF=RigidTransform.Identity(), model_instance=None))
self.assertIsInstance(frame, Frame)
np.testing.assert_equal(
np.eye(4), frame.GetFixedPoseInBodyFrame().GetAsMatrix4())
def random_frame(parent_frame):
# Returns a random frame, with an incrementing name.
i = i_next()
return plant.AddFrame(
FixedOffsetFrame(
name=f"frame_{i}",
P=parent_frame,
X_PF=random_X(),
model_instance=parent_frame.model_instance(),
))
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 add_goal_region_visual_geometry(mbp, goal_position, goal_delta):
''' Adds a 5cm cube at the specified pose. Uses a planar floating base
in the x-z plane. '''
shape = Box(goal_delta, goal_delta, goal_delta)
no_mass_no_inertia = SpatialInertia(mass=0.,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia(0., 0., 0.))
shape = Sphere(0.05)
model_instance = mbp.AddModelInstance("goal_vis")
vis_origin_frame = mbp.AddFrame(
frame=FixedOffsetFrame(name="goal_vis_origin",
P=mbp.world_frame(),
X_PF=RigidTransform(
p=(goal_position +
np.array([0., 0.5, 0.])))))
body = mbp.AddRigidBody("goal_vis", model_instance, no_mass_no_inertia)
mbp.WeldFrames(vis_origin_frame, body.body_frame())
mbp.RegisterVisualGeometry(body, RigidTransform(), shape, "goal_vis",
[0.4, 0.9, 0.4, 0.35])
def copy_frame(self, frame_src):
"""Copies a Frame to be added to the destination plant.
Note: BodyFrame's are handled by `copy_body`, and are ignored by this
method.
"""
if frame_src in self._builtins_src.frames:
return
plant_dest = self.plant_dest
assert not isinstance(frame_src,
BodyFrame), (f"{frame_src}, {frame_src.name()}")
parent_frame_src = frame_src.body().body_frame()
parent_frame_dest = self.frames[parent_frame_src]
model_instance_src = frame_src.model_instance()
model_instance_dest = (self.model_instances[model_instance_src])
frame_dest = FixedOffsetFrame(
name=frame_src.name(),
P=parent_frame_dest,
X_PF=frame_src.GetFixedPoseInBodyFrame(),
model_instance=model_instance_dest,
)
plant_dest.AddFrame(frame_dest)
_add_item(self.frames, frame_src, frame_dest)