def test_model_instance_state_access(self, T):
# N.B. Please check warning above in `check_multibody_state_access`.
# 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.
RigidTransform = RigidTransform_[T]
RollPitchYaw = RollPitchYaw_[T]
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")
# N.B. `Parser` only supports `MultibodyPlant_[float]`.
plant_f = MultibodyPlant_[float]()
parser = Parser(plant_f)
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_[float](
RollPitchYaw_[float](np.pi / 2, 0, np.pi / 2), [0, 0, 0.081])
plant_f.WeldFrames(
A=plant_f.world_frame(),
B=plant_f.GetFrameByName("iiwa_link_0", iiwa_model))
plant_f.WeldFrames(
A=plant_f.GetFrameByName("iiwa_link_7", iiwa_model),
B=plant_f.GetFrameByName("body", gripper_model),
X_AB=X_EeGripper)
plant_f.Finalize()
plant = to_type(plant_f, T)
# 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))
numpy_compare.assert_float_equal(
plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))
plant.SetPositionsAndVelocities(context, iiwa_model, x_iiwa_desired)
numpy_compare.assert_float_equal(
plant.GetPositionsAndVelocities(context, iiwa_model),
x_iiwa_desired)
numpy_compare.assert_float_equal(plant.GetPositionsAndVelocities(
context, gripper_model), np.zeros(nq_gripper + nv_gripper))
# Do the gripper model.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
numpy_compare.assert_float_equal(
plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))
plant.SetPositionsAndVelocities(
context, gripper_model, x_gripper_desired)
numpy_compare.assert_float_equal(
plant.GetPositionsAndVelocities(context, gripper_model),
x_gripper_desired)
numpy_compare.assert_float_equal(
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))
numpy_compare.assert_float_equal(
plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))
plant.SetPositions(context, iiwa_model, q_iiwa_desired)
numpy_compare.assert_float_equal(
plant.GetPositions(context, iiwa_model), q_iiwa_desired)
numpy_compare.assert_float_equal(plant.GetVelocities(
context, iiwa_model), np.zeros(nv_iiwa))
numpy_compare.assert_float_equal(plant.GetPositionsAndVelocities(
context, gripper_model), np.zeros(nq_gripper + nv_gripper))
# Do the gripper model.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
numpy_compare.assert_float_equal(
plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))
plant.SetPositions(context, gripper_model, q_gripper_desired)
numpy_compare.assert_float_equal(
plant.GetPositions(context, gripper_model),
q_gripper_desired)
numpy_compare.assert_float_equal(plant.GetVelocities(
context, gripper_model), np.zeros(nq_gripper))
numpy_compare.assert_float_equal(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))
numpy_compare.assert_float_equal(
plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))
plant.SetVelocities(context, iiwa_model, v_iiwa_desired)
numpy_compare.assert_float_equal(
plant.GetVelocities(context, iiwa_model), v_iiwa_desired)
numpy_compare.assert_float_equal(plant.GetPositions(
context, iiwa_model), np.zeros(nq_iiwa))
numpy_compare.assert_float_equal(plant.GetPositionsAndVelocities(
context, gripper_model), np.zeros(nq_gripper + nv_gripper))
# Do the gripper model.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
numpy_compare.assert_float_equal(
plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))
plant.SetVelocities(context, gripper_model, v_gripper_desired)
numpy_compare.assert_float_equal(
plant.GetVelocities(context, gripper_model),
v_gripper_desired)
numpy_compare.assert_float_equal(plant.GetPositions(
context, gripper_model), np.zeros(nv_gripper))
numpy_compare.assert_float_equal(plant.GetPositionsAndVelocities(
context, iiwa_model), np.zeros(nq_iiwa + nv_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 = Isometry3.Identity()
X_EeGripper.set_translation([0, 0, 0.081])
X_EeGripper.set_rotation(
RollPitchYaw(np.pi / 2, 0, np.pi / 2).ToRotationMatrix().matrix())
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()
tree = plant.tree()
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_plant_desired = np.zeros(nq + nv)
x_plant_desired[0:7] = q_iiwa_desired
x_plant_desired[7:9] = q_gripper_desired
x_plant_desired[nq:nq + 7] = v_iiwa_desired
x_plant_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 test_contact_force(self):
"""A block sitting on a table."""
object_file_path = FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf")
table_file_path = FindResourceOrThrow(
"drake/examples/kuka_iiwa_arm/models/table/"
"extra_heavy_duty_table_surface_only_collision.sdf")
# T: tabletop frame.
X_TObject = Isometry3.Identity()
X_TObject.set_translation([0, 0, 0.2])
builder = DiagramBuilder()
plant = MultibodyPlant(0.002)
_, scene_graph = AddMultibodyPlantSceneGraph(builder, plant)
object_model = Parser(plant=plant).AddModelFromFile(object_file_path)
table_model = Parser(plant=plant).AddModelFromFile(table_file_path)
# Weld table to world.
plant.WeldFrames(A=plant.world_frame(),
B=plant.GetFrameByName("link", table_model))
plant.AddForceElement(UniformGravityFieldElement())
plant.Finalize()
# Add meshcat visualizer.
viz = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=None, open_browser=False))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
viz.get_input_port(0))
# Add contact visualizer.
contact_viz = builder.AddSystem(
MeshcatContactVisualizer(meshcat_viz=viz,
force_threshold=0,
contact_force_scale=10,
plant=plant))
contact_input_port = contact_viz.GetInputPort("contact_results")
builder.Connect(plant.GetOutputPort("contact_results"),
contact_input_port)
builder.Connect(scene_graph.get_pose_bundle_output_port(),
contact_viz.GetInputPort("pose_bundle"))
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
mbp_context = diagram.GetMutableSubsystemContext(
plant, diagram_context)
X_WT = plant.CalcRelativeTransform(mbp_context, plant.world_frame(),
plant.GetFrameByName("top_center"))
plant.SetFreeBodyPose(mbp_context,
plant.GetBodyByName("base_link", object_model),
X_WT.multiply(X_TObject))
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.StepTo(1.0)
contact_viz_context = (diagram.GetMutableSubsystemContext(
contact_viz, diagram_context))
contact_results = contact_viz.EvalAbstractInput(
contact_viz_context, contact_input_port.get_index()).get_value()
self.assertGreater(contact_results.num_contacts(), 0)
self.assertEqual(contact_viz._contact_key_counter, 4)
def GetBrickPose(config_file, viz=False):
"""Estimates the pose of the foam brick in a ManipulationStation setup.
@param config_file str. The path to a camera configuration file.
@param viz bool. If True, save point clouds to numpy arrays.
@return An Isometry3 representing the pose of the brick.
"""
builder = DiagramBuilder()
# create the PointCloudToPoseSystem
pc_to_pose = builder.AddSystem(
PointCloudToPoseSystem(config_file, viz, SegmentFoamBrick,
GetFoamBrickPose))
# realsense serial numbers are >> 100
use_hardware = int(pc_to_pose.camera_configs["left_camera_serial"]) > 100
if use_hardware:
camera_ids = [
pc_to_pose.camera_configs["left_camera_serial"],
pc_to_pose.camera_configs["middle_camera_serial"],
pc_to_pose.camera_configs["right_camera_serial"]
]
station = builder.AddSystem(
ManipulationStationHardwareInterface(camera_ids))
station.Connect()
else:
station = builder.AddSystem(ManipulationStation())
station.AddCupboard()
object_file_path = \
"drake/examples/manipulation_station/models/061_foam_brick.sdf"
brick = AddModelFromSdfFile(FindResourceOrThrow(object_file_path),
station.get_mutable_multibody_plant(),
station.get_mutable_scene_graph())
station.Finalize()
# add systems to convert the depth images from ManipulationStation to
# PointClouds
left_camera_info = pc_to_pose.camera_configs["left_camera_info"]
middle_camera_info = pc_to_pose.camera_configs["middle_camera_info"]
right_camera_info = pc_to_pose.camera_configs["right_camera_info"]
left_dut = builder.AddSystem(
mut.DepthImageToPointCloud(camera_info=left_camera_info))
middle_dut = builder.AddSystem(
mut.DepthImageToPointCloud(camera_info=middle_camera_info))
right_dut = builder.AddSystem(
mut.DepthImageToPointCloud(camera_info=right_camera_info))
left_name_prefix = \
"camera_" + pc_to_pose.camera_configs["left_camera_serial"]
middle_name_prefix = \
"camera_" + pc_to_pose.camera_configs["middle_camera_serial"]
right_name_prefix = \
"camera_" + pc_to_pose.camera_configs["right_camera_serial"]
# connect the depth images to the DepthImageToPointCloud converters
builder.Connect(station.GetOutputPort(left_name_prefix + "_depth_image"),
left_dut.depth_image_input_port())
builder.Connect(station.GetOutputPort(middle_name_prefix + "_depth_image"),
middle_dut.depth_image_input_port())
builder.Connect(station.GetOutputPort(right_name_prefix + "_depth_image"),
right_dut.depth_image_input_port())
# connect the rgb images to the PointCloudToPoseSystem
builder.Connect(station.GetOutputPort(left_name_prefix + "_rgb_image"),
pc_to_pose.GetInputPort("camera_left_rgb"))
builder.Connect(station.GetOutputPort(middle_name_prefix + "_rgb_image"),
pc_to_pose.GetInputPort("camera_middle_rgb"))
builder.Connect(station.GetOutputPort(right_name_prefix + "_rgb_image"),
pc_to_pose.GetInputPort("camera_right_rgb"))
# connect the XYZ point clouds to PointCloudToPoseSystem
builder.Connect(left_dut.point_cloud_output_port(),
pc_to_pose.GetInputPort("left_point_cloud"))
builder.Connect(middle_dut.point_cloud_output_port(),
pc_to_pose.GetInputPort("middle_point_cloud"))
builder.Connect(right_dut.point_cloud_output_port(),
pc_to_pose.GetInputPort("right_point_cloud"))
diagram = builder.Build()
simulator = Simulator(diagram)
if not use_hardware:
X_WObject = Isometry3.Identity()
X_WObject.set_translation([.6, 0, 0])
station_context = diagram.GetMutableSubsystemContext(
station, simulator.get_mutable_context())
station.get_mutable_multibody_plant().tree().SetFreeBodyPoseOrThrow(
station.get_mutable_multibody_plant().GetBodyByName(
"base_link", brick), X_WObject,
station.GetMutableSubsystemContext(
station.get_mutable_multibody_plant(), station_context))
context = diagram.GetMutableSubsystemContext(
pc_to_pose, simulator.get_mutable_context())
# returns the pose of the brick, of type Isometry3
return pc_to_pose.GetOutputPort("X_WObject").Eval(context)
def test_multibody_plant_api_via_parsing(self):
# TODO(eric.cousineau): Decouple this when construction can be done
# without parsing.
# This a subset of `multibody_plant_sdf_parser_test.cc`.
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant(time_step=0.01)
model_instance = Parser(plant).AddModelFromFile(file_name)
self.assertIsInstance(model_instance, ModelInstanceIndex)
plant.Finalize()
benchmark = MakeAcrobotPlant(AcrobotParameters(), True)
self.assertEqual(plant.num_bodies(), benchmark.num_bodies())
self.assertEqual(plant.num_joints(), benchmark.num_joints())
self.assertEqual(plant.num_actuators(), benchmark.num_actuators())
self.assertEqual(plant.num_model_instances(),
benchmark.num_model_instances() + 1)
self.assertEqual(plant.num_positions(), benchmark.num_positions())
self.assertEqual(plant.num_positions(model_instance=model_instance),
benchmark.num_positions())
self.assertEqual(plant.num_velocities(), benchmark.num_velocities())
self.assertEqual(plant.num_multibody_states(),
benchmark.num_multibody_states())
self.assertEqual(plant.num_actuated_dofs(),
benchmark.num_actuated_dofs())
self.assertTrue(plant.is_finalized())
self.assertTrue(plant.HasBodyNamed(name="Link1"))
self.assertTrue(
plant.HasBodyNamed(name="Link1", model_instance=model_instance))
self.assertTrue(plant.HasJointNamed(name="ShoulderJoint"))
self.assertTrue(
plant.HasJointNamed(name="ShoulderJoint",
model_instance=model_instance))
shoulder = plant.GetJointByName(name="ShoulderJoint")
self._test_joint_api(shoulder)
np.testing.assert_array_equal(shoulder.lower_limits(), [-np.inf])
np.testing.assert_array_equal(shoulder.upper_limits(), [np.inf])
self.assertIs(
shoulder,
plant.GetJointByName(name="ShoulderJoint",
model_instance=model_instance))
self._test_joint_actuator_api(
plant.GetJointActuatorByName(name="ElbowJoint"))
self._test_body_api(plant.GetBodyByName(name="Link1"))
self.assertIs(
plant.GetBodyByName(name="Link1"),
plant.GetBodyByName(name="Link1", model_instance=model_instance))
self.assertEqual(len(plant.GetBodyIndices(model_instance)), 2)
self._test_frame_api(plant.GetFrameByName(name="Link1"))
self.assertIs(
plant.GetFrameByName(name="Link1"),
plant.GetFrameByName(name="Link1", model_instance=model_instance))
self.assertEqual(model_instance,
plant.GetModelInstanceByName(name="acrobot"))
self.assertIsInstance(plant.get_actuation_input_port(), InputPort)
self.assertIsInstance(plant.get_continuous_state_output_port(),
OutputPort)
self.assertIsInstance(plant.get_contact_results_output_port(),
OutputPort)
tree = plant.tree()
self.check_old_spelling_exists(tree.num_frames)
self.assertIsInstance(plant.num_frames(), int)
self.check_old_spelling_exists(tree.get_body)
self.assertIsInstance(plant.get_body(body_index=BodyIndex(0)), Body)
self.check_old_spelling_exists(tree.get_joint)
self.assertIs(shoulder, plant.get_joint(joint_index=JointIndex(0)))
self.check_old_spelling_exists(tree.get_joint_actuator)
self.assertIsInstance(
plant.get_joint_actuator(actuator_index=JointActuatorIndex(0)),
JointActuator)
self.check_old_spelling_exists(tree.get_frame)
self.assertIsInstance(plant.get_frame(frame_index=FrameIndex(0)),
Frame)
self.check_old_spelling_exists(tree.GetModelInstanceName)
self.assertEqual(
"acrobot",
plant.GetModelInstanceName(model_instance=model_instance))
def load_station(time_step=0.0, plan=None):
# TODO: map names to SDF paths
# https://github.com/RobotLocomotion/drake/blob/master/bindings/pydrake/examples/manipulation_station_py.cc
#object_file_path = FindResourceOrThrow(
# "drake/external/models_robotlocomotion/ycb_objects/061_foam_brick.sdf")
object_file_path = FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf")
# RuntimeError: Error control wants to select step smaller than minimum allowed (1e-14)
station = ManipulationStation(time_step, IiwaCollisionModel.kBoxCollision)
plant = station.get_mutable_multibody_plant()
scene_graph = station.get_mutable_scene_graph()
robot = plant.GetModelInstanceByName('iiwa')
gripper = plant.GetModelInstanceByName('gripper')
station.AddCupboard()
brick = AddModelFromSdfFile(file_name=object_file_path,
model_name="brick",
plant=plant,
scene_graph=scene_graph)
station.Finalize()
door_names = ['left_door', 'right_door']
doors = [plant.GetBodyByName(name).index() for name in door_names]
initial_conf = [0, 0.6 - np.pi / 6, 0, -1.75, 0, 1.0, 0]
#initial_conf[1] += np.pi / 6
initial_positions = {
plant.GetJointByName('left_door_hinge'):
-DOOR_CLOSED,
#plant.GetJointByName('left_door_hinge'): -np.pi / 2,
plant.GetJointByName('right_door_hinge'):
np.pi / 2,
#plant.GetJointByName('right_door_hinge'): np.pi,
}
initial_positions.update(
zip(get_movable_joints(plant, robot), initial_conf))
initial_poses = {
brick:
create_transform(translation=[0.3, 0, 0], rotation=[0, 0, np.pi / 2]),
}
goal_poses = {
brick:
create_transform(translation=[0.8, 0.2, 0.2927],
rotation=[0, 0, 5 * np.pi / 4]),
}
diagram, state_machine = build_manipulation_station(station, plan)
task = Task(diagram,
plant,
scene_graph,
robot,
gripper,
movable=[brick],
doors=doors,
initial_positions=initial_positions,
initial_poses=initial_poses,
goal_poses=goal_poses,
reset_robot=True,
reset_doors=False)
task.set_initial()
return task, diagram, state_machine
from pydrake.common import FindResourceOrThrow
from pydrake.multibody.rigid_body_plant import RigidBodyPlant
from pydrake.multibody.rigid_body_tree import RigidBodyTree
from pydrake.systems.analysis import Simulator
tree = RigidBodyTree(
FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf"))
simulator = Simulator(RigidBodyPlant(tree))
# The following code demonstrates how to access the components of the manipulator dynamics equations from Drake's MultibodyPlant class, using the Acrobot as an example system. Note that not all functionality from the C++ Drake API is available in the Python API; however, the Python API is sufficiently powerful to give access to the all componetents of the manipulator dynamics equations
from math import pi
# Import utilities from pydrake
from pydrake.common import FindResourceOrThrow
from pydrake.all import MultibodyPlant
from pydrake.multibody.parsing import Parser
#from pydrake.autodiffutils import AutoDiffXd
from pydrake.multibody.tree import MultibodyForces
#%% [markdown]
# # Loading a the acrobot model from a URDF
#
# The Acrobot model is supplied with the Drake docker image. We can load it and directly create a Drake MultibodyPlant. To fully utilitze the MultibodyPlant, we will also need to create a Context, which stores values associated with the MultibodyPlant such as the configuration and velocity variables.
# Find and load the Acrobot URDF
acro_file = FindResourceOrThrow("drake/examples/acrobot/Acrobot.urdf")
# Create a Multibody plant model from the acrobot
plant = MultibodyPlant(0.0)
acrobot = Parser(plant).AddModelFromFile(acro_file)
plant.Finalize()
# The CONTEXT of the system stores information, such as the state, about the multibody plant. The context is necessary to calculate values such as the inertia matrix
context = plant.CreateDefaultContext()
##% [markdown]
#
# The default context is not very interesting, so let's set some nonzero values for the configuration and velocity variables
q = [pi / 4, pi / 4]
v = [0.1, -0.1]
plant.SetPositions(context, q)
plant.SetVelocities(context, v)
#%% [markdown]
# # DYNAMICS
builder = DiagramBuilder()
# A multibody plant is a *system* (in the drake sense) holding all the info for multi-body rigid bodies, providing ports for forces and continuous state, geometry etc.
# The "builder" as an argument is a kind of a "side-effectey" thing, we want to add our created multibodyplant *to* this builder
# The 0.0 is the "discrete time step". A value of 0.0 means that we have made this a continuous system
# Note also, this constructor is overloaded (but this is not a thing you can do in python naturally. It is an artefact of the C++ port)
time_step = 0.002
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step)
# Load in the panda
# The parser takes a multibody plant mutably, so that anything parsed with it gets automatically added to this multibody system
parser = Parser(plant)
# Files are getting grabbed from "/opt/drake/share/drake/...
panda_arm_hand_file = FindResourceOrThrow("drake/manipulation/models/franka_description/urdf/panda_arm_hand.urdf")
brick_file = FindResourceOrThrow("drake/examples/manipulation_station/models/061_foam_brick.sdf")
bin_file = FindResourceOrThrow("drake/examples/manipulation_station/models/bin.sdf")
# Actually parse in the model
panda = parser.AddModelFromFile(panda_arm_hand_file, model_name="panda")
# Don't want panda to drop through the sky or fall over so...
# It would be nice if there was an option to create a floor...
# The X_AB part is "relative pose" (monogram notation: The pose (X) of B relative to A
plant.WeldFrames(
plant.world_frame(), plant.GetFrameByName("panda_link0", panda), X_AB=RigidTransform(np.array([0.0, 0.5, 0.0]))
)
# Add a little example brick
brick = parser.AddModelFromFile(brick_file, model_name="brick")
def test_multibody_tree_kinematics(self):
file_name = FindResourceOrThrow(
"drake/examples/double_pendulum/models/double_pendulum.sdf")
plant = MultibodyPlant()
AddModelFromSdfFile(file_name, plant)
plant.Finalize()
context = plant.CreateDefaultContext()
tree = plant.tree()
world_frame = plant.world_frame()
base = plant.GetBodyByName("base")
base_frame = plant.GetFrameByName("base")
X_WL = tree.CalcRelativeTransform(context,
frame_A=world_frame,
frame_B=base_frame)
self.assertIsInstance(X_WL, Isometry3)
p_AQi = tree.CalcPointsPositions(context=context,
frame_B=base_frame,
p_BQi=np.array([[0, 1, 2],
[10, 11, 12]]).T,
frame_A=world_frame).T
self.assertTupleEqual(p_AQi.shape, (2, 3))
Jv_WL = tree.CalcFrameGeometricJacobianExpressedInWorld(
context=context, frame_B=base_frame, p_BoFo_B=[0, 0, 0])
self.assertTupleEqual(Jv_WL.shape, (6, plant.num_velocities()))
# Compute body pose.
X_WBase = tree.EvalBodyPoseInWorld(context, base)
self.assertIsInstance(X_WBase, Isometry3)
# All body poses.
X_WB_list = tree.CalcAllBodyPosesInWorld(context)
self.assertEqual(len(X_WB_list), 4)
for X_WB in X_WB_list:
self.assertIsInstance(X_WB, Isometry3)
# Compute body velocities.
v_WB_list = tree.CalcAllBodySpatialVelocitiesInWorld(context)
self.assertEqual(len(v_WB_list), 4)
for v_WB in v_WB_list:
self.assertIsInstance(v_WB, SpatialVelocity)
# - Sanity check.
v_WBase_flat = np.hstack(
(v_WB_list[0].rotational(), v_WB_list[0].translational()))
self.assertTrue(np.allclose(v_WBase_flat, np.zeros(6)))
# Set pose for the base.
X_WB_desired = Isometry3.Identity()
X_WB = tree.CalcRelativeTransform(context, world_frame, base_frame)
tree.SetFreeBodyPoseOrThrow(body=base,
X_WB=X_WB_desired,
context=context)
self.assertTrue(np.allclose(X_WB.matrix(), X_WB_desired.matrix()))
# Set a spatial velocity for the base.
v_WB = SpatialVelocity(w=[1, 2, 3], v=[4, 5, 6])
tree.SetFreeBodySpatialVelocityOrThrow(body=base,
V_WB=v_WB,
context=context)
v_base = tree.EvalBodySpatialVelocityInWorld(context, base)
self.assertTrue(np.allclose(v_base.rotational(), v_WB.rotational()))
self.assertTrue(
np.allclose(v_base.translational(), v_WB.translational()))
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")
timestep = 0.0002
plant = MultibodyPlant(timestep)
iiwa_model = AddModelFromSdfFile(file_name=iiwa_sdf_path,
model_name='robot',
scene_graph=None,
plant=plant)
gripper_model = AddModelFromSdfFile(file_name=wsg50_sdf_path,
model_name='gripper',
scene_graph=None,
plant=plant)
# Weld the base of arm and gripper to reduce the number of states.
X_EeGripper = Isometry3.Identity()
X_EeGripper.set_translation([0, 0, 0.081])
X_EeGripper.set_rotation(
RollPitchYaw(np.pi / 2, 0, np.pi / 2).ToRotationMatrix().matrix())
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()
tree = plant.tree()
nq = plant.num_positions()
nv = plant.num_velocities()
q_iiwa_desired = np.zeros(7)
v_iiwa_desired = np.zeros(7)
q_gripper_desired = np.zeros(2)
v_gripper_desired = np.zeros(2)
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_plant_desired = np.zeros(nq + nv)
x_plant_desired[0:7] = q_iiwa_desired
x_plant_desired[7:9] = q_gripper_desired
x_plant_desired[nq:nq + 7] = v_iiwa_desired
x_plant_desired[nq + 7:nq + nv] = v_gripper_desired
x_plant = tree.get_mutable_multibody_state_vector(context)
x_plant[:] = x_plant_desired
# Get state from context.
x = tree.get_multibody_state_vector(context)
q = x[0:nq]
v = x[nq:nq + nv]
# Get positions and velocities of specific model instances
# from the postion/velocity vector of the plant.
q_iiwa = tree.get_positions_from_array(iiwa_model, q)
q_gripper = tree.get_positions_from_array(gripper_model, q)
v_iiwa = tree.get_velocities_from_array(iiwa_model, v)
v_gripper = tree.get_velocities_from_array(gripper_model, v)
# Assert that the get_positions_from_array 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))
def test_kinematics_api(self):
# TODO(eric.cousineau): Reduce these tests to only test API, and do
# simple sanity checks on the numbers.
tree = RigidBodyTree(
FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf"))
num_q = 7
num_v = 7
self.assertEqual(tree.number_of_positions(), num_q)
self.assertEqual(tree.number_of_velocities(), num_v)
q = np.zeros(num_q)
v = np.zeros(num_v)
# Trivial kinematics.
kinsol = tree.doKinematics(q, v)
p = tree.transformPoints(kinsol, np.zeros(3), 0, 1)
self.assertTrue(np.allclose(p, np.zeros(3)))
# AutoDiff jacobians.
def do_transform(q):
kinsol = tree.doKinematics(q)
point = np.ones(3)
return tree.transformPoints(kinsol, point, 2, 0)
# - Position.
value = do_transform(q)
self.assertTrue(np.allclose(value, np.ones(3)))
# - Gradient.
g = jacobian(do_transform, q)
g_expected = np.array([[[1, 0, 0, 0, 1, -1, 1]],
[[0, 1, 0, -1, 0, 1, 0]],
[[0, 0, 1, 1, -1, 0, -1]]])
self.assertTrue(np.allclose(g, g_expected))
# Relative transform.
q[:] = 0
q[6] = np.pi / 2
kinsol = tree.doKinematics(q)
T = tree.relativeTransform(kinsol, 1, 2)
T_expected = np.array([[0, 0, 1, 0], [0, 1, 0, 0], [-1, 0, 0, 0],
[0, 0, 0, 1]])
self.assertTrue(np.allclose(T, T_expected))
# Do FK and compare pose of 'arm' with expected pose.
q[:] = 0
q[6] = np.pi / 2
kinsol = tree.doKinematics(q)
T = tree.CalcBodyPoseInWorldFrame(kinsol, tree.FindBody("arm"))
T_expected = np.array([[0, 0, 1, 0], [0, 1, 0, 0], [-1, 0, 0, 0],
[0, 0, 0, 1]])
self.assertTrue(np.allclose(T, T_expected))
# Geometric Jacobian.
# Construct a new tree with a quaternion floating base.
tree = RigidBodyTree(
FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf"),
floating_base_type=FloatingBaseType.kQuaternion)
num_q = 8
num_v = 7
self.assertEqual(tree.number_of_positions(), num_q)
self.assertEqual(tree.number_of_velocities(), num_v)
q = tree.getZeroConfiguration()
kinsol = tree.doKinematics(q)
# - Sanity check sizes.
J_default, v_indices_default = tree.geometricJacobian(kinsol, 0, 2, 0)
self.assertEqual(J_default.shape[0], 6)
self.assertEqual(J_default.shape[1], num_v)
self.assertEqual(len(v_indices_default), num_v)
# - Check that default value for in_terms_of_qdot is false.
J_not_in_terms_of_q_dot, v_indices_not_in_terms_of_qdot = \
tree.geometricJacobian(kinsol, 0, 2, 0, False)
self.assertTrue((J_default == J_not_in_terms_of_q_dot).all())
self.assertEqual(v_indices_default, v_indices_not_in_terms_of_qdot)
# - Check with in_terms_of_qdot set to True.
J_in_terms_of_q_dot, v_indices_in_terms_of_qdot = \
tree.geometricJacobian(kinsol, 0, 2, 0, True)
self.assertEqual(J_in_terms_of_q_dot.shape[0], 6)
self.assertEqual(J_in_terms_of_q_dot.shape[1], num_q)
self.assertEqual(len(v_indices_in_terms_of_qdot), num_q)
# - Check that drawKinematicTree runs
tree.drawKinematicTree(
join(os.environ["TEST_TMPDIR"], "test_graph.dot"))
def test_multibody_dynamics(self, T):
MultibodyPlant = MultibodyPlant_[T]
MultibodyForces = MultibodyForces_[T]
SpatialForce = SpatialForce_[T]
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
# N.B. `Parser` only supports `MultibodyPlant_[float]`.
plant_f = MultibodyPlant_[float]()
Parser(plant_f).AddModelFromFile(file_name)
# Getting ready for when we set foot on Mars :-).
gravity_vector = np.array([0.0, 0.0, -3.71])
plant_f.mutable_gravity_field().set_gravity_vector(gravity_vector)
plant_f.Finalize()
plant = to_type(plant_f, T)
context = plant.CreateDefaultContext()
numpy_compare.assert_float_equal(
plant.gravity_field().gravity_vector(), gravity_vector)
# Set an arbitrary configuration away from the model's fixed point.
plant.SetPositions(context, [0.1, 0.2])
M = plant.CalcMassMatrixViaInverseDynamics(context)
Cv = plant.CalcBiasTerm(context)
self.assertTrue(M.shape == (2, 2))
self.assert_sane(M)
self.assertTrue(Cv.shape == (2, ))
self.assert_sane(Cv, nonzero=False)
nv = plant.num_velocities()
vd_d = np.zeros(nv)
tau = plant.CalcInverseDynamics(
context, vd_d, MultibodyForces(plant))
self.assertEqual(tau.shape, (2,))
self.assert_sane(tau, nonzero=False)
# - Existence checks.
# Gravity leads to non-zero potential energy.
potential_energy = plant.CalcPotentialEnergy(context)
numpy_compare.assert_float_not_equal(potential_energy, 0.)
plant.CalcConservativePower(context)
tau_g = plant.CalcGravityGeneralizedForces(context)
self.assertEqual(tau_g.shape, (nv,))
self.assert_sane(tau_g, nonzero=True)
B = plant.MakeActuationMatrix()
numpy_compare.assert_float_equal(B, np.array([[0.], [1.]]))
forces = MultibodyForces(plant=plant)
plant.CalcForceElementsContribution(context=context, forces=forces)
# Test generalized forces.
# N.B. Cannot use `ndarray[object]` to reference existing C arrays
# (#8116).
if T == float:
forces.mutable_generalized_forces()[:] = 1
np.testing.assert_equal(forces.generalized_forces(), 1)
forces.SetZero()
np.testing.assert_equal(forces.generalized_forces(), 0)
# Test body force accessors and mutators.
link2 = plant.GetBodyByName("Link2")
self.assertIsInstance(
link2.GetForceInWorld(context, forces), SpatialForce)
forces.SetZero()
F_expected = np.array([1., 2., 3., 4., 5., 6.])
link2.AddInForceInWorld(
context, F_Bo_W=SpatialForce(F=F_expected), forces=forces)
coeff = numpy_compare.to_float(
link2.GetForceInWorld(context, forces).get_coeffs())
numpy_compare.assert_float_equal(coeff, F_expected)
link2.AddInForce(
context, p_BP_E=[0, 0, 0], F_Bp_E=SpatialForce(F=F_expected),
frame_E=plant.world_frame(), forces=forces)
# Also check accumulation.
np.testing.assert_equal(numpy_compare.to_float(
link2.GetForceInWorld(context, forces).get_coeffs()),
2 * F_expected)
def test_model_instance_state_access_by_array(self, T):
# N.B. Please check warning above in `check_multibody_state_access`.
MultibodyPlant = MultibodyPlant_[T]
# 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
# N.B. `Parser` only supports `MultibodyPlant_[float]`.
plant_f = MultibodyPlant_[float](timestep)
parser = Parser(plant_f)
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_[float](
RollPitchYaw_[float](np.pi / 2, 0, np.pi / 2), [0, 0, 0.081])
plant_f.WeldFrames(
A=plant_f.world_frame(),
B=plant_f.GetFrameByName("iiwa_link_0", iiwa_model))
plant_f.WeldFrames(
A=plant_f.GetFrameByName("iiwa_link_7", iiwa_model),
B=plant_f.GetFrameByName("body", gripper_model),
X_AB=X_EeGripper)
plant_f.Finalize()
plant = to_type(plant_f, T)
# 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
if T == float:
x = plant.GetMutablePositionsAndVelocities(context=context)
x[:] = x_desired
else:
plant.SetPositionsAndVelocities(context, x_desired)
q = plant.GetPositions(context=context)
v = plant.GetVelocities(context=context)
# Get state from context.
x = plant.GetPositionsAndVelocities(context=context)
if T == float:
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)
numpy_compare.assert_equal(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)
numpy_compare.assert_equal(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.
numpy_compare.assert_float_equal(q_iiwa, q_iiwa_desired)
numpy_compare.assert_float_equal(v_iiwa, v_iiwa_desired)
numpy_compare.assert_float_equal(q_gripper, q_gripper_desired)
numpy_compare.assert_float_equal(v_gripper, v_gripper_desired)
if T == float:
# Verify that SetPositionsInArray() and SetVelocitiesInArray()
# works.
plant.SetPositionsInArray(
model_instance=iiwa_model, q_instance=np.zeros(nq_iiwa), q=q)
numpy_compare.assert_float_equal(
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)
numpy_compare.assert_float_equal(
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)
numpy_compare.assert_float_equal(u_iiwa, u[:7])
def load_manipulation(time_step=0.0,
use_meshcat=True,
use_controllers=True,
use_external=False):
if not use_external:
AMAZON_TABLE_PATH = FindResourceOrThrow(
"drake/examples/manipulation_station/models/amazon_table_simplified.sdf"
)
CUPBOARD_PATH = FindResourceOrThrow(
"drake/examples/manipulation_station/models/cupboard.sdf")
IIWA7_PATH = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/iiwa7/iiwa7_with_box_collision.sdf"
)
FOAM_BRICK_PATH = FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf")
goal_shelf = 'shelf_lower'
#goal_shelf = 'shelf_upper'
else:
AMAZON_TABLE_PATH = FindResourceOrThrow(
"drake/external/models_robotlocomotion/manipulation_station/amazon_table_simplified.sdf"
)
CUPBOARD_PATH = FindResourceOrThrow(
"drake/external/models_robotlocomotion/manipulation_station/cupboard.sdf"
)
IIWA7_PATH = FindResourceOrThrow(
"drake/external/models_robotlocomotion/iiwa7/iiwa7_no_collision.sdf"
)
FOAM_BRICK_PATH = FindResourceOrThrow(
"drake/external/models_robotlocomotion/ycb_objects/061_foam_brick.sdf"
)
goal_shelf = 'bottom'
#IIWA7_PATH = os.path.join(MODELS_DIR, "iiwa_description/iiwa7/iiwa7_with_box_collision.sdf")
plant = MultibodyPlant(time_step=time_step)
scene_graph = SceneGraph()
dx_table_center_to_robot_base = 0.3257
dz_table_top_robot_base = 0.0127
dx_cupboard_to_table_center = 0.43 + 0.15
dz_cupboard_to_table_center = 0.02
cupboard_height = 0.815
cupboard_x = dx_table_center_to_robot_base + dx_cupboard_to_table_center
cupboard_z = dz_cupboard_to_table_center + cupboard_height / 2.0 - dz_table_top_robot_base
robot = AddModelFromSdfFile(file_name=IIWA7_PATH,
model_name='iiwa',
scene_graph=scene_graph,
plant=plant)
gripper = AddModelFromSdfFile(file_name=WSG50_SDF_PATH,
model_name='gripper',
scene_graph=scene_graph,
plant=plant) # TODO: sdf frame/link error
table = AddModelFromSdfFile(file_name=AMAZON_TABLE_PATH,
model_name='table',
scene_graph=scene_graph,
plant=plant)
cupboard = AddModelFromSdfFile(file_name=CUPBOARD_PATH,
model_name='cupboard',
scene_graph=scene_graph,
plant=plant)
brick = AddModelFromSdfFile(file_name=FOAM_BRICK_PATH,
model_name='brick',
scene_graph=scene_graph,
plant=plant)
weld_gripper(plant, robot, gripper)
weld_to_world(plant, robot, create_transform())
weld_to_world(
plant, table,
create_transform(translation=[
dx_table_center_to_robot_base, 0, -dz_table_top_robot_base
]))
weld_to_world(
plant, cupboard,
create_transform(translation=[cupboard_x, 0, cupboard_z],
rotation=[0, 0, np.pi]))
plant.Finalize(scene_graph)
shelves = [
'bottom',
'shelf_lower',
'shelf_upper',
'top',
]
goal_surface = Surface(plant, cupboard, 'top_and_bottom',
shelves.index(goal_shelf))
surfaces = [
#Surface(plant, table, 'amazon_table', 0),
#goal_surface,
]
door_names = ['left_door', 'right_door']
#door_names = []
doors = [plant.GetBodyByName(name).index() for name in door_names]
door_position = DOOR_CLOSED # ~np.pi/2
#door_position = DOOR_OPEN
#door_position = np.pi
#door_position = np.pi/2
#door_position = np.pi/8
initial_positions = {
plant.GetJointByName('left_door_hinge'):
-door_position,
#plant.GetJointByName('right_door_hinge'): door_position,
plant.GetJointByName('right_door_hinge'):
np.pi / 2,
}
initial_conf = [0, 0.6 - np.pi / 6, 0, -1.75, 0, 1.0, 0]
#initial_conf[1] += np.pi / 6
initial_positions.update(
zip(get_movable_joints(plant, robot), initial_conf))
initial_poses = {
#brick: create_transform(translation=[0.6, 0, 0]),
brick:
create_transform(translation=[0.3, 0, 0], rotation=[0, 0, np.pi / 2]),
#brick: create_transform(translation=[0.2, -0.3, 0], rotation=[0, 0, np.pi/8]),
}
goal_poses = {
brick:
create_transform(translation=[0.8, 0.2, 0.2927],
rotation=[0, 0, np.pi / 8]),
}
goal_holding = [
#brick,
]
goal_on = [
#(brick, goal_surface),
]
diagram, state_machine = build_diagram(plant,
scene_graph,
robot,
gripper,
meshcat=use_meshcat,
controllers=use_controllers)
task = Task(diagram,
plant,
scene_graph,
robot,
gripper,
movable=[brick],
surfaces=surfaces,
doors=doors,
initial_positions=initial_positions,
initial_poses=initial_poses,
goal_poses=goal_poses,
goal_holding=goal_holding,
goal_on=goal_on,
reset_robot=True,
reset_doors=False)
return task, diagram, state_machine
def setUp(self):
self.dut = FindResourceOrThrow(
f"drake/examples/acrobot/dev/spong_sim_main_{_backend}")
# 4 states x 30 seconds of samples at 20 Hz per sample_scenario.
self.nominal_x_tape_shape = (4, 601)
def load_manipulation(time_step=0.0):
source = True
if source:
AMAZON_TABLE_PATH = FindResourceOrThrow(
"drake/examples/manipulation_station/models/amazon_table_simplified.sdf")
CUPBOARD_PATH = FindResourceOrThrow(
"drake/examples/manipulation_station/models/cupboard.sdf")
#IIWA7_PATH = FindResourceOrThrow(
# "drake/manipulation/models/iiwa_description/iiwa7/iiwa7_with_box_collision.sdf")
IIWA7_PATH = os.path.join(MODELS_DIR, "iiwa_description/iiwa7/iiwa7_with_box_collision.sdf")
FOAM_BRICK_PATH = FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf")
goal_shelf = 'shelf_lower'
else:
AMAZON_TABLE_PATH = FindResourceOrThrow(
"drake/external/models_robotlocomotion/manipulation_station/amazon_table_simplified.sdf")
CUPBOARD_PATH = FindResourceOrThrow(
"drake/external/models_robotlocomotion/manipulation_station/cupboard.sdf")
IIWA7_PATH = FindResourceOrThrow(
"drake/external/models_robotlocomotion/iiwa7/iiwa7_no_collision.sdf")
#IIWA7_PATH = os.path.join(MODELS_DIR, "iiwa_description/iiwa7/iiwa7_with_box_collision.sdf")
FOAM_BRICK_PATH = FindResourceOrThrow(
"drake/external/models_robotlocomotion/ycb_objects/061_foam_brick.sdf")
goal_shelf = 'bottom'
mbp = MultibodyPlant(time_step=time_step)
scene_graph = SceneGraph()
dx_table_center_to_robot_base = 0.3257
dz_table_top_robot_base = 0.0127
dx_cupboard_to_table_center = 0.43 + 0.15
dz_cupboard_to_table_center = 0.02
cupboard_height = 0.815
cupboard_x = dx_table_center_to_robot_base + dx_cupboard_to_table_center
cupboard_z = dz_cupboard_to_table_center + cupboard_height / 2.0 - dz_table_top_robot_base
robot = AddModelFromSdfFile(file_name=IIWA7_PATH, model_name='iiwa',
scene_graph=scene_graph, plant=mbp)
gripper = AddModelFromSdfFile(file_name=WSG50_SDF_PATH, model_name='gripper',
scene_graph=scene_graph, plant=mbp) # TODO: sdf frame/link error
amazon_table = AddModelFromSdfFile(file_name=AMAZON_TABLE_PATH, model_name='amazon_table',
scene_graph=scene_graph, plant=mbp)
cupboard = AddModelFromSdfFile(file_name=CUPBOARD_PATH, model_name='cupboard',
scene_graph=scene_graph, plant=mbp)
brick = AddModelFromSdfFile(file_name=FOAM_BRICK_PATH, model_name='brick',
scene_graph=scene_graph, plant=mbp)
# left_door, left_door_hinge, cylinder
weld_gripper(mbp, robot, gripper)
weld_to_world(mbp, robot, create_transform())
weld_to_world(mbp, amazon_table, create_transform(
translation=[dx_table_center_to_robot_base, 0, -dz_table_top_robot_base]))
weld_to_world(mbp, cupboard, create_transform(
translation=[cupboard_x, 0, cupboard_z], rotation=[0, 0, np.pi]))
mbp.Finalize(scene_graph)
shelves = [
'bottom',
'shelf_lower',
'shelf_upper'
'top',
]
goal_surface = VisualElement(cupboard, 'top_and_bottom', shelves.index(goal_shelf))
surfaces = [
#VisualElement(amazon_table, 'amazon_table', 0),
goal_surface,
]
door_names = [
'left_door',
'right_door',
]
doors = [mbp.GetBodyByName(name).index() for name in door_names]
door_position = DOOR_CLOSED # np.pi/2
#door_position = DOOR_OPEN
#door_position = np.pi
#door_position = np.pi/8
initial_positions = {
mbp.GetJointByName('left_door_hinge'): -door_position,
#mbp.GetJointByName('right_door_hinge'): door_position,
mbp.GetJointByName('right_door_hinge'): np.pi,
}
initial_conf = [0, 0.6 - np.pi / 6, 0, -1.75, 0, 1.0, 0]
#initial_conf[1] += np.pi / 6
initial_positions.update(zip(get_movable_joints(mbp, robot), initial_conf))
initial_poses = {
#brick: create_transform(translation=[0.6, 0, 0]),
brick: create_transform(translation=[0.4, 0, 0]),
}
goal_on = [
(brick, goal_surface),
]
task = Task(mbp, scene_graph, robot, gripper, movable=[brick], surfaces=surfaces, doors=doors,
initial_positions=initial_positions, initial_poses=initial_poses, goal_on=goal_on)
return mbp, scene_graph, task
def RunSimulation(robobee_plantBS_torque,
control_law,
x0=np.random.random((15, 1)),
duration=30):
robobee_controller = RobobeeController(control_law)
# Create a simple block diagram containing the plant in feedback
# with the controller.
builder = DiagramBuilder()
# The last pendulum plant we made is now owned by a deleted
# system, so easiest path is for us to make a new one.
plant = builder.AddSystem(
RobobeePlantBSTorque(m=robobee_plantBS_torque.m,
Ixx=robobee_plantBS_torque.Ixx,
Iyy=robobee_plantBS_torque.Iyy,
Izz=robobee_plantBS_torque.Izz,
g=robobee_plantBS_torque.g,
rw_l=robobee_plantBS_torque.rw_l,
bw=robobee_plantBS_torque.bw,
input_max=robobee_plantBS_torque.input_max))
Rigidbody_selector = builder.AddSystem(RigidBodySelection())
print("1. Connecting plant and controller\n")
controller = builder.AddSystem(robobee_controller)
builder.Connect(plant.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), plant.get_input_port(0))
# Create a logger to capture the simulation of our plant
set_time_interval = 0.001
time_interval_multiple = 1000
publish_period = set_time_interval * time_interval_multiple
print("2. Connecting plant to the logger\n")
input_log = builder.AddSystem(SignalLogger(4))
# input_log._DeclarePeriodicPublish(publish_period, 0.0)
builder.Connect(controller.get_output_port(0), input_log.get_input_port(0))
state_log = builder.AddSystem(SignalLogger(18))
# state_log._DeclarePeriodicPublish(publish_period, 0.0)
builder.Connect(plant.get_output_port(0), state_log.get_input_port(0))
# Drake visualization
print("3. Connecting plant output to DrakeVisualizer\n")
rtree = RigidBodyTree(
FindResourceOrThrow("drake/examples/robobee/robobee_twobar.urdf"),
FloatingBaseType.kQuaternion)
lcm = DrakeLcm()
visualizer = builder.AddSystem(
DrakeVisualizer(tree=rtree, lcm=lcm, enable_playback=True))
builder.Connect(plant.get_output_port(0),
Rigidbody_selector.get_input_port(0))
builder.Connect(Rigidbody_selector.get_output_port(0),
visualizer.get_input_port(0))
print("4. Building diagram\n")
diagram = builder.Build()
# Set the initial conditions for the simulation.
context = diagram.CreateDefaultContext()
state = context.get_mutable_continuous_state_vector()
state.SetFromVector(x0)
# Create the simulator.
print("5. Create simulation\n")
simulator = Simulator(diagram, context)
simulator.Initialize()
# simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(1)
simulator.get_integrator().set_fixed_step_mode(True)
simulator.get_integrator().set_maximum_step_size(set_time_interval)
# Simulate for the requested duration.
simulator.StepTo(duration)
return input_log, state_log
def test_contact_force(self):
"""A block sitting on a table."""
object_file_path = FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf")
table_file_path = FindResourceOrThrow(
"drake/examples/kuka_iiwa_arm/models/table/"
"extra_heavy_duty_table_surface_only_collision.sdf")
# T: tabletop frame.
X_TObject = RigidTransform([0, 0, 0.2])
builder = DiagramBuilder()
plant = MultibodyPlant(0.002)
_, scene_graph = AddMultibodyPlantSceneGraph(builder, plant)
table_model = Parser(plant=plant).AddModelFromFile(table_file_path)
object_model = Parser(plant=plant).AddModelFromFile(object_file_path)
# Weld table to world.
plant.WeldFrames(
frame_on_parent_P=plant.world_frame(),
frame_on_child_C=plant.GetFrameByName("link", table_model),
X_PC=RigidTransform(RotationMatrix.MakeXRotation(0.2)))
plant.Finalize()
# Add meshcat visualizer.
viz = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=ZMQ_URL,
open_browser=False))
builder.Connect(scene_graph.get_query_output_port(),
viz.get_geometry_query_input_port())
# Add contact visualizer.
contact_viz = builder.AddSystem(
MeshcatContactVisualizer(meshcat_viz=viz,
force_threshold=0,
contact_force_scale=1,
plant=plant,
contact_force_radius=0.005))
contact_input_port = contact_viz.GetInputPort("contact_results")
builder.Connect(plant.GetOutputPort("contact_results"),
contact_input_port)
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
mbp_context = diagram.GetMutableSubsystemContext(
plant, diagram_context)
X_WT = plant.CalcRelativeTransform(mbp_context, plant.world_frame(),
plant.GetFrameByName("top_center"))
plant.SetFreeBodyPose(mbp_context,
plant.GetBodyByName("base_link", object_model),
X_WT.multiply(X_TObject))
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.AdvanceTo(1.0)
contact_viz_context = (diagram.GetMutableSubsystemContext(
contact_viz, diagram_context))
contact_results = contact_viz.EvalAbstractInput(
contact_viz_context, contact_input_port.get_index()).get_value()
self.assertGreater(contact_results.num_point_pair_contacts(), 0)
self.assertEqual(len(contact_viz._published_contacts), 4)
def _make_tree(self):
urdf_path = FindResourceOrThrow(
"drake/examples/pendulum/Pendulum.urdf")
tree = RigidBodyTree(urdf_path,
floating_base_type=FloatingBaseType.kFixed)
return tree
from pydrake.common import FindResourceOrThrow
from pydrake.multibody.parsing import Parser
from pydrake.multibody.plant import AddMultibodyPlantSceneGraph
from pydrake.systems.analysis import Simulator
from pydrake.systems.framework import DiagramBuilder
from pydrake.systems.primitives import ConstantVectorSource
from pydrake.systems.planar_scenegraph_visualizer import (
PlanarSceneGraphVisualizer)
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
Parser(plant, scene_graph).AddModelFromFile(
FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/urdf/planar_iiwa14_spheres_dense_elbow_collision.urdf"
)) # noqa
plant.Finalize()
visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
torque_command = builder.AddSystem(ConstantVectorSource(np.zeros(3)))
builder.Connect(torque_command.get_output_port(0),
plant.get_actuation_input_port())
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
def RunSimulation(robobee_plant,
control_law,
x0=np.random.random((13, 1)),
duration=30):
robobee_controller = RobobeeController(control_law)
# Create a simple block diagram containing the plant in feedback
# with the controller.
builder = DiagramBuilder()
# The last pendulum plant we made is now owned by a deleted
# system, so easiest path is for us to make a new one.
plant = builder.AddSystem(
RobobeePlant(m=robobee_plant.m,
Ixx=robobee_plant.Ixx,
Iyy=robobee_plant.Iyy,
Izz=robobee_plant.Izz,
g=robobee_plant.g,
input_max=robobee_plant.input_max))
controller = builder.AddSystem(robobee_controller)
builder.Connect(plant.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), plant.get_input_port(0))
# Create a logger to capture the simulation of our plant
input_log = builder.AddSystem(SignalLogger(4))
input_log._DeclarePeriodicPublish(0.033333, 0.0)
builder.Connect(controller.get_output_port(0), input_log.get_input_port(0))
state_log = builder.AddSystem(SignalLogger(13))
state_log._DeclarePeriodicPublish(0.033333, 0.0)
builder.Connect(plant.get_output_port(0), state_log.get_input_port(0))
# Drake visualization
rtree = RigidBodyTree(
FindResourceOrThrow("drake/examples/robobee/robobee.urdf"),
FloatingBaseType.kQuaternion)
lcm = DrakeLcm()
visualizer = builder.AddSystem(
DrakeVisualizer(tree=rtree, lcm=lcm, enable_playback=True))
builder.Connect(plant.get_output_port(0), visualizer.get_input_port(0))
diagram = builder.Build()
# Set the initial conditions for the simulation.
context = diagram.CreateDefaultContext()
state = context.get_mutable_continuous_state_vector()
state.SetFromVector(x0)
# Create the simulator.
simulator = Simulator(diagram, context)
simulator.Initialize()
# simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(1)
simulator.get_integrator().set_fixed_step_mode(False)
simulator.get_integrator().set_maximum_step_size(0.005)
# Simulate for the requested duration.
simulator.StepTo(duration)
return input_log, state_log
def test_flat_terrain(self):
tree = RigidBodyTree(
FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf"))
# Test that AddFlatTerrainToWorld is spelled correctly.
AddFlatTerrainToWorld(tree)
from pydrake.multibody.rigid_body_tree import (
AddModelInstanceFromUrdfFile,
FloatingBaseType,
RigidBodyTree,
RigidBodyFrame,
)
from pydrake.systems.framework import (
BasicVector,
)
from pydrake.systems.sensors import (
CameraInfo,
RgbdCamera,
)
# Create tree describing scene.
urdf_path = FindResourceOrThrow(
"drake/multibody/models/box.urdf")
tree = RigidBodyTree()
AddModelInstanceFromUrdfFile(
urdf_path, FloatingBaseType.kFixed, None, tree)
# - Add frame for camera fixture.
frame = RigidBodyFrame(
name="rgbd camera frame",
body=tree.world(),
xyz=[-2, 0, 2], # Ensure that the box is within range.
rpy=[0, np.pi / 4, 0])
tree.addFrame(frame)
# Create camera.
camera = RgbdCamera(
name="camera", tree=tree, frame=frame,
z_near=0.5, z_far=5.0,
def test_multibody_tree_kinematics(self):
file_name = FindResourceOrThrow(
"drake/examples/double_pendulum/models/double_pendulum.sdf")
plant = MultibodyPlant()
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
context = plant.CreateDefaultContext()
tree = plant.tree()
world_frame = plant.world_frame()
base = plant.GetBodyByName("base")
base_frame = plant.GetFrameByName("base")
self.check_old_spelling_exists(tree.CalcRelativeTransform)
X_WL = plant.CalcRelativeTransform(context,
frame_A=world_frame,
frame_B=base_frame)
self.assertIsInstance(X_WL, Isometry3)
self.check_old_spelling_exists(tree.CalcPointsPositions)
p_AQi = plant.CalcPointsPositions(context=context,
frame_B=base_frame,
p_BQi=np.array([[0, 1, 2],
[10, 11, 12]]).T,
frame_A=world_frame).T
self.assertTupleEqual(p_AQi.shape, (2, 3))
self.check_old_spelling_exists(
tree.CalcFrameGeometricJacobianExpressedInWorld)
Jv_WL = plant.CalcFrameGeometricJacobianExpressedInWorld(
context=context, frame_B=base_frame, p_BoFo_B=[0, 0, 0])
self.assertTupleEqual(Jv_WL.shape, (6, plant.num_velocities()))
nq = plant.num_positions()
nv = plant.num_velocities()
wrt_list = [
(JacobianWrtVariable.kQDot, nq),
(JacobianWrtVariable.kV, nv),
]
for wrt, nw in wrt_list:
Jw_ABp_E = plant.CalcJacobianSpatialVelocity(context=context,
with_respect_to=wrt,
frame_B=base_frame,
p_BP=np.zeros(3),
frame_A=world_frame,
frame_E=world_frame)
self.assert_sane(Jw_ABp_E)
self.assertEqual(Jw_ABp_E.shape, (6, nw))
# Compute body pose.
self.check_old_spelling_exists(tree.EvalBodyPoseInWorld)
X_WBase = plant.EvalBodyPoseInWorld(context, base)
self.assertIsInstance(X_WBase, Isometry3)
# All body poses.
X_WB_list = tree.CalcAllBodyPosesInWorld(context)
self.assertEqual(len(X_WB_list), 4)
for X_WB in X_WB_list:
self.assertIsInstance(X_WB, Isometry3)
# Compute body velocities.
v_WB_list = tree.CalcAllBodySpatialVelocitiesInWorld(context)
self.assertEqual(len(v_WB_list), 4)
for v_WB in v_WB_list:
self.assertIsInstance(v_WB, SpatialVelocity)
# - Sanity check.
v_WBase_flat = np.hstack(
(v_WB_list[0].rotational(), v_WB_list[0].translational()))
self.assertTrue(np.allclose(v_WBase_flat, np.zeros(6)))
# Set pose for the base.
X_WB_desired = Isometry3.Identity()
X_WB = plant.CalcRelativeTransform(context, world_frame, base_frame)
self.check_old_spelling_exists(tree.SetFreeBodyPoseOrThrow)
plant.SetFreeBodyPose(context=context, body=base, X_WB=X_WB_desired)
self.assertTrue(np.allclose(X_WB.matrix(), X_WB_desired.matrix()))
# Set a spatial velocity for the base.
v_WB = SpatialVelocity(w=[1, 2, 3], v=[4, 5, 6])
self.check_old_spelling_exists(tree.SetFreeBodySpatialVelocityOrThrow)
plant.SetFreeBodySpatialVelocity(context=context, body=base, V_WB=v_WB)
self.check_old_spelling_exists(tree.EvalBodySpatialVelocityInWorld)
v_base = plant.EvalBodySpatialVelocityInWorld(context, base)
self.assertTrue(np.allclose(v_base.rotational(), v_WB.rotational()))
self.assertTrue(
np.allclose(v_base.translational(), v_WB.translational()))
# Compute accelerations.
vdot = np.zeros(nv)
A_WB_array = plant.CalcSpatialAccelerationsFromVdot(context=context,
known_vdot=vdot)
self.assertEqual(len(A_WB_array), plant.num_bodies())
from pydrake.multibody.multibody_tree.multibody_plant import MultibodyPlant
from pydrake.multibody.multibody_tree.parsing import AddModelFromSdfFile
from pydrake.systems.drawing import plot_system_graphviz
from pydrake.systems.framework import DiagramBuilder
import argparse
parser = argparse.ArgumentParser()
parser.add_argument("--test",
action='store_true',
help="Causes the script to run without blocking for "
"user input.",
default=False)
args = parser.parse_args()
file_name = FindResourceOrThrow(
"drake/examples/multibody/cart_pole/cart_pole.sdf")
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
cart_pole = builder.AddSystem(MultibodyPlant())
AddModelFromSdfFile(file_name=file_name,
plant=cart_pole,
scene_graph=scene_graph)
cart_pole.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
cart_pole.Finalize(scene_graph)
assert cart_pole.geometry_source_is_registered()
builder.Connect(scene_graph.get_query_output_port(),
cart_pole.get_geometry_query_input_port())
builder.Connect(cart_pole.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(cart_pole.get_source_id()))
builder.ExportInput(cart_pole.get_actuation_input_port())
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 = Isometry3.Identity()
X_EeGripper.set_translation([0, 0, 0.081])
X_EeGripper.set_rotation(
RollPitchYaw(np.pi / 2, 0, np.pi / 2).ToRotationMatrix().matrix())
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.zeros(7)
v_iiwa_desired = np.zeros(7)
q_gripper_desired = np.zeros(2)
v_gripper_desired = np.zeros(2)
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_plant_desired = np.zeros(nq + nv)
x_plant_desired[0:7] = q_iiwa_desired
x_plant_desired[7:9] = q_gripper_desired
x_plant_desired[nq:nq + 7] = v_iiwa_desired
x_plant_desired[nq + 7:nq + nv] = v_gripper_desired
x_plant = plant.GetMutablePositionsAndVelocities(context)
x_plant[:] = x_plant_desired
q_plant = plant.GetPositions(context)
v_plant = plant.GetVelocities(context)
# Get state from context.
x = plant.GetPositionsAndVelocities(context)
x_plant_tmp = plant.GetMutablePositionsAndVelocities(context)
self.assertTrue(np.allclose(x_plant_desired, x_plant_tmp))
# Get positions and velocities of specific model instances
# from the postion/velocity vector of the plant.
tree = plant.tree()
self.check_old_spelling_exists(tree.GetPositionsFromArray)
q_iiwa = plant.GetPositions(context, iiwa_model)
q_iiwa_array = plant.GetPositionsFromArray(iiwa_model, q_plant)
self.assertTrue(np.allclose(q_iiwa, q_iiwa_array))
q_gripper = plant.GetPositions(context, gripper_model)
self.check_old_spelling_exists(tree.GetVelocitiesFromArray)
v_iiwa = plant.GetVelocities(context, iiwa_model)
v_iiwa_array = plant.GetVelocitiesFromArray(iiwa_model, v_plant)
self.assertTrue(np.allclose(v_iiwa, v_iiwa_array))
v_gripper = plant.GetVelocities(context, 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(iiwa_model, np.zeros(nq_iiwa), q_plant)
self.assertTrue(
np.allclose(plant.GetPositionsFromArray(iiwa_model, q_plant),
np.zeros(nq_iiwa)))
plant.SetVelocitiesInArray(iiwa_model, np.zeros(nv_iiwa), v_plant)
self.assertTrue(
np.allclose(plant.GetVelocitiesFromArray(iiwa_model, v_plant),
np.zeros(nv_iiwa)))
# Check actuation.
nu = plant.num_actuated_dofs()
u_plant = np.zeros(nu)
u_iiwa = np.arange(nv_iiwa)
plant.SetActuationInArray(iiwa_model, u_iiwa, u_plant)
self.assertTrue(np.allclose(u_plant[:7], u_iiwa))
gripper_setpoint_list = [0.03, 0.0, 0.0]
return plan_list, gripper_setpoint_list
if __name__ == '__main__':
# define command line arguments
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--hardware",
action='store_true',
help="Use the ManipulationStationHardwareInterface instead of an "
"in-process simulation.")
args = parser.parse_args()
is_hardware = args.hardware
object_file_path = FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf")
manip_station_sim = ManipulationStationSimulator(
time_step=2e-3,
object_file_path=object_file_path,
object_base_link_name="base_link",
is_hardware=is_hardware)
# Generate plans.
q0 = [0, 0, 0, -1.75, 0, 1.0, 0]
plan_list, gripper_setpoint_list = \
GenerateIiwaPlansAndGripperSetPoints(manip_station_sim, q0)
# Add the position/impedance plan that opens the left door.
plan_list.append(
OpenLeftDoorPositionPlan(angle_start=theta0_hinge,
def test_inverse_dynamics_controller(self):
sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"iiwa_description/sdf/iiwa14_no_collision.sdf")
plant = MultibodyPlant(time_step=0.01)
Parser(plant).AddModelFromFile(sdf_path)
plant.WeldFrames(plant.world_frame(),
plant.GetFrameByName("iiwa_link_0"))
plant.mutable_gravity_field().set_gravity_vector([0.0, 0.0, 0.0])
plant.Finalize()
# We verify the (known) size of the model.
kNumPositions = 7
kNumVelocities = 7
kNumActuators = 7
kStateSize = kNumPositions + kNumVelocities
self.assertEqual(plant.num_positions(), kNumPositions)
self.assertEqual(plant.num_velocities(), kNumVelocities)
self.assertEqual(plant.num_actuators(), kNumActuators)
kp = np.array([1., 2., 3., 4., 5., 6., 7.])
ki = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7])
kd = np.array([.5, 1., 1.5, 2., 2.5, 3., 3.5])
controller = InverseDynamicsController(robot=plant,
kp=kp,
ki=ki,
kd=kd,
has_reference_acceleration=True)
context = controller.CreateDefaultContext()
output = controller.AllocateOutput()
estimated_state_port = 0
desired_state_port = 1
desired_acceleration_port = 2
control_port = 0
self.assertEqual(
controller.get_input_port(desired_acceleration_port).size(),
kNumVelocities)
self.assertEqual(
controller.get_input_port(estimated_state_port).size(), kStateSize)
self.assertEqual(
controller.get_input_port(desired_state_port).size(), kStateSize)
self.assertEqual(
controller.get_output_port(control_port).size(), kNumVelocities)
# Current state.
q = np.array([-0.3, -0.2, -0.1, 0.0, 0.1, 0.2, 0.3])
v = np.array([-0.9, -0.6, -0.3, 0.0, 0.3, 0.6, 0.9])
x = np.concatenate([q, v])
# Reference state and acceleration.
q_r = q + 0.1 * np.ones_like(q)
v_r = v + 0.1 * np.ones_like(v)
x_r = np.concatenate([q_r, v_r])
vd_r = np.array([1., 2., 3., 4., 5., 6., 7.])
integral_term = np.array([-1., -2., -3., -4., -5., -6., -7.])
vd_d = vd_r + kp * (q_r - q) + kd * (v_r - v) + ki * integral_term
context.FixInputPort(estimated_state_port, BasicVector(x))
context.FixInputPort(desired_state_port, BasicVector(x_r))
context.FixInputPort(desired_acceleration_port, BasicVector(vd_r))
controller.set_integral_value(context, integral_term)
# Set the plant's context.
plant_context = plant.CreateDefaultContext()
x_plant = plant.GetMutablePositionsAndVelocities(plant_context)
x_plant[:] = x
# Compute the expected value of the generalized forces using
# inverse dynamics.
tau_id = plant.CalcInverseDynamics(plant_context, vd_d,
MultibodyForces(plant))
# Verify the result.
controller.CalcOutput(context, output)
self.assertTrue(
np.allclose(output.get_vector_data(0).CopyToVector(), tau_id))
def test_multibody_state_access(self, T):
MultibodyPlant = MultibodyPlant_[T]
plant_f = MultibodyPlant_[float]()
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
# N.B. `Parser` only supports `MultibodyPlant_[float]`.
Parser(plant_f).AddModelFromFile(file_name)
plant_f.Finalize()
plant = to_type(plant_f, T)
context = plant.CreateDefaultContext()
nq = 2
nv = 2
self.assertEqual(plant.num_positions(), nq)
self.assertEqual(plant.num_velocities(), nv)
q0 = np.array([3.14, 2.])
v0 = np.array([-0.5, 1.])
x0 = np.concatenate([q0, v0])
# The default state is all values set to zero.
x = plant.GetPositionsAndVelocities(context)
numpy_compare.assert_float_equal(x, np.zeros(4))
# WARNING: The following oddities occur from the fact that
# `ndarray[object]` cannot be referenced (#8116). Be careful when
# writing scalar-generic code.
if T == float:
# Can reference matrices. Use `x_ref`.
# Write into a mutable reference to the state vector.
x_ref = plant.GetMutablePositionsAndVelocities(context)
x_ref[:] = x0
def set_zero():
x_ref.fill(0)
else:
# Cannot reference matrices. Use setters.
plant.SetPositionsAndVelocities(context, x0)
def set_zero():
plant.SetPositionsAndVelocities(
context, np.zeros(nq + nv))
# Verify that positions and velocities were set correctly.
numpy_compare.assert_float_equal(plant.GetPositions(context), q0)
numpy_compare.assert_float_equal(plant.GetVelocities(context), v0)
# Verify we did modify the state stored in context.
x = plant.GetPositionsAndVelocities(context)
numpy_compare.assert_float_equal(x, x0)
# Now set positions and velocities independently and check them.
zeros_2 = np.zeros([2, ])
set_zero()
plant.SetPositions(context, q0)
numpy_compare.assert_float_equal(plant.GetPositions(context), q0)
numpy_compare.assert_float_equal(plant.GetVelocities(context), zeros_2)
set_zero()
plant.SetVelocities(context, v0)
numpy_compare.assert_float_allclose(
plant.GetPositions(context), zeros_2)
numpy_compare.assert_float_allclose(plant.GetVelocities(context), v0)
# Now test SetPositionsAndVelocities().
set_zero()
plant.SetPositionsAndVelocities(context, x0)
numpy_compare.assert_float_allclose(
plant.GetPositionsAndVelocities(context), x0)
# Test existence of context resetting methods.
plant.SetDefaultState(context, state=context.get_mutable_state())
# Test existence of limits.
self.assertEqual(plant.GetPositionLowerLimits().shape, (nq,))
self.assertEqual(plant.GetPositionUpperLimits().shape, (nq,))
self.assertEqual(plant.GetVelocityLowerLimits().shape, (nv,))
self.assertEqual(plant.GetVelocityUpperLimits().shape, (nv,))
self.assertEqual(plant.GetAccelerationLowerLimits().shape, (nv,))
self.assertEqual(plant.GetAccelerationUpperLimits().shape, (nv,))
