def test_model_instance_port_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')
plant.Finalize()
# Test that we can get an actuation input port and a continuous state
# output port.
self.assertIsInstance(plant.get_actuation_input_port(iiwa_model),
InputPort)
self.assertIsInstance(
plant.get_continuous_state_output_port(gripper_model), OutputPort)
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 test_multibody_dynamics(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
context = plant.CreateDefaultContext()
H = plant.CalcMassMatrixViaInverseDynamics(context)
Cv = plant.CalcBiasTerm(context)
self.assertTrue(H.shape == (2, 2))
self.assert_sane(H)
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.
self.assertEqual(plant.CalcPotentialEnergy(context), 0)
plant.CalcConservativePower(context)
tau_g = plant.CalcGravityGeneralizedForces(context)
self.assertEqual(tau_g.shape, (nv,))
self.assert_sane(tau_g, nonzero=False)
forces = MultibodyForces(plant=plant)
plant.CalcForceElementsContribution(context=context, forces=forces)
def test_joint_slider(self):
# Simply test to make sure that the UI loads and produces the output.
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
slider = JointSliders(robot=plant,
lower_limit=-5.,
upper_limit=5.,
resolution=0.001,
update_period_sec=0.01,
title='test',
length=300)
slider.window.withdraw() # Don't open a window during testing.
context = slider.CreateDefaultContext()
output = slider.AllocateOutput()
q = [.1, .2]
slider.set_position(q)
slider.set_joint_position(q)
slider.CalcOutput(context, output)
np.testing.assert_array_equal(output.get_vector_data(0).get_value(), q)
def test_multibody_state_access(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
AddModelFromSdfFile(file_name, plant)
plant.Finalize()
context = plant.CreateDefaultContext()
tree = plant.tree()
self.assertEqual(plant.num_positions(), 2)
self.assertEqual(plant.num_velocities(), 2)
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 = tree.get_multibody_state_vector(context)
self.assertTrue(np.allclose(x, np.zeros(4)))
# Write into a mutable reference to the state vector.
x_reff = tree.get_mutable_multibody_state_vector(context)
x_reff[:] = x0
# Verify we did modify the state stored in context.
x = tree.get_multibody_state_vector(context)
self.assertTrue(np.allclose(x, x0))
def test_model_instance_port_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')
plant.Finalize()
# Test that we can get an actuation input port and a continuous state
# output port.
self.assertIsInstance(
plant.get_actuation_input_port(iiwa_model), InputPort)
self.assertIsInstance(
plant.get_continuous_state_output_port(gripper_model), OutputPort)
def test_multibody_plant_parsing(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant(time_step=0.01)
model_instance = AddModelFromSdfFile(file_name=file_name, plant=plant)
self.assertIsInstance(model_instance, ModelInstanceIndex)
plant = MultibodyPlant(time_step=0.01)
model_instance = AddModelFromSdfFile(file_name=file_name,
model_name="acrobot",
plant=plant)
def load_tables(time_step=0.0):
mbp = MultibodyPlant(time_step=time_step)
scene_graph = SceneGraph()
# TODO: meshes aren't supported during collision checking
robot = AddModelFromSdfFile(file_name=IIWA14_SDF_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
table = AddModelFromSdfFile(file_name=TABLE_SDF_PATH, model_name='table',
scene_graph=scene_graph, plant=mbp)
table2 = AddModelFromSdfFile(file_name=TABLE_SDF_PATH, model_name='table2',
scene_graph=scene_graph, plant=mbp)
sink = AddModelFromSdfFile(file_name=SINK_PATH, model_name='sink',
scene_graph=scene_graph, plant=mbp)
stove = AddModelFromSdfFile(file_name=STOVE_PATH, model_name='stove',
scene_graph=scene_graph, plant=mbp)
broccoli = AddModelFromSdfFile(file_name=BROCCOLI_PATH, model_name='broccoli',
scene_graph=scene_graph, plant=mbp)
#wall = AddModelFromSdfFile(file_name=WALL_PATH, model_name='wall',
# scene_graph=scene_graph, plant=mbp)
wall = None
table2_x = 0.75
table_top_z = get_aabb_z_placement(AABBs['sink'], AABBs['table']) # TODO: use geometry
weld_gripper(mbp, robot, gripper)
weld_to_world(mbp, robot, create_transform(translation=[0, 0, table_top_z]))
weld_to_world(mbp, table, create_transform())
weld_to_world(mbp, table2, create_transform(translation=[table2_x, 0, 0]))
weld_to_world(mbp, sink, create_transform(translation=[table2_x, 0.25, table_top_z]))
weld_to_world(mbp, stove, create_transform(translation=[table2_x, -0.25, table_top_z]))
if wall is not None:
weld_to_world(mbp, wall, create_transform(translation=[table2_x / 2, 0, table_top_z]))
mbp.Finalize(scene_graph)
movable = [broccoli]
surfaces = [
VisualElement(sink, 'base_link', 0), # Could also just pass the link index
VisualElement(stove, 'base_link', 0),
]
initial_poses = {
broccoli: create_transform(translation=[table2_x, 0, table_top_z]),
}
task = Task(mbp, scene_graph, robot, gripper,
movable=movable, surfaces=surfaces,
initial_poses=initial_poses,
goal_cooked=[broccoli])
return mbp, scene_graph, task
def test_multibody_dynamics(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
AddModelFromSdfFile(file_name, plant)
plant.Finalize()
context = plant.CreateDefaultContext()
tree = plant.tree()
H = tree.CalcMassMatrixViaInverseDynamics(context)
Cv = tree.CalcBiasTerm(context)
self.assertTrue(H.shape == (2, 2))
self.assertTrue(Cv.shape == (2, ))
def test_multibody_state_access(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
context = plant.CreateDefaultContext()
self.assertEqual(plant.num_positions(), 2)
self.assertEqual(plant.num_velocities(), 2)
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)
self.assertTrue(np.allclose(x, np.zeros(4)))
# Write into a mutable reference to the state vector.
x_ref = plant.GetMutablePositionsAndVelocities(context)
x_ref[:] = x0
# Verify that positions and velocities were set correctly.
self.assertTrue(np.allclose(plant.GetPositions(context), q0))
self.assertTrue(np.allclose(plant.GetVelocities(context), v0))
# Verify we did modify the state stored in context.
x = plant.GetPositionsAndVelocities(context)
self.assertTrue(np.allclose(x, x0))
# Now set positions and velocities independently and check them.
zeros_2 = np.zeros([2, 1])
x_ref.fill(0)
plant.SetPositions(context, q0)
self.assertTrue(np.allclose(plant.GetPositions(context), q0))
self.assertTrue(np.allclose(plant.GetVelocities(context), zeros_2))
x_ref.fill(0)
plant.SetVelocities(context, v0)
self.assertTrue(np.allclose(plant.GetPositions(context), zeros_2))
self.assertTrue(np.allclose(plant.GetVelocities(context), v0))
# Now test SetPositionsAndVelocities().
x_ref.fill(0)
plant.SetPositionsAndVelocities(context, x0)
self.assertTrue(np.allclose(
plant.GetPositionsAndVelocities(context), x0))
# Test existence of context resetting methods.
plant.SetDefaultState(context, state=context.get_mutable_state())
def test_multibody_dynamics(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
AddModelFromSdfFile(file_name, plant)
plant.Finalize()
context = plant.CreateDefaultContext()
tree = plant.tree()
H = tree.CalcMassMatrixViaInverseDynamics(context)
Cv = tree.CalcBiasTerm(context)
self.assertTrue(H.shape == (2, 2))
self.assertTrue(Cv.shape == (2, ))
def test_multibody_tree_kinematics(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
AddModelFromSdfFile(file_name, plant)
plant.Finalize()
context = plant.CreateDefaultContext()
tree = plant.model()
world_frame = plant.world_frame()
# TODO(eric.cousineau): Replace this with `GetFrameByName`.
link1_frame = plant.GetBodyByName("Link1").body_frame()
X_WL = tree.CalcRelativeTransform(context,
frame_A=world_frame,
frame_B=link1_frame)
self.assertIsInstance(X_WL, Isometry3)
p_AQi = tree.CalcPointsPositions(context=context,
frame_B=link1_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=link1_frame, p_BoFo_B=[0, 0, 0])
self.assertTupleEqual(Jv_WL.shape, (6, plant.num_velocities()))
def test_set_free_body_pose(self):
file_name = FindResourceOrThrow(
"drake/examples/double_pendulum/models/double_pendulum.sdf")
plant = MultibodyPlant()
plant_model = AddModelFromSdfFile(file_name, plant)
plant.Finalize()
context = plant.CreateDefaultContext()
tree = plant.tree()
X_WB_desired = Isometry3.Identity()
R_WB = np.array([[0., 1., 0.],
[0., 0., 1.],
[1., 0., 0.]])
X_WB_desired.set_rotation(R_WB)
tree.SetFreeBodyPoseOrThrow(
body=plant.GetBodyByName("base", plant_model),
X_WB=X_WB_desired, context=context)
world_frame = plant.world_frame()
base_frame = plant.GetBodyByName("base").body_frame()
X_WB = tree.CalcRelativeTransform(
context, frame_A=world_frame, frame_B=base_frame)
self.assertTrue(np.allclose(X_WB.matrix(), X_WB_desired.matrix()))
def test_multibody_plant_inverse_dynamics(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.Finalize()
# Just test that the constructor doesn't throw.
controller = InverseDynamics(
plant=plant,
mode=InverseDynamics.InverseDynamicsMode.kGravityCompensation)
def RunSimulation(self, real_time_rate=1.0):
'''
The Princess Diaries was a good movie.
'''
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
# object_file_path = FindResourceOrThrow(
# "drake/examples/manipulation_station/models/061_foam_brick.sdf")
# sdf_file = FindResourceOrThrow("drake/multibody/benchmarks/acrobot/acrobot.sdf")
# urdf_file = FindResourceOrThrow("drake/multibody/benchmarks/acrobot/acrobot.urdf")
sdf_file = "assets/acrobot.sdf"
urdf_file = "assets/acrobot.urdf"
plant = builder.AddSystem(MultibodyPlant())
plant.RegisterAsSourceForSceneGraph(scene_graph)
Parser(plant, scene_graph).AddModelFromFile(sdf_file)
plant.Finalize(scene_graph)
assert plant.geometry_source_is_registered()
builder.Connect(
plant.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
builder.Connect(scene_graph.get_query_output_port(),
plant.get_geometry_query_input_port())
# Add
nn_system = NNSystem(self.pytorch_nn_object)
builder.AddSystem(nn_system)
# NN -> plant
builder.Connect(nn_system.NN_out_output_port,
plant.get_actuation_input_port())
# plant -> NN
builder.Connect(plant.get_continuous_state_output_port(),
nn_system.NN_in_input_port)
# Add meshcat visualizer
meshcat = MeshcatVisualizer(scene_graph)
builder.AddSystem(meshcat)
# builder.Connect(scene_graph.GetOutputPort("lcm_visualization"),
builder.Connect(scene_graph.get_pose_bundle_output_port(),
meshcat.GetInputPort("lcm_visualization"))
# build diagram
diagram = builder.Build()
meshcat.load()
# time.sleep(2.0)
RenderSystemWithGraphviz(diagram)
# construct simulator
simulator = Simulator(diagram)
# context = diagram.GetMutableSubsystemContext(
# self.station, simulator.get_mutable_context())
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(real_time_rate)
simulator.Initialize()
sim_duration = 5.
simulator.StepTo(sim_duration)
print("stepping complete")
def test_kuka(self):
"""Kuka IIWA with mesh geometry."""
file_name = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/sdf/"
"iiwa14_no_collision.sdf")
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
kuka = builder.AddSystem(MultibodyPlant())
parser = Parser(plant=kuka, scene_graph=scene_graph)
parser.AddModelFromFile(file_name)
kuka.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
kuka.Finalize(scene_graph)
assert kuka.geometry_source_is_registered()
builder.Connect(kuka.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(kuka.get_source_id()))
visualizer = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=None, open_browser=False))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
kuka_context = diagram.GetMutableSubsystemContext(
kuka, diagram_context)
kuka_context.FixInputPort(
kuka.get_actuation_input_port().get_index(),
np.zeros(kuka.get_actuation_input_port().size()))
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.StepTo(.1)
def test_parser(self):
# Calls every combination of argments for the Parser methods and
# inspects their return type.
sdf_file = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
urdf_file = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.urdf")
for dut, file_name, model_name, result_dim in (
(Parser.AddModelFromFile, sdf_file, None, int),
(Parser.AddModelFromFile, sdf_file, "", int),
(Parser.AddModelFromFile, sdf_file, "a", int),
(Parser.AddModelFromFile, urdf_file, None, int),
(Parser.AddModelFromFile, urdf_file, "", int),
(Parser.AddModelFromFile, urdf_file, "a", int),
(Parser.AddAllModelsFromFile, sdf_file, None, list),
(Parser.AddAllModelsFromFile, urdf_file, None, list),
):
plant = MultibodyPlant(time_step=0.01)
parser = Parser(plant=plant)
if model_name is None:
result = dut(parser, file_name=file_name)
else:
result = dut(parser, file_name=file_name,
model_name=model_name)
if result_dim is int:
self.assertIsInstance(result, ModelInstanceIndex)
else:
assert result_dim is list
self.assertIsInstance(result, list)
self.assertIsInstance(result[0], ModelInstanceIndex)
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--target_realtime_rate",
type=float,
default=1.0,
help="Desired rate relative to real time. See documentation for "
"Simulator::set_target_realtime_rate() for details.")
parser.add_argument("--simulation_time",
type=float,
default=10.0,
help="Desired duration of the simulation in seconds.")
parser.add_argument(
"--time_step",
type=float,
default=0.,
help="If greater than zero, the plant is modeled as a system with "
"discrete updates and period equal to this time_step. "
"If 0, the plant is modeled as a continuous system.")
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(time_step=args.time_step))
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(
cart_pole.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(cart_pole.get_source_id()))
lcm = DrakeLcm()
ConnectVisualization(scene_graph=scene_graph, builder=builder, lcm=lcm)
diagram = builder.Build()
DispatchLoadMessage(scene_graph=scene_graph, lcm=lcm)
diagram_context = diagram.CreateDefaultContext()
cart_pole_context = diagram.GetMutableSubsystemContext(
cart_pole, diagram_context)
cart_pole_context.FixInputPort(
cart_pole.get_actuation_input_port().get_index(), [0])
cart_slider = cart_pole.GetJointByName("CartSlider")
pole_pin = cart_pole.GetJointByName("PolePin")
cart_slider.set_translation(context=cart_pole_context, translation=0.)
pole_pin.set_angle(context=cart_pole_context, angle=2.)
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(args.target_realtime_rate)
simulator.Initialize()
simulator.StepTo(args.simulation_time)
def test_mbp_overloads(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
AddModelFromSdfFile(file_name=file_name, plant=plant, scene_graph=None)
plant.Finalize()
context = plant.CreateDefaultContext()
frame = plant.GetFrameByName("Link2")
parameters = mut.DifferentialInverseKinematicsParameters(2, 2)
mut.DoDifferentialInverseKinematics(plant, context, np.zeros(6), frame,
parameters)
mut.DoDifferentialInverseKinematics(plant, context,
Isometry3.Identity(), frame,
parameters)
def testMultibodyPositionToGeometryPose(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant(time_step=0.01)
model_instance = AddModelFromSdfFile(file_name=file_name, plant=plant)
scene_graph = SceneGraph()
plant.RegisterAsSourceForSceneGraph(scene_graph)
plant.Finalize()
to_pose = MultibodyPositionToGeometryPose(plant)
# Check the size of the input.
self.assertEqual(to_pose.get_input_port().size(), 2)
# Just check the spelling of the output port (size is not meaningful
# for Abstract-valued ports).
to_pose.get_output_port()
def test_multibody_state_access(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
AddModelFromSdfFile(file_name, plant)
plant.Finalize()
context = plant.CreateDefaultContext()
tree = plant.tree()
self.assertEqual(plant.num_positions(), 2)
self.assertEqual(plant.num_velocities(), 2)
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 = tree.get_multibody_state_vector(context)
self.assertTrue(np.allclose(x, np.zeros(4)))
# Write into a mutable reference to the state vector.
x_reff = tree.get_mutable_multibody_state_vector(context)
x_reff[:] = x0
# Verify we did modify the state stored in context.
x = tree.get_multibody_state_vector(context)
self.assertTrue(np.allclose(x, x0))
def add_plant_and_scene_graph(builder):
# TODO(eric.cousineau): Hoist to C++.
plant = builder.AddSystem(MultibodyPlant())
scene_graph = builder.AddSystem(SceneGraph())
plant.RegisterAsSourceForSceneGraph(scene_graph)
builder.Connect(scene_graph.get_query_output_port(),
plant.get_geometry_query_input_port())
builder.Connect(plant.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
return plant, scene_graph
def setUp(self):
file_name = FindResourceOrThrow(
"drake/bindings/pydrake/multibody/test/two_bodies.sdf")
self.plant = MultibodyPlant(time_step=0.01)
model_instance = Parser(self.plant).AddModelFromFile(file_name)
self.plant.Finalize()
self.body1_frame = self.plant.GetBodyByName("body1").body_frame()
self.body2_frame = self.plant.GetBodyByName("body2").body_frame()
self.ik_two_bodies = ik.InverseKinematics(self.plant)
self.prog = self.ik_two_bodies.get_mutable_prog()
self.q = self.ik_two_bodies.q()
def squaredNorm(x):
return np.array([x[0]**2 + x[1]**2 + x[2]**2 + x[3]**2])
self.prog.AddConstraint(squaredNorm, [1], [1],
self._body1_quat(self.q))
self.prog.AddConstraint(squaredNorm, [1], [1],
self._body2_quat(self.q))
self.prog.SetInitialGuess(self._body1_quat(self.q), [1, 0, 0, 0])
self.prog.SetInitialGuess(self._body2_quat(self.q), [1, 0, 0, 0])
def test_deprecated_tree_api(self):
plant = MultibodyPlant()
plant.Finalize()
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always', DrakeDeprecationWarning)
num_expected_warnings = [0]
def expect_new_warning(msg_part):
num_expected_warnings[0] += 1
self.assertEqual(len(w), num_expected_warnings[0])
self.assertIn(msg_part, str(w[-1].message))
tree = plant.tree()
expect_new_warning("`tree()`")
MobilizerIndex(0)
expect_new_warning("`MobilizerIndex`")
BodyNodeIndex(0)
expect_new_warning("`BodyNodeIndex`")
MultibodyForces(model=tree)
expect_new_warning("`MultibodyForces(plant)`")
element = plant.world_body()
self.assertIsInstance(element.get_parent_tree(), MultibodyTree)
expect_new_warning("`get_parent_tree()`")
# Check old spellings (no deprecation warnings).
self.check_old_spelling_exists(tree.CalcRelativeTransform)
self.check_old_spelling_exists(tree.CalcPointsPositions)
self.check_old_spelling_exists(
tree.CalcFrameGeometricJacobianExpressedInWorld)
self.check_old_spelling_exists(tree.EvalBodyPoseInWorld)
self.check_old_spelling_exists(tree.SetFreeBodyPoseOrThrow)
self.check_old_spelling_exists(tree.SetFreeBodySpatialVelocityOrThrow)
self.check_old_spelling_exists(tree.EvalBodySpatialVelocityInWorld)
self.check_old_spelling_exists(tree.GetPositionsFromArray)
self.check_old_spelling_exists(tree.GetVelocitiesFromArray)
self.check_old_spelling_exists(tree.CalcMassMatrixViaInverseDynamics)
self.check_old_spelling_exists(tree.CalcBiasTerm)
self.check_old_spelling_exists(tree.CalcInverseDynamics)
self.check_old_spelling_exists(tree.num_frames)
self.check_old_spelling_exists(tree.get_body)
self.check_old_spelling_exists(tree.get_joint)
self.check_old_spelling_exists(tree.get_joint_actuator)
self.check_old_spelling_exists(tree.get_frame)
self.check_old_spelling_exists(tree.GetModelInstanceName)
context = plant.CreateDefaultContext()
# All body poses.
X_WB, = tree.CalcAllBodyPosesInWorld(context)
self.assertIsInstance(X_WB, Isometry3)
v_WB, = tree.CalcAllBodySpatialVelocitiesInWorld(context)
self.assertIsInstance(v_WB, SpatialVelocity)
def test_joint_slider(self):
# Simply test to make sure that the UI loads and produces the output.
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
AddModelFromSdfFile(file_name=file_name, plant=plant, scene_graph=None)
plant.Finalize()
slider = JointSliders(robot=plant,
lower_limit=-5.,
upper_limit=5.,
resolution=0.001,
update_period_sec=0.01,
title='test')
context = slider.CreateDefaultContext()
output = slider.AllocateOutput()
q = [.1, .2]
slider.set(q)
slider.CalcOutput(context, output)
np.testing.assert_array_equal(output.get_vector_data(0).get_value(), q)
def test_multibody_add_joint(self):
"""
Tests joint constructors and `AddJoint`.
"""
instance_file = FindResourceOrThrow(
"drake/examples/double_pendulum/models/double_pendulum.sdf")
# Add different joints between multiple model instances.
# TODO(eric.cousineau): Remove the multiple instances and use
# programmatically constructed bodies once this API is exposed in
# Python.
num_joints = 2
plant = MultibodyPlant()
parser = Parser(plant)
instances = []
for i in range(num_joints + 1):
instance = parser.AddModelFromFile(instance_file,
"instance_{}".format(i))
instances.append(instance)
proximal_frame = "base"
distal_frame = "lower_link"
joints = [
RevoluteJoint(
name="revolve_things",
frame_on_parent=plant.GetBodyByName(distal_frame,
instances[1]).body_frame(),
frame_on_child=plant.GetBodyByName(proximal_frame,
instances[2]).body_frame(),
axis=[0, 0, 1],
damping=0.),
WeldJoint(
name="weld_things",
parent_frame_P=plant.GetBodyByName(distal_frame,
instances[0]).body_frame(),
child_frame_C=plant.GetBodyByName(proximal_frame,
instances[1]).body_frame(),
X_PC=Isometry3.Identity()),
]
for joint in joints:
joint_out = plant.AddJoint(joint)
self.assertIs(joint, joint_out)
# The model is now complete.
plant.Finalize()
for joint in joints:
self._test_joint_api(joint)
def test_deprecated_parsing(self):
sdf_file = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant(time_step=0.01)
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter("default", DrakeDeprecationWarning)
result = AddModelFromSdfFile(plant=plant, file_name=sdf_file)
self.assertIsInstance(result, ModelInstanceIndex)
self.assertEqual(len(w), 1)
plant = MultibodyPlant(time_step=0.01)
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter("default", DrakeDeprecationWarning)
result = DeprecatedParser(plant).AddModelFromFile(sdf_file)
self.assertIsInstance(result, ModelInstanceIndex)
self.assertEqual(len(w), 1)
plant = MultibodyPlant(time_step=0.01)
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter("default", DrakeDeprecationWarning)
result = DeprecatedParser(plant).AddAllModelsFromFile(sdf_file)
self.assertIsInstance(result, list)
self.assertEqual(len(w), 1)
def main():
parser = argparse.ArgumentParser(
description=__doc__,
formatter_class=argparse.RawDescriptionHelpFormatter)
parser.add_argument(
"filename", type=str,
help="Path to an SDF or URDF file.")
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
filename = args.filename
if not os.path.isfile(filename):
parser.error("File does not exist: {}".format(filename))
# Construct Plant + SceneGraph.
builder = DiagramBuilder()
plant = builder.AddSystem(MultibodyPlant())
scene_graph = builder.AddSystem(SceneGraph())
plant.RegisterAsSourceForSceneGraph(scene_graph)
builder.Connect(
scene_graph.get_query_output_port(),
plant.get_geometry_query_input_port())
builder.Connect(
plant.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
# Load the model file.
Parser(plant).AddModelFromFile(filename)
plant.Finalize()
# Publish to Drake Visualizer.
drake_viz_pub = ConnectDrakeVisualizer(builder, scene_graph)
# Publish to Meshcat.
if args.meshcat is not None:
meshcat_viz = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=args.meshcat))
builder.Connect(
scene_graph.get_pose_bundle_output_port(),
meshcat_viz.get_input_port(0))
diagram = builder.Build()
context = diagram.CreateDefaultContext()
# Use Simulator to dispatch initialization events.
# TODO(eric.cousineau): Simplify as part of #10015.
Simulator(diagram).Initialize()
# Publish draw messages with current state.
diagram.Publish(context)
def cartPoleTest(self):
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(
cart_pole.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(cart_pole.get_source_id()))
visualizer = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=None))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
visualizer.load()
diagram_context = diagram.CreateDefaultContext()
cart_pole_context = diagram.GetMutableSubsystemContext(
cart_pole, diagram_context)
cart_pole_context.FixInputPort(
cart_pole.get_actuation_input_port().get_index(), [0])
cart_slider = cart_pole.GetJointByName("CartSlider")
pole_pin = cart_pole.GetJointByName("PolePin")
cart_slider.set_translation(context=cart_pole_context, translation=0.)
pole_pin.set_angle(context=cart_pole_context, angle=2.)
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(args.target_realtime_rate)
simulator.Initialize()
simulator.StepTo(args.duration)
def test_multibody_plant_inverse_dynamics(self):
sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"iiwa_description/sdf/iiwa14_no_collision.sdf")
plant = MultibodyPlant(time_step=0.01)
AddModelFromSdfFile(file_name=sdf_path, plant=plant)
plant.WeldFrames(plant.world_frame(),
plant.GetFrameByName("iiwa_link_0"))
plant.Finalize()
# Just test that the constructor doesn't throw.
controller = InverseDynamics(
plant=plant,
mode=InverseDynamics.InverseDynamicsMode.kGravityCompensation)
def kukaTest(args):
file_name = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision"
".sdf")
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
kuka = builder.AddSystem(MultibodyPlant())
AddModelFromSdfFile(
file_name=file_name, plant=kuka, scene_graph=scene_graph)
kuka.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
kuka.Finalize(scene_graph)
assert kuka.geometry_source_is_registered()
builder.Connect(
kuka.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(kuka.get_source_id()))
visualizer = builder.AddSystem(MeshcatVisualizer(scene_graph))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
visualizer.load()
diagram_context = diagram.CreateDefaultContext()
kuka_context = diagram.GetMutableSubsystemContext(
kuka, diagram_context)
kuka_context.FixInputPort(
kuka.get_actuation_input_port().get_index(), np.zeros(
kuka.get_actuation_input_port().size()))
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(args.target_realtime_rate)
simulator.Initialize()
simulator.StepTo(args.duration)
class TestInverseKinematics(unittest.TestCase):
"""
This test reflects inverse_kinematics_test.cc
"""
def setUp(self):
file_name = FindResourceOrThrow(
"drake/bindings/pydrake/multibody/test/two_bodies.sdf")
self.plant = MultibodyPlant(time_step=0.01)
model_instance = AddModelFromSdfFile(
file_name=file_name, plant=self.plant, scene_graph=None)
self.plant.Finalize()
self.body1_frame = self.plant.GetBodyByName("body1").body_frame()
self.body2_frame = self.plant.GetBodyByName("body2").body_frame()
self.ik_two_bodies = ik.InverseKinematics(self.plant)
self.prog = self.ik_two_bodies.get_mutable_prog()
self.q = self.ik_two_bodies.q()
def squaredNorm(x):
return np.array([x[0] ** 2 + x[1] ** 2 + x[2] ** 2 + x[3] ** 2])
self.prog.AddConstraint(
squaredNorm, [1], [1], self._body1_quat(self.q))
self.prog.AddConstraint(
squaredNorm, [1], [1], self._body2_quat(self.q))
self.prog.SetInitialGuess(self._body1_quat(self.q), [1, 0, 0, 0])
self.prog.SetInitialGuess(self._body2_quat(self.q), [1, 0, 0, 0])
def _body1_quat(self, q):
# TODO(eric.cousineau): Replace with state indexing.
return q[0:4]
def _body1_xyz(self, q):
# TODO(eric.cousineau): Replace with state indexing.
return q[4:7]
def _body2_quat(self, q):
# TODO(eric.cousineau): Replace with state indexing.
return q[7:11]
def _body2_xyz(self, q):
# TODO(eric.cousineau): Replace with state indexing.
return q[11:14]
def test_AddPositionConstraint(self):
p_BQ = np.array([0.2, 0.3, 0.5])
p_AQ_lower = np.array([-0.1, -0.2, -0.3])
p_AQ_upper = np.array([-0.05, -0.12, -0.28])
self.ik_two_bodies.AddPositionConstraint(
frameB=self.body1_frame, p_BQ=p_BQ,
frameA=self.body2_frame,
p_AQ_lower=p_AQ_lower, p_AQ_upper=p_AQ_upper)
result = self.prog.Solve()
self.assertEqual(result, mp.SolutionResult.kSolutionFound)
q_val = self.prog.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body1_pos = self._body1_xyz(q_val)
body2_quat = self._body2_quat(q_val)
body2_pos = self._body2_xyz(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
p_AQ = body2_rotmat.transpose().dot(
body1_rotmat.dot(p_BQ) + body1_pos - body2_pos)
self.assertTrue(np.greater(p_AQ, p_AQ_lower -
1E-6 * np.ones((3, 1))).all())
self.assertTrue(np.less(p_AQ, p_AQ_upper +
1E-6 * np.ones((3, 1))).all())
def test_AddOrientationConstraint(self):
theta_bound = 0.2 * math.pi
R_AbarA = pydrake.math.RotationMatrix(
quaternion=Quaternion(0.5, -0.5, 0.5, 0.5))
R_BbarB = pydrake.math.RotationMatrix(
quaternion=Quaternion(1.0 / 3, 2.0 / 3, 0, 2.0 / 3))
self.ik_two_bodies.AddOrientationConstraint(
frameAbar=self.body1_frame, R_AbarA=R_AbarA,
frameBbar=self.body2_frame, R_BbarB=R_BbarB,
theta_bound=theta_bound)
result = self.prog.Solve()
self.assertEqual(result, mp.SolutionResult.kSolutionFound)
q_val = self.prog.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat)
R_AB = R_AbarA.matrix().transpose().dot(
R_AbarBbar.dot(R_BbarB.matrix()))
self.assertGreater(R_AB.trace(), 1 + 2 * math.cos(theta_bound) - 1E-6)
def test_AddGazeTargetConstraint(self):
p_AS = np.array([0.1, 0.2, 0.3])
n_A = np.array([0.3, 0.5, 1.2])
p_BT = np.array([1.1, 0.2, 1.5])
cone_half_angle = 0.2 * math.pi
self.ik_two_bodies.AddGazeTargetConstraint(
frameA=self.body1_frame, p_AS=p_AS, n_A=n_A,
frameB=self.body2_frame, p_BT=p_BT,
cone_half_angle=cone_half_angle)
result = self.prog.Solve()
self.assertEqual(result, mp.SolutionResult.kSolutionFound)
q_val = self.prog.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body1_pos = self._body1_xyz(q_val)
body2_quat = self._body2_quat(q_val)
body2_pos = self._body2_xyz(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
p_WS = body1_pos + body1_rotmat.dot(p_AS)
p_WT = body2_pos + body2_rotmat.dot(p_BT)
p_ST_W = p_WT - p_WS
n_W = body1_rotmat.dot(n_A)
self.assertGreater(p_ST_W.dot(n_W), np.linalg.norm(
p_ST_W) * np.linalg.norm(n_W) * math.cos(cone_half_angle) - 1E-6)
def test_AddAngleBetweenVectorsConstraint(self):
na_A = np.array([0.2, -0.4, 0.9])
nb_B = np.array([1.4, -0.1, 1.8])
angle_lower = 0.2 * math.pi
angle_upper = 0.2 * math.pi
self.ik_two_bodies.AddAngleBetweenVectorsConstraint(
frameA=self.body1_frame, na_A=na_A,
frameB=self.body2_frame, nb_B=nb_B,
angle_lower=angle_lower, angle_upper=angle_upper)
result = self.prog.Solve()
self.assertEqual(result, mp.SolutionResult.kSolutionFound)
q_val = self.prog.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
na_W = body1_rotmat.dot(na_A)
nb_W = body2_rotmat.dot(nb_B)
angle = math.acos(na_W.transpose().dot(nb_W) /
(np.linalg.norm(na_W) * np.linalg.norm(nb_W)))
self.assertLess(math.fabs(angle - angle_lower), 1E-6)
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)
self.assertIsInstance(plant.num_frames(), int)
self.assertIsInstance(plant.get_body(body_index=BodyIndex(0)), Body)
self.assertIs(shoulder, plant.get_joint(joint_index=JointIndex(0)))
self.assertIsInstance(plant.get_joint_actuator(
actuator_index=JointActuatorIndex(0)), JointActuator)
self.assertIsInstance(
plant.get_frame(frame_index=FrameIndex(0)), Frame)
self.assertEqual("acrobot", plant.GetModelInstanceName(
model_instance=model_instance))
def test_multibody_plant_inverse_dynamics_controller(self):
sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"iiwa_description/sdf/iiwa14_no_collision.sdf")
plant = MultibodyPlant(time_step=0.01)
AddModelFromSdfFile(file_name=sdf_path, plant=plant)
plant.WeldFrames(plant.world_frame(),
plant.GetFrameByName("iiwa_link_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.tree().get_mutable_multibody_state_vector(
plant_context)
x_plant[:] = x
# Compute the expected value of the generalized forces using
# inverse dynamics.
tau_id = plant.tree().CalcInverseDynamics(
plant_context, vd_d, MultibodyForces(plant.tree()))
# Verify the result.
controller.CalcOutput(context, output)
self.assertTrue(np.allclose(output.get_vector_data(0).CopyToVector(),
tau_id))
"default value.",
default=[])
parser.add_argument(
"--test",
action='store_true',
help="Disable opening the gui window for testing."
)
# TODO(russt): Add option to weld the base to the world pending the
# availability of GetUniqueBaseBody requested in #9747.
args = parser.parse_args()
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
# Construct a MultibodyPlant and load the SDF into it.
plant = MultibodyPlant()
plant.RegisterAsSourceForSceneGraph(scene_graph)
AddModelFromSdfFile(args.filename, plant)
plant.Finalize(scene_graph)
# Add sliders to set positions of the joints.
sliders = builder.AddSystem(JointSliders(robot=plant))
to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(plant))
builder.Connect(sliders.get_output_port(0), to_pose.get_input_port())
builder.Connect(to_pose.get_output_port(), scene_graph.get_source_pose_port(
plant.get_source_id()))
# Connect this to drake_visualizer.
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
if len(args.position):
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()
world_frame = plant.world_frame()
base = plant.GetBodyByName("base")
base_frame = plant.GetFrameByName("base")
X_WL = plant.CalcRelativeTransform(
context, frame_A=world_frame, frame_B=base_frame)
self.assertIsInstance(X_WL, Isometry3)
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))
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.
X_WBase = plant.EvalBodyPoseInWorld(context, base)
self.assertIsInstance(X_WBase, Isometry3)
# Set pose for the base.
X_WB_desired = Isometry3.Identity()
X_WB = plant.CalcRelativeTransform(context, world_frame, base_frame)
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])
plant.SetFreeBodySpatialVelocity(
context=context, body=base, V_WB=v_WB)
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())
def test_multibody_dynamics(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
context = plant.CreateDefaultContext()
H = plant.CalcMassMatrixViaInverseDynamics(context)
Cv = plant.CalcBiasTerm(context)
self.assertTrue(H.shape == (2, 2))
self.assert_sane(H)
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.
self.assertEqual(plant.CalcPotentialEnergy(context), 0)
plant.CalcConservativePower(context)
tau_g = plant.CalcGravityGeneralizedForces(context)
self.assertEqual(tau_g.shape, (nv, ))
self.assert_sane(tau_g, nonzero=False)
forces = MultibodyForces(plant=plant)
plant.CalcForceElementsContribution(context=context, forces=forces)
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_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.
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)
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))
def main():
args_parser = argparse.ArgumentParser(
description=__doc__,
formatter_class=argparse.RawDescriptionHelpFormatter)
args_parser.add_argument(
"filename", type=str,
help="Path to an SDF or URDF file.")
args_parser.add_argument(
"--package_path",
type=str,
default=None,
help="Full path to the root package for reading in SDF resources.")
position_group = args_parser.add_mutually_exclusive_group()
position_group.add_argument(
"--position", type=float, nargs="+", default=[],
help="A list of positions which must be the same length as the number "
"of positions in the sdf model. Note that most models have a "
"floating-base joint by default (unless the sdf explicitly welds "
"the base to the world, and so have 7 positions corresponding to "
"the quaternion representation of that floating-base position.")
position_group.add_argument(
"--joint_position", type=float, nargs="+", default=[],
help="A list of positions which must be the same length as the number "
"of positions ASSOCIATED WITH JOINTS in the sdf model. This "
"does not include, e.g., floating-base coordinates, which will "
"be assigned a default value.")
args_parser.add_argument(
"--test", action='store_true',
help="Disable opening the gui window for testing.")
# TODO(russt): Add option to weld the base to the world pending the
# availability of GetUniqueBaseBody requested in #9747.
args = args_parser.parse_args()
filename = args.filename
if not os.path.isfile(filename):
args_parser.error("File does not exist: {}".format(filename))
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
# Construct a MultibodyPlant.
plant = MultibodyPlant()
plant.RegisterAsSourceForSceneGraph(scene_graph)
# Create the parser.
parser = Parser(plant)
# Get the package pathname.
if args.package_path:
# Verify that package.xml is found in the designated path.
package_path = os.path.abspath(args.package_path)
if not os.path.isfile(os.path.join(package_path, "package.xml")):
parser.error("package.xml not found at: {}".format(package_path))
# Get the package map and populate it using the package path.
package_map = parser.package_map()
package_map.PopulateFromFolder(package_path)
# Add the model from the file and finalize the plant.
parser.AddModelFromFile(filename)
plant.Finalize(scene_graph)
# Add sliders to set positions of the joints.
sliders = builder.AddSystem(JointSliders(robot=plant))
to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(plant))
builder.Connect(sliders.get_output_port(0), to_pose.get_input_port())
builder.Connect(
to_pose.get_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
# Connect this to drake_visualizer.
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
if len(args.position):
sliders.set_position(args.position)
elif len(args.joint_position):
sliders.set_joint_position(args.joint_position)
# Make the diagram and run it.
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_publish_every_time_step(False)
if args.test:
sliders.window.withdraw()
simulator.StepTo(0.1)
else:
simulator.set_target_realtime_rate(1.0)
simulator.StepTo(np.inf)
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 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()
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)))
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())
cart_pole.RegisterAsSourceForSceneGraph(scene_graph)
AddModelFromSdfFile(file_name=file_name, plant=cart_pole)
cart_pole.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
cart_pole.Finalize()
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())
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
diagram = builder.Build()
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))
