def add_arms_from_config(self, config):
X_iiwa_wsg = RigidTransform(RollPitchYaw(np.pi/2., 0, np.pi/2.), [0, 0, 0.114])
X_franka = RigidTransform(RollPitchYaw(0, 0, -np.pi/4.), [0, 0, 0])
for arm_name, arm_config in iteritems(config['env']['arms']):
X_arm = transform_from_dict(arm_config)
if arm_config["arm_type"] == "iiwa_7":
if arm_config["collision_model"] == "none":
iiwa7_path = iiwa_7_no_collision_path
elif arm_config["collision_model"] == "box":
iiwa7_path = iiwa_7_box_collision_path
self.add_arm_gripper(arm_name, iiwa7_path, "iiwa_link_0", X_arm,
schunk_path, "iiwa_link_7",
"body", X_iiwa_wsg)
elif arm_config["arm_type"] == "iiwa_14":
if arm_config["collision_model"] == "none":
iiwa14_path = iiwa14_no_collision_path
elif arm_config["collision_model"] == "sphere":
iiwa14_path = iiwa14_sphere_collision_path
elif arm_config["collision_model"] == "cylinder":
iiwa14_path = iiwa14_primitive_collision_path
elif arm_config["collision_model"] == "ee_mesh":
iiwa14_path = iiwa14_poly_collision_path
self.add_arm_gripper(arm_name, iiwa14_path, "base", X_arm,
schunk_path, "iiwa_link_7",
"body", X_iiwa_wsg)
elif arm_config["arm_type"] == "franka":
self.add_arm_gripper(arm_name, franka_arm_path,
"panda_link0", X_arm,
franka_hand_path, "panda_link8",
"panda_hand", X_franka)
elif arm_config["arm_type"] == "floating":
self.add_floating_gripper(arm_name, schunk_path, None, "body", X_arm)
def get_q_object_final(self, q_object_init, ee_init, ee_final):
"""
Input:
q_object_init: [quaternion, xyz]
ee_init: [x y z r p y]
ee_final: [x y z r p y]
xyz_iiwa: [x, y, z]
Return:
q_object_final: [quaternion, xyz]
"""
xyz_obj = np.array(q_object_init[4:]).transpose()
# xyz_obj[0] -= np.array(xyz_iiwa) # convert translation from world frame to robot base frame
quaternion_obj = Quaternion(np.array(q_object_init[:4]))
rot_obj = RotationMatrix(quaternion_obj)
X_WO = RigidTransform(rot_obj, xyz_obj)
xyz_ee_init = np.array(ee_init[:3]).transpose()
rot_ee_init = RollPitchYaw(ee_init[3], ee_init[4], ee_init[5]).ToRotationMatrix()
X_WE1 = RigidTransform(rot_ee_init, xyz_ee_init)
xyz_ee_final = np.array(ee_final[:3]).transpose()
rot_ee_final = RollPitchYaw(ee_final[3], ee_final[4], ee_final[5]).ToRotationMatrix()
X_WE2 = RigidTransform(rot_ee_final, xyz_ee_final)
X_EO = X_WE1.inverse().multiply(X_WO)
X_WO2 = X_WE2.multiply(X_EO)
wxyz_obj_final = X_WO2.rotation().ToQuaternion().wxyz()
xyz_obj_final = X_WO2.translation()
# xyz_ee_obj_init = xyz_obj - xyz_ee_init
# rot_ee_init_final = rot_ee_init.multiply(rot_ee_final.transpose())
# xyz_ee_obj_final = np.matmul(rot_ee_init_final.matrix(), xyz_ee_obj_init)
# # xyz_ee_obj_final += np.array(xyz_iiwa) # convert translation from robot base frame to world frame
# xyz_obj_final = xyz_ee_final + xyz_ee_obj_final
# rot_obj_final = rot_ee_init_final.multiply(rot_obj)
# quaternion_obj_final = rot_obj_final.ToQuaternion()
# wxyz_obj_final = quaternion_obj_final.wxyz()
q_object_final = []
for i in range(4):
q_object_final.append(wxyz_obj_final[i])
for i in range(3):
q_object_final.append(xyz_obj_final[i])
return q_object_final
def _slider_callback(change, index, body=None):
if body: # Then it's a floating base
pose = plant.GetFreeBodyPose(plant_context, body)
vector_pose = np.concatenate(
(RollPitchYaw(pose.rotation()).vector(), pose.translation()))
vector_pose[index] = change.new
plant.SetFreeBodyPose(
plant_context, body,
RigidTransform(RollPitchYaw(vector_pose[:3]), vector_pose[3:]))
else:
positions[index] = change.new
plant.SetPositions(plant_context, positions)
publishing_system.Publish(publishing_context)
if my_callback:
my_callback(plant_context)
def InterpolatePitchAngle(i, num_knot_points):
assert i <= num_knot_points
pitch_start = np.pi / 180 * 135
pitch_end = np.pi / 180 * 90
pitch_angle = pitch_start + (pitch_end -
pitch_start) / num_knot_points * i
return RollPitchYaw(0, pitch_angle, 0).ToRotationMatrix()
def DoCalcOutput(self, context, output):
"""
Constructs the output values from the widget elements.
"""
output.set_value(RigidTransform(
RollPitchYaw(self._roll.value, self._pitch.value, self._yaw.value),
[self._x.value, self._y.value, self._z.value]))
def CalcKinematics(self, X_L7E, l7_frame, world_frame, tree_iiwa,
context_iiwa, handle_angle_ref):
# calculate Geometric jacobian (6 by 7 matrix) of point Q in EE frame.
p_L7Q = X_L7E.multiply(p_EQ)
Jv_WL7q = tree_iiwa.CalcFrameGeometricJacobianExpressedInWorld(
context=context_iiwa, frame_B=l7_frame, p_BoFo_B=p_L7Q)
# Translation
# Hr: handle reference frame
X_WHr = Isometry3()
X_WHr.set_rotation(
RollPitchYaw(0, 0, -handle_angle_ref).ToRotationMatrix().matrix())
X_WHr.set_translation(p_WC_left_hinge + [
-r_handle * np.sin(handle_angle_ref), -r_handle *
np.cos(handle_angle_ref), 0
])
X_HrW = X_WHr.inverse()
X_WL7 = tree_iiwa.CalcRelativeTransform(context_iiwa,
frame_A=world_frame,
frame_B=l7_frame)
p_WQ = X_WL7.multiply(p_L7Q)
p_HrQ = X_HrW.multiply(p_WQ)
return Jv_WL7q, p_HrQ
def xyz_rpy_deg(xyz, rpy_deg):
"""Shorthand to create a rigid transform from XYZ and RPY (degrees)."""
rpy = np.deg2rad(rpy_deg)
return RigidTransform(
R=RotationMatrix(rpy=RollPitchYaw(rpy), ),
p=xyz,
)
def transform_from_dict(data):
pos, _ = get_data_from_dict(data, ['pos', 'position', 'translation'])
orientation, key = get_data_from_dict(data, ['quat', 'quaternion', 'rpy'])
if key == "rpy":
return RigidTransform(RollPitchYaw(orientation), pos)
else:
return RigidTransform(Quaternion(orientation), pos)
def _DoCalcOutput(self, context, output):
output.get_mutable_value().set_rotation(
RollPitchYaw(self.roll.get(), self.pitch.get(),
self.yaw.get()).ToRotationMatrix().matrix())
output.get_mutable_value().set_translation(
[self.x.get(), self.y.get(),
self.z.get()])
def SetPose(self, pose):
"""
@param pose is an Isometry3.
"""
tf = RigidTransform(pose)
self.SetRPY(RollPitchYaw(tf.rotation()))
self.SetXYZ(pose.translation())
def test_map_qdot_to_v_and_back(self):
plant = MultibodyPlant()
iiwa_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/"
"iiwa_description/sdf/iiwa14_no_collision.sdf")
# Use floating base to effectively add a quatnerion in the generalized
# quaternion.
iiwa_model = Parser(plant=plant).AddModelFromFile(
file_name=iiwa_sdf_path, model_name='robot')
plant.Finalize()
context = plant.CreateDefaultContext()
# Try mapping velocity to qdot and back.
nq = plant.num_positions()
nv = plant.num_velocities()
q_init = np.linspace(start=1.0, stop=nq, num=nq)
plant.SetPositions(context, q_init)
# Overwrite the (invalid) base coordinates, wherever in `q` they are.
plant.SetFreeBodyPose(
context, plant.GetBodyByName("iiwa_link_0"),
RigidTransform(RollPitchYaw([0.1, 0.2, 0.3]),
p=[0.4, 0.5, 0.6]).GetAsIsometry3())
v_expected = np.linspace(start=-1.0, stop=-nv, num=nv)
qdot = plant.MapVelocityToQDot(context, v_expected)
v_remap = plant.MapQDotToVelocity(context, qdot)
self.assertTrue(np.allclose(v_expected, v_remap))
def DepthCameraDemoSystem():
builder = DiagramBuilder()
# Create the physics engine + scene graph.
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
# Add a single object into it.
Parser(plant, scene_graph).AddModelFromFile(
FindResourceOrThrow(
"drake/manipulation/models/ycb/sdf/006_mustard_bottle.sdf"))
# Add a rendering engine
renderer = "my_renderer"
scene_graph.AddRenderer(renderer,
MakeRenderEngineVtk(RenderEngineVtkParams()))
plant.Finalize()
# Add a visualizer just to help us see the object.
use_meshcat = False
if use_meshcat:
meshcat = builder.AddSystem(MeshcatVisualizer(scene_graph))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
meshcat.get_input_port(0))
# Add a camera to the environment.
pose = RigidTransform(RollPitchYaw(-0.2, 0.2, 0), [-.1, -0.1, -.5])
properties = DepthCameraProperties(width=640,
height=480,
fov_y=np.pi / 4.0,
renderer_name=renderer,
z_near=0.1,
z_far=10.0)
camera = builder.AddSystem(
RgbdSensor(parent_id=scene_graph.world_frame_id(),
X_PB=pose,
properties=properties,
show_window=False))
camera.set_name("rgbd_sensor")
builder.Connect(scene_graph.get_query_output_port(),
camera.query_object_input_port())
# Export the camera outputs
builder.ExportOutput(camera.color_image_output_port(), "color_image")
builder.ExportOutput(camera.depth_image_32F_output_port(), "depth_image")
# Add a system to convert the camera output into a point cloud
to_point_cloud = builder.AddSystem(
DepthImageToPointCloud(camera_info=camera.depth_camera_info(),
fields=BaseField.kXYZs | BaseField.kRGBs))
builder.Connect(camera.depth_image_32F_output_port(),
to_point_cloud.depth_image_input_port())
builder.Connect(camera.color_image_output_port(),
to_point_cloud.color_image_input_port())
# Export the point cloud output.
builder.ExportOutput(to_point_cloud.point_cloud_output_port(),
"point_cloud")
diagram = builder.Build()
diagram.set_name("depth_camera_demo_system")
return diagram
def SetPose(self, pose):
"""
@param pose is a RigidTransform or else any type accepted by
RigidTransform's constructor
"""
tf = RigidTransform(pose)
self.SetRPY(RollPitchYaw(tf.rotation()))
self.SetXYZ(pose.translation())
def add_thrusters(builder, mbp):
robot_body_index = mbp.GetBodyByName("robot").index()
robot_tail_forward = RigidTransform(p=[-0.5, 0, 0],
rpy=RollPitchYaw([math.pi / 2, 0, 0]))
robot_tail_clockwise = RigidTransform(p=[-0.5, 0, 0],
rpy=RollPitchYaw(
[math.pi / 2, 0, math.pi / 2]))
thrusters = builder.AddSystem(
Propeller([
PropellerInfo(robot_body_index, X_BP=robot_tail_forward),
PropellerInfo(robot_body_index, X_BP=robot_tail_clockwise)
]))
builder.Connect(thrusters.get_spatial_forces_output_port(),
mbp.get_applied_spatial_force_input_port())
builder.Connect(mbp.get_body_poses_output_port(),
thrusters.get_body_poses_input_port())
return thrusters
def get_q_object_init(self, object_name, t, xyz_iiwa):
""" Returns vector of length 7 [quaternion, xyz]
"""
q = self.get_object_state(object_name, t)
quaternion = RollPitchYaw(q[3], q[4], q[5]).ToQuaternion()
wxyz = quaternion.wxyz()
xyz_obj = np.array(q[:3]) - np.array(xyz_iiwa)
q_object_init = []
for i in range(4):
q_object_init.append(wxyz[i])
for i in range(3):
q_object_init.append(xyz_obj[i])
return q_object_init
def SetPose(self, pose):
"""
Sets the current value of the sliders.
Args:
pose: Any viable argument for the RigidTransform
constructor.
"""
tf = RigidTransform(pose)
self.SetRpy(RollPitchYaw(tf.rotation()))
self.SetXyz(tf.translation())
def reset_simulator_from_config(config, simulator, diagram, systems):
for rope_name, _ in iteritems(config['env']['ropes']):
initialize_rope_zero(diagram, simulator, systems["station"], rope_name)
sim_context = simulator.get_mutable_context()
station_context = diagram.GetMutableSubsystemContext(
systems["station"], sim_context)
if 'arms' in config['env']:
values = {}
for arm_name, arm_config in iteritems(config['env']['arms']):
rpy = RollPitchYaw(config["env"]["arms"][arm_name]["rpy"])
xyz = config["env"]["arms"][arm_name]["pos"]
init_state = np.append(rpy.vector(), xyz)
grip = config["env"]["arms"][arm_name]["grip"]
quat = rpy.ToQuaternion()
systems["station"].set_model_state(
station_context, arm_name,
np.array([
quat.w(),
quat.x(),
quat.y(),
quat.z(), xyz[0], xyz[1], xyz[2], -grip / 2, grip / 2
]), np.zeros(8))
values[arm_name] = init_state
values[f"{arm_name}_width"] = grip
set_targets(simulator, diagram, systems, values)
sp_context = diagram.GetMutableSubsystemContext(systems["sp_control"],
sim_context)
if 'arms' in config['env']:
for arm_name, arm_config in iteritems(config['env']['arms']):
systems["sp_control"].SetPositions(
sp_context, arm_name,
np.append(config["env"]["arms"][arm_name]["rpy"],
config["env"]["arms"][arm_name]["pos"]),
[config["env"]["arms"][arm_name]["grip"]])
pid_context = diagram.GetMutableSubsystemContext(systems["pid"],
sim_context)
systems["pid"].reset(pid_context)
simulator.set_target_realtime_rate(config['env']['target_realtime_rate'])
sim_context.SetTime(0.)
simulator.Initialize()
def transform_to_vector(X, rpy=True):
"""
If rpy True, returns RollPitchYaw vector followed by translation vector.
Otherwise, returns Quaternion vector followed by translation vector.
"""
if rpy:
vector = np.empty(6)
vector[:3] = RollPitchYaw(X.rotation()).vector()
else:
vector = np.empty(7)
vector[:4] = X.rotation().ToQuaternion().wxyz()
vector[-3:] = X.translation()
return vector
def DoCalcAbstractOutput(self, context, y_data):
"""
Sets external forces
"""
external_forces = []
body_positions = self.body_positions_input_port.Eval(context)
for arm in self.arm_info:
desired = self.desired_input_ports[arm].Eval(context)
pose = body_positions[int(self.arm_info[arm])]
rotation = RollPitchYaw(pose.rotation())
estimated = [
rotation.roll_angle(),
rotation.pitch_angle(),
rotation.yaw_angle()
]
estimated.extend(pose.translation())
error = np.subtract(desired, estimated)
error[0:3] = self.clamp_angles(error[0:3])
prev_error = context.get_mutable_discrete_state(
self.previous_error[arm])
integral = context.get_mutable_discrete_state(self.integral[arm])
integral.SetFromVector(integral.get_value() +
error * self.timestep)
derivative = np.subtract(error,
prev_error.get_value()) / self.timestep
forces = np.multiply(self.kp, error)
forces += np.multiply(self.kd, derivative)
forces += np.multiply(self.ki, integral.get_value())
clipped_forces = np.clip(
forces, [-100, -100, -100, -10000, -10000, -10000],
[100, 100, 100, 10000, 10000, 10000])
prev_error.SetFromVector(error)
# print(f"arm {arm} forces {clipped_forces}")
external_forces.append(
self.get_external_force(self.arm_info[arm], clipped_forces))
y_data.set_value(external_forces)
def _DoCalcDiscreteVariableUpdates(self, context, events, discrete_state):
rpy_xyz_desired = self.EvalVectorInput(context, 0).get_value()
X_WE_desired = RigidTransform(RollPitchYaw(rpy_xyz_desired[:3]),
rpy_xyz_desired[-3:]).GetAsIsometry3()
q_last = context.get_discrete_state_vector().get_value()
x = self.robot.GetMutablePositionsAndVelocities(self.robot_context)
x[:robot.num_positions()] = q_last
result = DoDifferentialInverseKinematics(self.robot,
self.robot_context,
X_WE_desired, self.frame_E,
self.parameters)
if (result.status != result.status.kSolutionFound):
print("Differential IK could not find a solution.")
discrete_state.get_mutable_vector().SetFromVector(q_last)
else:
discrete_state.get_mutable_vector().\
SetFromVector(q_last + self.time_step*result.joint_velocities)
def make_ball_paddle(contact_model, contact_surface_representation, time_step):
multibody_plant_config = \
MultibodyPlantConfig(
time_step=time_step,
contact_model=contact_model,
contact_surface_representation=contact_surface_representation)
# We pose the paddle, so that its top surface is on World's X-Y plane.
# Intuitively we push it down 1 cm because the box is 2 cm thick.
p_WPaddle_fixed = RigidTransform(RollPitchYaw(0, 0, 0),
np.array([0, 0, -0.01]))
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlant(multibody_plant_config, builder)
parser = Parser(plant)
paddle_sdf_file_name = \
FindResourceOrThrow("drake/examples/hydroelastic/python_ball_paddle"
"/paddle.sdf")
paddle = parser.AddModelFromFile(paddle_sdf_file_name, model_name="paddle")
plant.WeldFrames(frame_on_parent_F=plant.world_frame(),
frame_on_child_M=plant.GetFrameByName("paddle", paddle),
X_FM=p_WPaddle_fixed)
ball_sdf_file_name = \
FindResourceOrThrow("drake/examples/hydroelastic/python_ball_paddle"
"/ball.sdf")
parser.AddModelFromFile(ball_sdf_file_name)
# TODO(DamrongGuoy): Let users override hydroelastic modulus, dissipation,
# and resolution hint from the two SDF files above.
plant.Finalize()
DrakeVisualizer.AddToBuilder(builder=builder, scene_graph=scene_graph)
ConnectContactResultsToDrakeVisualizer(builder=builder,
plant=plant,
scene_graph=scene_graph)
nx = plant.num_positions() + plant.num_velocities()
state_logger = builder.AddSystem(VectorLogSink(nx))
builder.Connect(plant.get_state_output_port(),
state_logger.get_input_port())
diagram = builder.Build()
return diagram, plant, state_logger
def AddWsg(plant, iiwa_model_instance, roll=np.pi / 2.0, welded=False):
parser = pydrake.multibody.parsing.Parser(plant)
if welded:
parser.package_map().Add(
"wsg_50_description",
os.path.dirname(
pydrake.common.FindResourceOrThrow(
"drake/manipulation/models/wsg_50_description/package.xml")
))
gripper = parser.AddModelFromFile(
FindResource("models/schunk_wsg_50_welded_fingers.sdf"), "gripper")
else:
gripper = parser.AddModelFromFile(
pydrake.common.FindResourceOrThrow(
"drake/manipulation/models/"
"wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf"))
X_7G = RigidTransform(RollPitchYaw(np.pi / 2.0, 0, roll), [0, 0, 0.114])
plant.WeldFrames(plant.GetFrameByName("iiwa_link_7", iiwa_model_instance),
plant.GetFrameByName("body", gripper), X_7G)
return gripper
def setUp(self):
self.points = np.array([[1, 1, 0], [2, 1, 0]]).T
self.translation = RigidTransform(p=[1, 2, 3])
self.rotation = RigidTransform(
RotationMatrix(RollPitchYaw(0, 0, np.pi / 2)))
def _xyz_rpy_deg(xyz, rpy_deg):
return RigidTransform(RollPitchYaw(np.asarray(rpy_deg) * np.pi / 180), xyz)
def _xyz_rpy(p, rpy):
return RigidTransform(rpy=RollPitchYaw(rpy), p=p)
def test_composition_gripper_workflow(self):
"""Tests subgraphs (pre-finalize) for composition, with a scene graph,
welding bodies together across different subgraphs."""
# N.B. The frame-welding is done so that we can easily set the
# positions of the IIWA / WSG without having to worry about / work
# around the floating body coordinates.
# Create IIWA.
iiwa_builder = DiagramBuilder()
iiwa_plant, iiwa_scene_graph = AddMultibodyPlantSceneGraph(
iiwa_builder, time_step=0.)
iiwa_file = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/urdf/"
"iiwa14_spheres_dense_elbow_collision.urdf")
iiwa_model = Parser(iiwa_plant).AddModelFromFile(iiwa_file, "iiwa")
iiwa_plant.WeldFrames(iiwa_plant.world_frame(),
iiwa_plant.GetFrameByName("base"))
iiwa_plant.Finalize()
if VISUALIZE:
print("test_composition")
DrakeVisualizer.AddToBuilder(iiwa_builder, iiwa_scene_graph)
iiwa_diagram = iiwa_builder.Build()
iiwa_subgraph = mut.MultibodyPlantSubgraph(
mut.get_elements_from_plant(iiwa_plant, iiwa_scene_graph))
self.assertIsInstance(iiwa_subgraph, mut.MultibodyPlantSubgraph)
iiwa_subgraph.remove_body(iiwa_plant.world_body())
# Create WSG.
wsg_builder = DiagramBuilder()
wsg_plant, wsg_scene_graph = AddMultibodyPlantSceneGraph(wsg_builder,
time_step=0.)
wsg_file = FindResourceOrThrow(
"drake/manipulation/models/wsg_50_description/sdf/"
"schunk_wsg_50.sdf")
wsg_model = Parser(wsg_plant).AddModelFromFile(wsg_file,
"gripper_model")
wsg_plant.WeldFrames(wsg_plant.world_frame(),
wsg_plant.GetFrameByName("__model__"))
wsg_plant.Finalize()
if VISUALIZE:
DrakeVisualizer.AddToBuilder(wsg_builder, wsg_scene_graph)
wsg_diagram = wsg_builder.Build()
wsg_subgraph = mut.MultibodyPlantSubgraph(
mut.get_elements_from_plant(wsg_plant, wsg_scene_graph))
self.assertIsInstance(wsg_subgraph, mut.MultibodyPlantSubgraph)
wsg_subgraph.remove_body(wsg_plant.world_body())
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder,
time_step=1e-3)
iiwa_to_plant = iiwa_subgraph.add_to(plant, scene_graph)
self.assertIsInstance(iiwa_to_plant, mut.MultibodyPlantElementsMap)
wsg_to_plant = wsg_subgraph.add_to(plant, scene_graph)
self.assertIsInstance(wsg_to_plant, mut.MultibodyPlantElementsMap)
if VISUALIZE:
DrakeVisualizer.AddToBuilder(builder, scene_graph)
rpy_deg = np.array([90., 0., 90])
X_7G = RigidTransform(p=[0, 0, 0.114],
rpy=RollPitchYaw(rpy_deg * np.pi / 180))
frame_7 = plant.GetFrameByName("iiwa_link_7")
frame_G = plant.GetFrameByName("body")
plant.WeldFrames(frame_7, frame_G, X_7G)
plant.Finalize()
q_iiwa = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7]
q_wsg = [-0.03, 0.03]
iiwa_d_context = iiwa_diagram.CreateDefaultContext()
iiwa_context = iiwa_plant.GetMyContextFromRoot(iiwa_d_context)
iiwa_plant.SetPositions(iiwa_context, iiwa_model, q_iiwa)
wsg_d_context = wsg_diagram.CreateDefaultContext()
wsg_context = wsg_plant.GetMyContextFromRoot(wsg_d_context)
wsg_plant.SetPositions(wsg_context, wsg_model, q_wsg)
# Transfer state and briefly compare.
diagram = builder.Build()
d_context = diagram.CreateDefaultContext()
context = plant.GetMyContextFromRoot(d_context)
iiwa_to_plant.copy_state(iiwa_context, context)
wsg_to_plant.copy_state(wsg_context, context)
# Compare frames from sub-plants.
util.compare_frame_poses(plant, context, iiwa_plant, iiwa_context,
"base", "iiwa_link_7")
util.compare_frame_poses(plant, context, wsg_plant, wsg_context,
"body", "left_finger")
# Visualize.
if VISUALIZE:
print(" Visualize IIWA")
Simulator(iiwa_diagram, iiwa_d_context.Clone()).Initialize()
input(" Press enter...")
print(" Visualize WSG")
Simulator(wsg_diagram, wsg_d_context.Clone()).Initialize()
input(" Press enter...")
print(" Visualize Composite")
Simulator(diagram, d_context.Clone()).Initialize()
input(" Press enter...")
def test_model_instance_state_access_by_array(self):
# Create a MultibodyPlant with a kuka arm and a schunk gripper.
# the arm is welded to the world, the gripper is welded to the
# arm's end effector.
wsg50_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"wsg_50_description/sdf/schunk_wsg_50.sdf")
iiwa_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"iiwa_description/sdf/iiwa14_no_collision.sdf")
timestep = 0.0002
plant = MultibodyPlant(timestep)
parser = Parser(plant)
iiwa_model = parser.AddModelFromFile(file_name=iiwa_sdf_path,
model_name='robot')
gripper_model = parser.AddModelFromFile(file_name=wsg50_sdf_path,
model_name='gripper')
# Weld the base of arm and gripper to reduce the number of states.
X_EeGripper = RigidTransform(RollPitchYaw(np.pi / 2, 0, np.pi / 2),
[0, 0, 0.081])
plant.WeldFrames(A=plant.world_frame(),
B=plant.GetFrameByName("iiwa_link_0", iiwa_model))
plant.WeldFrames(A=plant.GetFrameByName("iiwa_link_7", iiwa_model),
B=plant.GetFrameByName("body", gripper_model),
X_AB=X_EeGripper)
plant.Finalize()
# Create a context of the MBP and set the state of the context
# to desired values.
context = plant.CreateDefaultContext()
nq = plant.num_positions()
nq_iiwa = plant.num_positions(iiwa_model)
nv = plant.num_velocities()
nv_iiwa = plant.num_velocities(iiwa_model)
q_iiwa_desired = np.linspace(0, 0.3, 7)
v_iiwa_desired = q_iiwa_desired + 0.4
q_gripper_desired = [0.4, 0.5]
v_gripper_desired = [-1., -2.]
x_desired = np.zeros(nq + nv)
x_desired[0:7] = q_iiwa_desired
x_desired[7:9] = q_gripper_desired
x_desired[nq:nq + 7] = v_iiwa_desired
x_desired[nq + 7:nq + nv] = v_gripper_desired
x = plant.GetMutablePositionsAndVelocities(context=context)
x[:] = x_desired
q = plant.GetPositions(context=context)
v = plant.GetVelocities(context=context)
# Get state from context.
x = plant.GetPositionsAndVelocities(context=context)
x_tmp = plant.GetMutablePositionsAndVelocities(context=context)
self.assertTrue(np.allclose(x_desired, x_tmp))
# Get positions and velocities of specific model instances
# from the position/velocity vector of the plant.
q_iiwa = plant.GetPositions(context=context, model_instance=iiwa_model)
q_iiwa_array = plant.GetPositionsFromArray(model_instance=iiwa_model,
q=q)
self.assertTrue(np.allclose(q_iiwa, q_iiwa_array))
q_gripper = plant.GetPositions(context=context,
model_instance=gripper_model)
v_iiwa = plant.GetVelocities(context=context,
model_instance=iiwa_model)
v_iiwa_array = plant.GetVelocitiesFromArray(model_instance=iiwa_model,
v=v)
self.assertTrue(np.allclose(v_iiwa, v_iiwa_array))
v_gripper = plant.GetVelocities(context=context,
model_instance=gripper_model)
# Assert that the `GetPositions` and `GetVelocities` return
# the desired values set earlier.
self.assertTrue(np.allclose(q_iiwa_desired, q_iiwa))
self.assertTrue(np.allclose(v_iiwa_desired, v_iiwa))
self.assertTrue(np.allclose(q_gripper_desired, q_gripper))
self.assertTrue(np.allclose(v_gripper_desired, v_gripper))
# Verify that SetPositionsInArray() and SetVelocitiesInArray() works.
plant.SetPositionsInArray(model_instance=iiwa_model,
q_instance=np.zeros(nq_iiwa),
q=q)
self.assertTrue(
np.allclose(
plant.GetPositionsFromArray(model_instance=iiwa_model, q=q),
np.zeros(nq_iiwa)))
plant.SetVelocitiesInArray(model_instance=iiwa_model,
v_instance=np.zeros(nv_iiwa),
v=v)
self.assertTrue(
np.allclose(
plant.GetVelocitiesFromArray(model_instance=iiwa_model, v=v),
np.zeros(nv_iiwa)))
# Check actuation.
nu = plant.num_actuated_dofs()
u = np.zeros(nu)
u_iiwa = np.arange(nv_iiwa)
plant.SetActuationInArray(model_instance=iiwa_model,
u_instance=u_iiwa,
u=u)
self.assertTrue(np.allclose(u[:7], u_iiwa))
def test_model_instance_state_access(self):
# Create a MultibodyPlant with a kuka arm and a schunk gripper.
# the arm is welded to the world, the gripper is welded to the
# arm's end effector.
wsg50_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"wsg_50_description/sdf/schunk_wsg_50.sdf")
iiwa_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"iiwa_description/sdf/iiwa14_no_collision.sdf")
plant = MultibodyPlant()
parser = Parser(plant)
iiwa_model = parser.AddModelFromFile(file_name=iiwa_sdf_path,
model_name='robot')
gripper_model = parser.AddModelFromFile(file_name=wsg50_sdf_path,
model_name='gripper')
# Weld the base of arm and gripper to reduce the number of states.
X_EeGripper = RigidTransform(RollPitchYaw(np.pi / 2, 0, np.pi / 2),
[0, 0, 0.081])
plant.WeldFrames(A=plant.world_frame(),
B=plant.GetFrameByName("iiwa_link_0", iiwa_model))
plant.WeldFrames(A=plant.GetFrameByName("iiwa_link_7", iiwa_model),
B=plant.GetFrameByName("body", gripper_model),
X_AB=X_EeGripper)
plant.Finalize()
# Create a context of the MBP and set the state of the context
# to desired values.
context = plant.CreateDefaultContext()
nq = plant.num_positions()
nv = plant.num_velocities()
nq_iiwa = 7
nv_iiwa = 7
nq_gripper = 2
nv_gripper = 2
q_iiwa_desired = np.zeros(nq_iiwa)
v_iiwa_desired = np.zeros(nv_iiwa)
q_gripper_desired = np.zeros(nq_gripper)
v_gripper_desired = np.zeros(nv_gripper)
q_iiwa_desired[2] = np.pi / 3
q_gripper_desired[0] = 0.1
v_iiwa_desired[1] = 5.0
q_gripper_desired[0] = -0.3
x_iiwa_desired = np.zeros(nq_iiwa + nv_iiwa)
x_iiwa_desired[0:nq_iiwa] = q_iiwa_desired
x_iiwa_desired[nq_iiwa:nq_iiwa + nv_iiwa] = v_iiwa_desired
x_gripper_desired = np.zeros(nq_gripper + nv_gripper)
x_gripper_desired[0:nq_gripper] = q_gripper_desired
x_gripper_desired[nq_gripper:nq_gripper +
nv_gripper] = v_gripper_desired
x_desired = np.zeros(nq + nv)
x_desired[0:7] = q_iiwa_desired
x_desired[7:9] = q_gripper_desired
x_desired[nq:nq + 7] = v_iiwa_desired
x_desired[nq + 7:nq + nv] = v_gripper_desired
# Check SetPositionsAndVelocities() for each model instance.
# Do the iiwa model first.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetPositionsAndVelocities(context, iiwa_model, x_iiwa_desired)
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context, iiwa_model),
x_iiwa_desired))
self.assertTrue(
np.allclose(
plant.GetPositionsAndVelocities(context, gripper_model),
np.zeros(nq_gripper + nv_gripper)))
# Do the gripper model.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetPositionsAndVelocities(context, gripper_model,
x_gripper_desired)
self.assertTrue(
np.allclose(
plant.GetPositionsAndVelocities(context, gripper_model),
x_gripper_desired))
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context, iiwa_model),
np.zeros(nq_iiwa + nv_iiwa)))
# Check SetPositions() for each model instance.
# Do the iiwa model first.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetPositions(context, iiwa_model, q_iiwa_desired)
self.assertTrue(
np.allclose(plant.GetPositions(context, iiwa_model),
q_iiwa_desired))
self.assertTrue(
np.allclose(plant.GetVelocities(context, iiwa_model),
np.zeros(nv_iiwa)))
self.assertTrue(
np.allclose(
plant.GetPositionsAndVelocities(context, gripper_model),
np.zeros(nq_gripper + nv_gripper)))
# Do the gripper model.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetPositions(context, gripper_model, q_gripper_desired)
self.assertTrue(
np.allclose(plant.GetPositions(context, gripper_model),
q_gripper_desired))
self.assertTrue(
np.allclose(plant.GetVelocities(context, gripper_model),
np.zeros(nq_gripper)))
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context, iiwa_model),
np.zeros(nq_iiwa + nv_iiwa)))
# Check SetVelocities() for each model instance.
# Do the iiwa model first.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetVelocities(context, iiwa_model, v_iiwa_desired)
self.assertTrue(
np.allclose(plant.GetVelocities(context, iiwa_model),
v_iiwa_desired))
self.assertTrue(
np.allclose(plant.GetPositions(context, iiwa_model),
np.zeros(nq_iiwa)))
self.assertTrue(
np.allclose(
plant.GetPositionsAndVelocities(context, gripper_model),
np.zeros(nq_gripper + nv_gripper)))
# Do the gripper model.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetVelocities(context, gripper_model, v_gripper_desired)
self.assertTrue(
np.allclose(plant.GetVelocities(context, gripper_model),
v_gripper_desired))
self.assertTrue(
np.allclose(plant.GetPositions(context, gripper_model),
np.zeros(nv_gripper)))
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context, iiwa_model),
np.zeros(nq_iiwa + nv_iiwa)))
def xyz_rpy_deg(xyz, rpy_deg):
"""Shorthand for defining a pose."""
rpy_deg = np.asarray(rpy_deg)
return RigidTransform(RollPitchYaw(rpy_deg * np.pi / 180), xyz)
def _get_transform(self):
return RigidTransform(
RollPitchYaw(self._value[0], self._value[1], self._value[2]),
self._value[3:])
