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(time_step=2e-3)
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_state_output_port(gripper_model),
OutputPort)
self.assertIsInstance(
plant.get_generalized_contact_forces_output_port(
model_instance=gripper_model), OutputPort)
def test_scene_graph_queries(self):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
parser = Parser(plant=plant, scene_graph=scene_graph)
parser.AddModelFromFile(
FindResourceOrThrow(
"drake/bindings/pydrake/multibody/test/two_bodies.sdf"))
plant.Finalize(scene_graph)
diagram = builder.Build()
# The model `two_bodies` has two (implicitly) floating bodies that are
# placed in the same position. The default state would be for these two
# bodies to be coincident, and thus collide.
context = diagram.CreateDefaultContext()
sg_context = diagram.GetMutableSubsystemContext(scene_graph, context)
query_object = scene_graph.get_query_output_port().Eval(sg_context)
# Implicitly require that this should be size 1.
point_pair, = query_object.ComputePointPairPenetration()
self.assertIsInstance(point_pair, PenetrationAsPointPair)
signed_distance_pair, = query_object.\
ComputeSignedDistancePairwiseClosestPoints()
self.assertIsInstance(signed_distance_pair, SignedDistancePair)
inspector = query_object.inspector()
bodies = {
plant.GetBodyFromFrameId(inspector.GetFrameId(id_))
for id_ in [point_pair.id_A, point_pair.id_B]
}
self.assertSetEqual(
bodies,
{plant.GetBodyByName("body1"),
plant.GetBodyByName("body2")})
def run_pendulum_example(args):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
parser = Parser(plant)
parser.AddModelFromFile(
FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf"))
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base"))
plant.Finalize()
pose_bundle_output_port = scene_graph.get_pose_bundle_output_port()
Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]],
dtype=np.float64)
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph,
T_VW=Tview,
xlim=[-1.2, 1.2],
ylim=[-1.2, 1.2]))
builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
# Fix the input port to zero.
plant_context = diagram.GetMutableSubsystemContext(
plant, simulator.get_mutable_context())
plant_context.FixInputPort(plant.get_actuation_input_port().get_index(),
np.zeros(plant.num_actuators()))
plant_context.SetContinuousState([0.5, 0.1])
simulator.AdvanceTo(args.duration)
def test_viewer_applet(self):
"""Check that _ViewerApplet doesn't crash when receiving messages.
Note that many geometry types are not yet covered by this test.
"""
# Create the device under test.
dut = mut.Meldis()
meshcat = dut.meshcat
lcm = dut._lcm
# The path is created by the constructor.
self.assertEqual(meshcat.HasPath("/DRAKE_VIEWER"), True)
# Enqueue the load + draw messages.
sdf_file = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
parser = Parser(plant=plant)
parser.AddModelFromFile(sdf_file)
plant.Finalize()
DrakeVisualizer.AddToBuilder(builder=builder, scene_graph=scene_graph,
lcm=lcm)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
diagram.Publish(context)
# The geometry isn't registered until the load is processed.
link_path = "/DRAKE_VIEWER/2/plant/acrobot/Link2/0"
self.assertEqual(meshcat.HasPath(link_path), False)
# Process the load + draw; make sure the geometry exists now.
lcm.HandleSubscriptions(timeout_millis=0)
dut._invoke_subscriptions()
self.assertEqual(meshcat.HasPath(link_path), True)
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 run_pendulum_example(args):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
parser = Parser(plant)
parser.AddModelFromFile(FindResourceOrThrow(
"drake/examples/pendulum/Pendulum.urdf"))
plant.Finalize()
T_VW = np.array([[1., 0., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]])
visualizer = ConnectPlanarSceneGraphVisualizer(
builder, scene_graph, T_VW=T_VW, xlim=[-1.2, 1.2], ylim=[-1.2, 1.2])
if args.playback:
visualizer.start_recording()
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
# Fix the input port to zero.
plant_context = diagram.GetMutableSubsystemContext(
plant, simulator.get_mutable_context())
plant.get_actuation_input_port().FixValue(
plant_context, np.zeros(plant.num_actuators()))
plant_context.SetContinuousState([0.5, 0.1])
simulator.AdvanceTo(args.duration)
if args.playback:
visualizer.stop_recording()
ani = visualizer.get_recording_as_animation()
# Playback the recording and save the output.
ani.save("{}/pend_playback.mp4".format(temp_directory()), fps=30)
def pendulum_example(duration=1., playback=True, show=True):
"""
Simulate the pendulum
Arguments:
duration: Simulation duration (sec)
playback: enable pyplot animations
"""
# To make a visualization, we have to attach a multibody plant, a scene graph, and a visualizer together. In Drake, we can connect all these systems together in a Diagram.
builder = DiagramBuilder()
# AddMultibodyPlantSceneGraph: Adds a multibody plant and scene graph to the Diagram, and connects their geometry ports. The second input is the timestep for MultibodyPlant
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.)
# Now we create the plant model from a file
parser = Parser(plant)
parser.AddModelFromFile(
FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf"))
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base"))
plant.Finalize()
# The SceneGraph port that communicates with the visualizer is the pose bundle output port. A PoseBundle is just a set of poses in SE(3) and a set of frame velocities, expressed in the world frame, used for rendering.
pose_bundle_output_port = scene_graph.get_pose_bundle_output_port()
# T_VW is the projection matrix from view coordinates to world coordinates
T_VW = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]])
# Now we add a planar visualizer to the the diagram, so we can see the results
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph,
T_VW=T_VW,
xlim=[-1.2, 1.2],
ylim=[-1.2, 1.2],
show=show))
# finally, we must connect the scene_graph to the visualizer so they can communicate
builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0))
if playback:
visualizer.start_recording()
# To finalize the diagram, we build it
diagram = builder.Build()
# We create a simulator of our diagram to step through the diagram in time
simulator = Simulator(diagram)
# Initialize prepares the simulator for simulation
simulator.Initialize()
# Slow down the simulator to realtime. Otherwise it could run too fast
simulator.set_target_realtime_rate(1.)
# To set initial conditions, we modify the mutable simulator context (we could do this before Initialize)
plant_context = diagram.GetMutableSubsystemContext(
plant, simulator.get_mutable_context())
plant_context.SetContinuousState([0.5, 0.1])
# Now we fix the value of the actuation to get an unactuated simulation
plant.get_actuation_input_port().FixValue(
plant_context, np.zeros([plant.num_actuators()]))
# Run the simulation to the specified duration
simulator.AdvanceTo(duration)
# Return an animation, if one was made
if playback:
visualizer.stop_recording()
ani = visualizer.get_recording_as_animation()
return ani
else:
return None
def test_joint_sliders(self):
# Load up an acrobot.
acrobot_file = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.urdf")
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
parser = Parser(plant)
parser.AddModelFromFile(acrobot_file)
plant.Finalize()
# Construct a sliders system, using every available option.
meshcat = Meshcat()
dut = JointSliders(
meshcat=meshcat,
plant=plant,
initial_value=[0.1, 0.2],
lower_limit=[-3.0, -6.0],
upper_limit=[3.0, 6.0],
step=[0.25, 0.50],
)
# Various methods should not crash.
dut.get_output_port()
dut.Delete()
# The constructor also accepts single values for broadcast (except for
# the initial value).
dut = JointSliders(
meshcat=meshcat,
plant=plant,
initial_value=[0.1, 0.2],
lower_limit=-3.0,
upper_limit=3.0,
step=0.1,
)
dut.Delete()
# The constructor also accepts None directly, for optionals.
dut = JointSliders(
meshcat=meshcat,
plant=plant,
initial_value=None,
lower_limit=None,
upper_limit=None,
step=None,
)
dut.Delete()
# The constructor has default values, in any case.
dut = JointSliders(meshcat, plant)
# The Run function doesn't crash.
builder.AddSystem(dut)
diagram = builder.Build()
dut.Run(diagram=diagram, timeout=1.0)
# The SetPositions function doesn't crash (Acrobot has two positions).
dut.SetPositions(q=[1, 2])
def test_manipulation_station_add_iiwa_and_wsg_explicitly(self):
station = ManipulationStation()
parser = Parser(station.get_mutable_multibody_plant(),
station.get_mutable_scene_graph())
plant = station.get_mutable_multibody_plant()
# Add models for iiwa and wsg
iiwa_model_file = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/iiwa7/"
"iiwa7_no_collision.sdf")
iiwa = parser.AddModelFromFile(iiwa_model_file, "iiwa")
X_WI = RigidTransform.Identity()
plant.WeldFrames(plant.world_frame(),
plant.GetFrameByName("iiwa_link_0", iiwa),
X_WI)
wsg_model_file = FindResourceOrThrow(
"drake/manipulation/models/wsg_50_description/sdf/"
"schunk_wsg_50.sdf")
wsg = parser.AddModelFromFile(wsg_model_file, "gripper")
X_7G = RigidTransform.Identity()
plant.WeldFrames(
plant.GetFrameByName("iiwa_link_7", iiwa),
plant.GetFrameByName("body", wsg),
X_7G)
# Register models for the controller.
station.RegisterIiwaControllerModel(
iiwa_model_file, iiwa, plant.world_frame(),
plant.GetFrameByName("iiwa_link_0", iiwa), X_WI)
station.RegisterWsgControllerModel(
wsg_model_file, wsg,
plant.GetFrameByName("iiwa_link_7", iiwa),
plant.GetFrameByName("body", wsg), X_7G)
# Finalize
station.Finalize()
self.assertEqual(station.num_iiwa_joints(), 7)
# This WSG gripper model has 2 independent dof, and the IIWA model
# has 7.
self.assertEqual(plant.num_positions(), 9)
self.assertEqual(plant.num_velocities(), 9)
def test_contact_applet_hydroelastic(self):
"""Check that _ContactApplet doesn't crash when receiving hydroelastic
messages.
"""
# Create the device under test.
dut = mut.Meldis()
meshcat = dut.meshcat
lcm = dut._lcm
# Enqueue a hydroelastic contact message.
sdf_file = FindResourceOrThrow(
"drake/multibody/meshcat/test/hydroelastic.sdf")
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.001)
parser = Parser(plant=plant)
parser.AddModelFromFile(sdf_file)
body1 = plant.GetBodyByName("body1")
body2 = plant.GetBodyByName("body2")
plant.AddJoint(
PrismaticJoint(name="body1",
frame_on_parent=plant.world_body().body_frame(),
frame_on_child=body1.body_frame(),
axis=[0, 0, 1]))
plant.AddJoint(
PrismaticJoint(name="body2",
frame_on_parent=plant.world_body().body_frame(),
frame_on_child=body2.body_frame(),
axis=[1, 0, 0]))
plant.Finalize()
ConnectContactResultsToDrakeVisualizer(builder=builder,
plant=plant,
scene_graph=scene_graph,
lcm=lcm)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant.SetPositions(plant.GetMyMutableContextFromRoot(context),
[0.1, 0.3])
diagram.Publish(context)
# The geometry isn't registered until the load is processed.
hydro_path = "/CONTACT_RESULTS/hydroelastic/" + \
"body1.two_bodies::body1_collision+body2"
hydro_path2 = "/CONTACT_RESULTS/hydroelastic/" + \
"body1.two_bodies::body1_collision2+body2"
self.assertEqual(meshcat.HasPath(hydro_path), False)
self.assertEqual(meshcat.HasPath(hydro_path2), False)
# Process the load + draw; contact results should now exist.
lcm.HandleSubscriptions(timeout_millis=1)
dut._invoke_subscriptions()
self.assertEqual(meshcat.HasPath("/CONTACT_RESULTS/hydroelastic"),
True)
self.assertEqual(meshcat.HasPath(hydro_path), True)
self.assertEqual(meshcat.HasPath(hydro_path2), True)
def make_ball_paddle(contact_model, contact_surface_representation, time_step):
multibody_plant_config = \
MultibodyPlantConfig(
time_step=time_step,
contact_model=contact_model,
contact_surface_representation=contact_surface_representation)
# We pose the paddle, so that its top surface is on World's X-Y plane.
# Intuitively we push it down 1 cm because the box is 2 cm thick.
p_WPaddle_fixed = RigidTransform(RollPitchYaw(0, 0, 0),
np.array([0, 0, -0.01]))
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlant(multibody_plant_config, builder)
parser = Parser(plant)
paddle_sdf_file_name = \
FindResourceOrThrow("drake/examples/hydroelastic/python_ball_paddle"
"/paddle.sdf")
paddle = parser.AddModelFromFile(paddle_sdf_file_name, model_name="paddle")
plant.WeldFrames(frame_on_parent_F=plant.world_frame(),
frame_on_child_M=plant.GetFrameByName("paddle", paddle),
X_FM=p_WPaddle_fixed)
ball_sdf_file_name = \
FindResourceOrThrow("drake/examples/hydroelastic/python_ball_paddle"
"/ball.sdf")
parser.AddModelFromFile(ball_sdf_file_name)
# TODO(DamrongGuoy): Let users override hydroelastic modulus, dissipation,
# and resolution hint from the two SDF files above.
plant.Finalize()
DrakeVisualizer.AddToBuilder(builder=builder, scene_graph=scene_graph)
ConnectContactResultsToDrakeVisualizer(builder=builder,
plant=plant,
scene_graph=scene_graph)
nx = plant.num_positions() + plant.num_velocities()
state_logger = builder.AddSystem(VectorLogSink(nx))
builder.Connect(plant.get_state_output_port(),
state_logger.get_input_port())
diagram = builder.Build()
return diagram, plant, state_logger
def test_multibody_add_joint(self, T):
"""
Tests joint constructors and `AddJoint`.
"""
MultibodyPlant = MultibodyPlant_[T]
RigidTransform = RigidTransform_[T]
RollPitchYaw = RollPitchYaw_[T]
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
# N.B. `Parser` only supports `MultibodyPlant_[float]`.
plant_f = MultibodyPlant_[float]()
parser = Parser(plant_f)
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_f = [
RevoluteJoint_[float](
name="revolve_things",
frame_on_parent=plant_f.GetBodyByName(
distal_frame, instances[1]).body_frame(),
frame_on_child=plant_f.GetBodyByName(
proximal_frame, instances[2]).body_frame(),
axis=[0, 0, 1],
damping=0.),
WeldJoint_[float](
name="weld_things",
parent_frame_P=plant_f.GetBodyByName(
distal_frame, instances[0]).body_frame(),
child_frame_C=plant_f.GetBodyByName(
proximal_frame, instances[1]).body_frame(),
X_PC=RigidTransform_[float].Identity()),
]
for joint_f in joints_f:
joint_out = plant_f.AddJoint(joint_f)
self.assertIs(joint_f, joint_out)
# The model is now complete.
plant_f.Finalize()
plant = to_type(plant_f, T)
for joint_f in joints_f:
# Not using `joint` because we converted the `plant_f` to `plant`
joint = plant.get_joint(joint_index=joint_f.index())
self._test_joint_api(T, joint)
def main():
args_parser = argparse.ArgumentParser()
args_parser.add_argument("--simulation_sec", type=float, default=np.inf)
args_parser.add_argument("--sim_dt", type=float, default=0.1)
args_parser.add_argument(
"--single_shot",
action="store_true",
help="Test workflow of visulaization through Simulator.Initialize")
args_parser.add_argument("--realtime_rate", type=float, default=1.)
args_parser.add_argument("--num_models", type=int, default=3)
args = args_parser.parse_args()
sdf_file = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/iiwa7/"
"iiwa7_no_collision.sdf")
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.01)
parser = Parser(plant)
models = []
for i in range(args.num_models):
model_name = f"iiwa{i}"
# TODO(eric.cousineau): This warns about mutating the package path
# multiple times :(
model = parser.AddModelFromFile(sdf_file, model_name)
models.append(model)
base_frame = plant.GetFrameByName("iiwa_link_0", model)
# Translate along x-axis by 1m to separate.
X_WB = RigidTransform([i, 0, 0])
plant.WeldFrames(plant.world_frame(), base_frame, X_WB)
plant.Finalize()
for model in models:
no_control(plant, builder, model)
ConnectDrakeVisualizer(builder, scene_graph)
ConnectRvizVisualizer(builder, scene_graph)
diagram = builder.Build()
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
simulator.set_target_realtime_rate(args.realtime_rate)
# Wait for ROS publishers to wake up :(
time.sleep(0.3)
if args.single_shot:
# To see what 'preview' scripts look like.
# TODO(eric.cousineau): Make this work *robustly* with Rviz. Markers
# still don't always show up :(
simulator.Initialize()
diagram.Publish(context)
else:
while context.get_time() < args.simulation_sec:
# Use increments to permit Ctrl+C to be caught.
simulator.AdvanceTo(context.get_time() + args.sim_dt)
def _make_diagram(self, *, sdf_filename, visualizer_params, lcm):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
parser = Parser(plant=plant)
parser.AddModelFromFile(FindResourceOrThrow(sdf_filename))
plant.Finalize()
DrakeVisualizer.AddToBuilder(builder=builder,
scene_graph=scene_graph,
params=visualizer_params,
lcm=lcm)
diagram = builder.Build()
return diagram
def test_scene_graph_queries(self, T):
# SceneGraph does not support `Expression` type.
PenetrationAsPointPair = PenetrationAsPointPair_[T]
builder_f = DiagramBuilder_[float]()
# N.B. `Parser` only supports `MultibodyPlant_[float]`.
plant_f, scene_graph_f = AddMultibodyPlantSceneGraph(builder_f)
parser = Parser(plant=plant_f, scene_graph=scene_graph_f)
parser.AddModelFromFile(
FindResourceOrThrow(
"drake/bindings/pydrake/multibody/test/two_bodies.sdf"))
plant_f.Finalize()
diagram_f = builder_f.Build()
# Do conversion.
diagram = to_type(diagram_f, T)
plant = diagram.GetSubsystemByName(plant_f.get_name())
scene_graph = diagram.GetSubsystemByName(scene_graph_f.get_name())
# The model `two_bodies` has two (implicitly) floating bodies that are
# placed in the same position. The default state would be for these two
# bodies to be coincident, and thus collide.
context = diagram.CreateDefaultContext()
sg_context = diagram.GetMutableSubsystemContext(scene_graph, context)
query_object = scene_graph.get_query_output_port().Eval(sg_context)
# Implicitly require that this should be size 1.
point_pair, = query_object.ComputePointPairPenetration()
self.assertIsInstance(point_pair, PenetrationAsPointPair_[float])
signed_distance_pair, = query_object.\
ComputeSignedDistancePairwiseClosestPoints()
self.assertIsInstance(signed_distance_pair, SignedDistancePair_[T])
signed_distance_to_point = query_object.\
ComputeSignedDistanceToPoint(p_WQ=np.ones(3))
self.assertEqual(len(signed_distance_to_point), 2)
self.assertIsInstance(signed_distance_to_point[0],
SignedDistanceToPoint_[T])
self.assertIsInstance(signed_distance_to_point[1],
SignedDistanceToPoint_[T])
inspector = query_object.inspector()
def get_body_from_frame_id(frame_id):
# Get body from frame id, and check inverse method.
body = plant.GetBodyFromFrameId(frame_id)
self.assertEqual(
plant.GetBodyFrameIdIfExists(body.index()), frame_id)
return body
bodies = {get_body_from_frame_id(inspector.GetFrameId(id_))
for id_ in [point_pair.id_A, point_pair.id_B]}
self.assertSetEqual(
bodies,
{plant.GetBodyByName("body1"), plant.GetBodyByName("body2")})
def add_object_from_file(self, object_name, object_path):
parser = Parser(self._mbp, self._sg)
self._model_ids[object_name] = parser.AddModelFromFile(object_path, object_name)
# this should be masked
self._model_names_to_mask.append(object_name)
def finalize_func():
state_output_port = self._mbp.get_state_output_port(self._model_ids[object_name])
port_state_output_name = object_name + '_state_output'
self._port_names.append(port_state_output_name)
self._builder.ExportOutput(state_output_port, port_state_output_name)
self._finalize_functions.append(finalize_func)
def run_pendulum_example(duration=1.0, playback=True, show=True):
"""
Runs a simulation of a pendulum
Arguments:
duration: Simulation duration (sec).
playback: Enable pyplot animations to be produced.
"""
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
parser = Parser(plant)
parser.AddModelFromFile(
FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf"))
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base"))
plant.Finalize()
pose_bundle_output_port = scene_graph.get_pose_bundle_output_port()
T_VW = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]])
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph,
T_VW=T_VW,
xlim=[-1.2, 1.2],
ylim=[-1.2, 1.2],
show=show))
builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0))
if playback:
visualizer.start_recording()
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
# Fix the input port to zero
plant_context = diagram.GetMutableSubsystemContext(
plant, simulator.get_mutable_context())
plant.get_actuation_input_port().FixValue(plant_context,
np.zeros(plant.num_actuators()))
plant_context.SetContinuousState([0.5, 0.1])
simulator.AdvanceTo(duration)
if playback:
visualizer.stop_recording()
ani = visualizer.get_recording_as_animation()
return ani
else:
return None
def test_contact_applet_point_pair(self):
"""Check that _ContactApplet doesn't crash when receiving point
contact messages.
"""
# Create the device under test.
dut = mut.Meldis()
meshcat = dut.meshcat
lcm = dut._lcm
# Enqueue a point contact result message.
sdf_file = FindResourceOrThrow(
"drake/examples/manipulation_station/models/sphere.sdf")
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.001)
parser = Parser(plant=plant)
sphere1_model = parser.AddModelFromFile(sdf_file, "sphere1")
sphere2_model = parser.AddModelFromFile(sdf_file, "sphere2")
body1 = plant.GetBodyByName("base_link", sphere1_model)
body2 = plant.GetBodyByName("base_link", sphere2_model)
plant.AddJoint(
PrismaticJoint(name="sphere1_x",
frame_on_parent=plant.world_body().body_frame(),
frame_on_child=body1.body_frame(),
axis=[1, 0, 0]))
plant.AddJoint(
PrismaticJoint(name="sphere2_x",
frame_on_parent=plant.world_body().body_frame(),
frame_on_child=body2.body_frame(),
axis=[1, 0, 0]))
plant.Finalize()
ConnectContactResultsToDrakeVisualizer(builder=builder,
plant=plant,
scene_graph=scene_graph,
lcm=lcm)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant.SetPositions(plant.GetMyMutableContextFromRoot(context),
[-0.03, 0.03])
diagram.Publish(context)
# The geometry isn't registered until the load is processed.
pair_path = "/CONTACT_RESULTS/point/base_link(2)+base_link(3)"
self.assertEqual(meshcat.HasPath(pair_path), False)
# Process the load + draw; make sure the geometry exists now.
lcm.HandleSubscriptions(timeout_millis=0)
dut._invoke_subscriptions()
self.assertEqual(meshcat.HasPath(pair_path), True)
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=RigidTransform.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 __init__(self, scene_graph):
LeafSystem.__init__(self)
assert scene_graph
mbp = MultibodyPlant(0.0)
parser = Parser(mbp, scene_graph)
model_id = parser.AddModelFromFile(
FindResource("models/glider/glider.urdf"))
mbp.Finalize()
self.source_id = mbp.get_source_id()
self.body_frame_id = mbp.GetBodyFrameIdOrThrow(
mbp.GetBodyIndices(model_id)[0])
self.elevator_frame_id = mbp.GetBodyFrameIdOrThrow(
mbp.GetBodyIndices(model_id)[1])
self.DeclareVectorInputPort("state", BasicVector(7))
self.DeclareAbstractOutputPort(
"geometry_pose", lambda: AbstractValue.Make(FramePoseVector()),
self.OutputGeometryPose)
def CreateIiwaControllerPlant():
# creates plant that includes only the robot, used for controllers.
robot_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/iiwa7/iiwa7_no_collision.sdf"
)
sim_timestep = 1e-3
plant_robot = MultibodyPlant(sim_timestep)
parser = Parser(plant=plant_robot)
parser.AddModelFromFile(robot_sdf_path)
plant_robot.WeldFrames(A=plant_robot.world_frame(),
B=plant_robot.GetFrameByName("iiwa_link_0"))
plant_robot.mutable_gravity_field().set_gravity_vector([0, 0, 0])
plant_robot.Finalize()
link_frame_indices = []
for i in range(8):
link_frame_indices.append(
plant_robot.GetFrameByName("iiwa_link_" + str(i)).index())
return plant_robot, link_frame_indices
def test_cart_pole(self):
"""Cart-Pole with simple geometry."""
file_name = FindResourceOrThrow(
"drake/examples/multibody/cart_pole/cart_pole.sdf")
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
cart_pole = builder.AddSystem(MultibodyPlant())
parser = Parser(plant=cart_pole, scene_graph=scene_graph)
parser.AddModelFromFile(file_name)
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, open_browser=False))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
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.StepTo(.1)
def calculate_ee_composite_inertia(gripper_path):
plant = MultibodyPlant(0)
parser = Parser(plant)
parser.AddModelFromFile(gripper_path)
plant.Finalize()
if gripper_path == schunk_path:
gripper_body = plant.GetBodyByName("body")
left_finger = plant.GetBodyByName("left_finger")
right_finger = plant.GetBodyByName("right_finger")
left_joint = plant.GetJointByName("left_finger_sliding_joint")
right_joint = plant.GetJointByName("right_finger_sliding_joint")
elif gripper_path == franka_hand_path:
gripper_body = plant.GetBodyByName("panda_hand")
left_finger = plant.GetBodyByName("panda_leftfinger")
right_finger = plant.GetBodyByName("panda_rightfinger")
left_joint = plant.GetJointByName("panda_finger_joint1")
right_joint = plant.GetJointByName("panda_finger_joint2")
else:
raise ValueError("Gripper %s not known" % gripper_path)
X_FLP = left_joint.frame_on_parent().GetFixedPoseInBodyFrame()
X_FLC = left_joint.frame_on_child().GetFixedPoseInBodyFrame()
X_FL = X_FLP.multiply(X_FLC.inverse())
X_FRP = right_joint.frame_on_parent().GetFixedPoseInBodyFrame()
X_FRC = right_joint.frame_on_child().GetFixedPoseInBodyFrame()
X_FR = X_FRP.multiply(X_FRC.inverse())
I_base = gripper_body.default_spatial_inertia()
I_FL = left_finger.default_spatial_inertia()
I_FR = right_finger.default_spatial_inertia()
I_ee = I_base
I_ee += I_FL.ReExpress(X_FL.rotation()).Shift(-X_FL.translation())
I_ee += I_FR.ReExpress(X_FR.rotation()).Shift(-X_FR.translation())
return I_ee
def test_model_instance_state_access_by_array(self):
# Create a MultibodyPlant with a kuka arm and a schunk gripper.
# the arm is welded to the world, the gripper is welded to the
# arm's end effector.
wsg50_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"wsg_50_description/sdf/schunk_wsg_50.sdf")
iiwa_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"iiwa_description/sdf/iiwa14_no_collision.sdf")
timestep = 0.0002
plant = MultibodyPlant(timestep)
parser = Parser(plant)
iiwa_model = parser.AddModelFromFile(file_name=iiwa_sdf_path,
model_name='robot')
gripper_model = parser.AddModelFromFile(file_name=wsg50_sdf_path,
model_name='gripper')
# Weld the base of arm and gripper to reduce the number of states.
X_EeGripper = RigidTransform(RollPitchYaw(np.pi / 2, 0, np.pi / 2),
[0, 0, 0.081])
plant.WeldFrames(A=plant.world_frame(),
B=plant.GetFrameByName("iiwa_link_0", iiwa_model))
plant.WeldFrames(A=plant.GetFrameByName("iiwa_link_7", iiwa_model),
B=plant.GetFrameByName("body", gripper_model),
X_AB=X_EeGripper)
plant.Finalize()
# Create a context of the MBP and set the state of the context
# to desired values.
context = plant.CreateDefaultContext()
nq = plant.num_positions()
nq_iiwa = plant.num_positions(iiwa_model)
nv = plant.num_velocities()
nv_iiwa = plant.num_velocities(iiwa_model)
q_iiwa_desired = np.linspace(0, 0.3, 7)
v_iiwa_desired = q_iiwa_desired + 0.4
q_gripper_desired = [0.4, 0.5]
v_gripper_desired = [-1., -2.]
x_desired = np.zeros(nq + nv)
x_desired[0:7] = q_iiwa_desired
x_desired[7:9] = q_gripper_desired
x_desired[nq:nq + 7] = v_iiwa_desired
x_desired[nq + 7:nq + nv] = v_gripper_desired
x = plant.GetMutablePositionsAndVelocities(context=context)
x[:] = x_desired
q = plant.GetPositions(context=context)
v = plant.GetVelocities(context=context)
# Get state from context.
x = plant.GetPositionsAndVelocities(context=context)
x_tmp = plant.GetMutablePositionsAndVelocities(context=context)
self.assertTrue(np.allclose(x_desired, x_tmp))
# Get positions and velocities of specific model instances
# from the position/velocity vector of the plant.
q_iiwa = plant.GetPositions(context=context, model_instance=iiwa_model)
q_iiwa_array = plant.GetPositionsFromArray(model_instance=iiwa_model,
q=q)
self.assertTrue(np.allclose(q_iiwa, q_iiwa_array))
q_gripper = plant.GetPositions(context=context,
model_instance=gripper_model)
v_iiwa = plant.GetVelocities(context=context,
model_instance=iiwa_model)
v_iiwa_array = plant.GetVelocitiesFromArray(model_instance=iiwa_model,
v=v)
self.assertTrue(np.allclose(v_iiwa, v_iiwa_array))
v_gripper = plant.GetVelocities(context=context,
model_instance=gripper_model)
# Assert that the `GetPositions` and `GetVelocities` return
# the desired values set earlier.
self.assertTrue(np.allclose(q_iiwa_desired, q_iiwa))
self.assertTrue(np.allclose(v_iiwa_desired, v_iiwa))
self.assertTrue(np.allclose(q_gripper_desired, q_gripper))
self.assertTrue(np.allclose(v_gripper_desired, v_gripper))
# Verify that SetPositionsInArray() and SetVelocitiesInArray() works.
plant.SetPositionsInArray(model_instance=iiwa_model,
q_instance=np.zeros(nq_iiwa),
q=q)
self.assertTrue(
np.allclose(
plant.GetPositionsFromArray(model_instance=iiwa_model, q=q),
np.zeros(nq_iiwa)))
plant.SetVelocitiesInArray(model_instance=iiwa_model,
v_instance=np.zeros(nv_iiwa),
v=v)
self.assertTrue(
np.allclose(
plant.GetVelocitiesFromArray(model_instance=iiwa_model, v=v),
np.zeros(nv_iiwa)))
# Check actuation.
nu = plant.num_actuated_dofs()
u = np.zeros(nu)
u_iiwa = np.arange(nv_iiwa)
plant.SetActuationInArray(model_instance=iiwa_model,
u_instance=u_iiwa,
u=u)
self.assertTrue(np.allclose(u[:7], u_iiwa))
def test_model_instance_state_access(self):
# Create a MultibodyPlant with a kuka arm and a schunk gripper.
# the arm is welded to the world, the gripper is welded to the
# arm's end effector.
wsg50_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"wsg_50_description/sdf/schunk_wsg_50.sdf")
iiwa_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"iiwa_description/sdf/iiwa14_no_collision.sdf")
plant = MultibodyPlant()
parser = Parser(plant)
iiwa_model = parser.AddModelFromFile(file_name=iiwa_sdf_path,
model_name='robot')
gripper_model = parser.AddModelFromFile(file_name=wsg50_sdf_path,
model_name='gripper')
# Weld the base of arm and gripper to reduce the number of states.
X_EeGripper = RigidTransform(RollPitchYaw(np.pi / 2, 0, np.pi / 2),
[0, 0, 0.081])
plant.WeldFrames(A=plant.world_frame(),
B=plant.GetFrameByName("iiwa_link_0", iiwa_model))
plant.WeldFrames(A=plant.GetFrameByName("iiwa_link_7", iiwa_model),
B=plant.GetFrameByName("body", gripper_model),
X_AB=X_EeGripper)
plant.Finalize()
# Create a context of the MBP and set the state of the context
# to desired values.
context = plant.CreateDefaultContext()
nq = plant.num_positions()
nv = plant.num_velocities()
nq_iiwa = 7
nv_iiwa = 7
nq_gripper = 2
nv_gripper = 2
q_iiwa_desired = np.zeros(nq_iiwa)
v_iiwa_desired = np.zeros(nv_iiwa)
q_gripper_desired = np.zeros(nq_gripper)
v_gripper_desired = np.zeros(nv_gripper)
q_iiwa_desired[2] = np.pi / 3
q_gripper_desired[0] = 0.1
v_iiwa_desired[1] = 5.0
q_gripper_desired[0] = -0.3
x_iiwa_desired = np.zeros(nq_iiwa + nv_iiwa)
x_iiwa_desired[0:nq_iiwa] = q_iiwa_desired
x_iiwa_desired[nq_iiwa:nq_iiwa + nv_iiwa] = v_iiwa_desired
x_gripper_desired = np.zeros(nq_gripper + nv_gripper)
x_gripper_desired[0:nq_gripper] = q_gripper_desired
x_gripper_desired[nq_gripper:nq_gripper +
nv_gripper] = v_gripper_desired
x_desired = np.zeros(nq + nv)
x_desired[0:7] = q_iiwa_desired
x_desired[7:9] = q_gripper_desired
x_desired[nq:nq + 7] = v_iiwa_desired
x_desired[nq + 7:nq + nv] = v_gripper_desired
# Check SetPositionsAndVelocities() for each model instance.
# Do the iiwa model first.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetPositionsAndVelocities(context, iiwa_model, x_iiwa_desired)
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context, iiwa_model),
x_iiwa_desired))
self.assertTrue(
np.allclose(
plant.GetPositionsAndVelocities(context, gripper_model),
np.zeros(nq_gripper + nv_gripper)))
# Do the gripper model.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetPositionsAndVelocities(context, gripper_model,
x_gripper_desired)
self.assertTrue(
np.allclose(
plant.GetPositionsAndVelocities(context, gripper_model),
x_gripper_desired))
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context, iiwa_model),
np.zeros(nq_iiwa + nv_iiwa)))
# Check SetPositions() for each model instance.
# Do the iiwa model first.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetPositions(context, iiwa_model, q_iiwa_desired)
self.assertTrue(
np.allclose(plant.GetPositions(context, iiwa_model),
q_iiwa_desired))
self.assertTrue(
np.allclose(plant.GetVelocities(context, iiwa_model),
np.zeros(nv_iiwa)))
self.assertTrue(
np.allclose(
plant.GetPositionsAndVelocities(context, gripper_model),
np.zeros(nq_gripper + nv_gripper)))
# Do the gripper model.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetPositions(context, gripper_model, q_gripper_desired)
self.assertTrue(
np.allclose(plant.GetPositions(context, gripper_model),
q_gripper_desired))
self.assertTrue(
np.allclose(plant.GetVelocities(context, gripper_model),
np.zeros(nq_gripper)))
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context, iiwa_model),
np.zeros(nq_iiwa + nv_iiwa)))
# Check SetVelocities() for each model instance.
# Do the iiwa model first.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetVelocities(context, iiwa_model, v_iiwa_desired)
self.assertTrue(
np.allclose(plant.GetVelocities(context, iiwa_model),
v_iiwa_desired))
self.assertTrue(
np.allclose(plant.GetPositions(context, iiwa_model),
np.zeros(nq_iiwa)))
self.assertTrue(
np.allclose(
plant.GetPositionsAndVelocities(context, gripper_model),
np.zeros(nq_gripper + nv_gripper)))
# Do the gripper model.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetVelocities(context, gripper_model, v_gripper_desired)
self.assertTrue(
np.allclose(plant.GetVelocities(context, gripper_model),
v_gripper_desired))
self.assertTrue(
np.allclose(plant.GetPositions(context, gripper_model),
np.zeros(nv_gripper)))
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context, iiwa_model),
np.zeros(nq_iiwa + nv_iiwa)))
help="Disable opening the gui window for testing.")
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
builder = DiagramBuilder()
if args.hardware:
station = builder.AddSystem(ManipulationStationHardwareInterface())
station.Connect(wait_for_cameras=False)
else:
station = builder.AddSystem(ManipulationStation())
station.SetupDefaultStation()
parser = Parser(station.get_mutable_multibody_plant(),
station.get_mutable_scene_graph())
object = parser.AddModelFromFile(
FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf"),
"object")
station.Finalize()
ConnectDrakeVisualizer(builder, station.get_scene_graph(),
station.GetOutputPort("pose_bundle"))
if args.meshcat:
meshcat = builder.AddSystem(
MeshcatVisualizer(station.get_scene_graph(), zmq_url=args.meshcat))
builder.Connect(station.GetOutputPort("pose_bundle"),
meshcat.get_input_port(0))
robot = station.get_controller_plant()
params = DifferentialInverseKinematicsParameters(robot.num_positions(),
robot.num_velocities())
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.
MeshcatVisualizer.add_argparse_argument(args_parser)
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(0.0)
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()
# 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)
# Connect 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))
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.AdvanceTo(0.1)
else:
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(np.inf)
def LittleDogSimulationDiagram2(lcm, rb_tree, dt, drake_visualizer):
builder = DiagramBuilder()
num_pos = rb_tree.get_num_positions()
num_actuators = rb_tree.get_num_actuators()
zero_config = np.zeros(
(rb_tree.get_num_actuators() * 2, 1)) # just for test
# zero_config = np.concatenate((np.array([0, 1, 1, 1, 1.7, 0.5, 0, 0, 0]),np.zeros(9)), axis=0)
# zero_config = np.concatenate((rb_tree.getZeroConfiguration(),np.zeros(9)), axis=0)
zero_config = np.concatenate(
(np.array([1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]), np.zeros(12)), axis=0)
# zero_config[0] = 1.7
# zero_config[1] = 1.7
# zero_config[2] = 1.7
# zero_config[3] = 1.7
# 1.SceneGraph system
scene_graph = builder.AddSystem(SceneGraph())
scene_graph.RegisterSource('scene_graph_n')
plant = builder.AddSystem(MultibodyPlant())
plant_parser = Parser(plant, scene_graph)
plant_parser.AddModelFromFile(file_name=model_path, model_name="littledog")
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("body"))
# Add gravity to the model.
plant.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
# Model is completed
plant.Finalize()
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()))
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
# Robot State Encoder
robot_state_encoder = builder.AddSystem(
RobotStateEncoder(rb_tree)) # force_sensor_info
robot_state_encoder.set_name('robot_state_encoder')
builder.Connect(plant.get_continuous_state_output_port(),
robot_state_encoder.joint_state_results_input_port())
# Robot command to Plant Converter
robot_command_to_rigidbodyplant_converter = builder.AddSystem(
RobotCommandToRigidBodyPlantConverter(
rb_tree.actuators,
motor_gain=None)) # input argu: rigidbody actuators
robot_command_to_rigidbodyplant_converter.set_name(
'robot_command_to_rigidbodyplant_converter')
builder.Connect(
robot_command_to_rigidbodyplant_converter.desired_effort_output_port(),
plant.get_actuation_input_port())
# PID controller
kp, ki, kd = joints_PID_params(rb_tree)
# PIDcontroller = builder.AddSystem(RobotPDAndFeedForwardController(rb_tree, kp, ki, kd))
# PIDcontroller.set_name('PID_controller')
rb = RigidBodyTree()
robot_frame = RigidBodyFrame("robot_frame", rb_tree.world(), [0, 0, 0],
[0, 0, 0])
# insert a robot from urdf files
pmap = PackageMap()
pmap.PopulateFromFolder(os.path.dirname(model_path))
AddModelInstanceFromUrdfStringSearchingInRosPackages(
open(model_path, 'r').read(),
pmap,
os.path.dirname(model_path),
FloatingBaseType.kFixed, # FloatingBaseType.kRollPitchYaw,
robot_frame,
rb)
PIDcontroller = builder.AddSystem(
RbtInverseDynamicsController(rb, kp, ki, kd, False))
PIDcontroller.set_name('PID_controller')
builder.Connect(robot_state_encoder.joint_state_outport_port(),
PIDcontroller.get_input_port(0))
builder.Connect(
PIDcontroller.get_output_port(0),
robot_command_to_rigidbodyplant_converter.robot_command_input_port())
# Ref trajectory
traj_src = builder.AddSystem(ConstantVectorSource(zero_config))
builder.Connect(traj_src.get_output_port(0),
PIDcontroller.get_input_port(1))
# Signal logger
logger = builder.AddSystem(SignalLogger(num_pos * 2))
builder.Connect(plant.get_continuous_state_output_port(),
logger.get_input_port(0))
return builder.Build(), logger, plant
def __init__(self, config=None):
if config is None:
config_path = os.path.join(os.path.dirname(__file__),
"config.yaml")
config = yaml.safe_load(open(config_path, 'r'))
self._config = config
self._step_dt = config["step_dt"]
self._model_name = config["model_name"]
self._sim_diagram = DrakeSimDiagram(config)
mbp = self._sim_diagram.mbp
builder = self._sim_diagram.builder
# === Add table =====
dims = config["table"]["size"]
color = np.array(config["table"]["color"])
friction_params = config["table"]["coulomb_friction"]
box_shape = Box(*dims)
X_W_T = RigidTransform(p=np.array([0., 0., -dims[2] / 2.]))
# This rigid body will be added to the world model instance since
# the model instance is not specified.
box_body = mbp.AddRigidBody(
"table",
SpatialInertia(mass=1.0,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia(1.0, 1.0, 1.0)))
mbp.WeldFrames(mbp.world_frame(), box_body.body_frame(), X_W_T)
mbp.RegisterVisualGeometry(box_body, RigidTransform(), box_shape,
"table_vis", color)
mbp.RegisterCollisionGeometry(box_body, RigidTransform(), box_shape,
"table_collision",
CoulombFriction(*friction_params))
# === Add pusher ====
parser = Parser(mbp, self._sim_diagram.sg)
self._pusher_model_id = parser.AddModelFromFile(
path_util.pusher_peg, "pusher")
base_link = mbp.GetBodyByName("base", self._pusher_model_id)
mbp.WeldFrames(mbp.world_frame(), base_link.body_frame())
def pusher_port_func():
actuation_input_port = mbp.get_actuation_input_port(
self._pusher_model_id)
state_output_port = mbp.get_state_output_port(
self._pusher_model_id)
builder.ExportInput(actuation_input_port, "pusher_actuation_input")
builder.ExportOutput(state_output_port, "pusher_state_output")
self._sim_diagram.finalize_functions.append(pusher_port_func)
# === Add slider ====
parser = Parser(mbp, self._sim_diagram.sg)
self._slider_model_id = parser.AddModelFromFile(
path_util.model_paths[self._model_name], self._model_name)
def slider_port_func():
state_output_port = mbp.get_state_output_port(
self._slider_model_id)
builder.ExportOutput(state_output_port, "slider_state_output")
self._sim_diagram.finalize_functions.append(slider_port_func)
if "rgbd_sensors" in config and config["rgbd_sensors"]["enabled"]:
self._sim_diagram.add_rgbd_sensors_from_config(config)
if "visualization" in config:
if not config["visualization"]:
pass
elif config["visualization"] == "meshcat":
self._sim_diagram.connect_to_meshcat()
elif config["visualization"] == "drake_viz":
self._sim_diagram.connect_to_drake_visualizer()
else:
raise ValueError("Unknown visualization:",
config["visualization"])
self._sim_diagram.finalize()
builder = DiagramBuilder()
builder.AddSystem(self._sim_diagram)
pid = builder.AddSystem(
PidController(kp=[0, 0], kd=[1000, 1000], ki=[0, 0]))
builder.Connect(self._sim_diagram.GetOutputPort("pusher_state_output"),
pid.get_input_port_estimated_state())
builder.Connect(
pid.get_output_port_control(),
self._sim_diagram.GetInputPort("pusher_actuation_input"))
builder.ExportInput(pid.get_input_port_desired_state(),
"pid_input_port_desired_state")
self._diagram = builder.Build()
self._pid_desired_state_port = self._diagram.get_input_port(0)
self._simulator = Simulator(self._diagram)
self.reset()
self.action_space = spaces.Box(low=-1.,
high=1.,
shape=(2, ),
dtype=np.float32)
# TODO: Observation space for images is currently too loose
self.observation_space = convert_observation_to_space(
self.get_observation())
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))
