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 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)
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_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 load_station(time_step=0.0):
# https://github.com/RobotLocomotion/drake/blob/master/bindings/pydrake/examples/manipulation_station_py.cc
object_file_path = FindResourceOrThrow(
"drake/external/models_robotlocomotion/ycb_objects/061_foam_brick.sdf")
#object_file_path = FOAM_BRICK_PATH
station = ManipulationStation(time_step)
station.AddCupboard()
mbp = station.get_mutable_multibody_plant()
scene_graph = station.get_mutable_scene_graph()
object = AddModelFromSdfFile(
file_name=object_file_path,
model_name="object",
plant=mbp,
scene_graph=scene_graph)
station.Finalize()
robot = mbp.GetModelInstanceByName('iiwa')
gripper = mbp.GetModelInstanceByName('gripper')
initial_conf = [0, 0.6 - np.pi / 6, 0, -1.75, 0, 1.0, 0]
#initial_conf[1] += np.pi / 6
initial_positions = dict(zip(get_movable_joints(mbp, robot), initial_conf))
initial_poses = {
object: create_transform(translation=[.6, 0, 0]),
}
task = Task(mbp, scene_graph, robot, gripper, movable=[object], surfaces=[],
initial_positions=initial_positions, initial_poses=initial_poses,
goal_on=[])
return mbp, scene_graph, task
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_scene_graph_queries(self):
builder = DiagramBuilder()
plant, scene_graph = add_plant_and_scene_graph(builder)
AddModelFromSdfFile(
FindResourceOrThrow(
"drake/bindings/pydrake/multibody/test/two_bodies.sdf"), plant,
scene_graph)
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)
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 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 __init__(self, time_step,
object_file_paths=None,
object_base_link_names=None,
X_WObject_list=(X_WObject_default,)):
self.time_step = time_step
# Finalize manipulation station by adding manipuland.
self.station = ManipulationStation(self.time_step)
self.station.AddCupboard()
self.plant = self.station.get_mutable_multibody_plant()
self.objects = []
if object_file_paths is not None:
for i, file_path in enumerate(object_file_paths):
self.objects.append(AddModelFromSdfFile(
file_name=file_path,
model_name="object_{}".format(i),
plant=self.plant,
scene_graph=self.station.get_mutable_scene_graph()
))
self.station.Finalize()
# Object base link names for reference
self.object_base_link_names = object_base_link_names
# Initial pose of the object
self.X_WObject_list = X_WObject_list
def __init__(
self,
time_step,
object_file_path=None,
object_base_link_name=None,
X_WObject=X_WObject_default,
):
self.object_base_link_name = object_base_link_name
self.time_step = time_step
# Finalize manipulation station by adding manipuland.
self.station = ManipulationStation(self.time_step)
self.station.SetupDefaultStation()
self.plant = self.station.get_mutable_multibody_plant()
if object_file_path is not None:
self.object = AddModelFromSdfFile(
file_name=object_file_path,
model_name="object",
plant=self.station.get_mutable_multibody_plant(),
scene_graph=self.station.get_mutable_scene_graph())
self.station.Finalize()
self.simulator = None
self.plan_runner = None
# Initial pose of the object
self.X_WObject = X_WObject
def __init__(self, time_step,
object_file_path=None,
object_base_link_name=None,
X_WObject=X_WObject_default,):
self.object_base_link_name = object_base_link_name
self.time_step = time_step
# Finalize manipulation station by adding manipuland.
self.station = ManipulationStation(self.time_step)
self.station.AddCupboard()
self.plant = self.station.get_mutable_multibody_plant()
if object_file_path is not None:
self.object = AddModelFromSdfFile(
file_name=object_file_path,
model_name="object",
plant=self.station.get_mutable_multibody_plant(),
scene_graph=self.station.get_mutable_scene_graph() )
self.station.Finalize()
self.tree = self.plant.tree()
self.simulator = None
self.plan_runner = None
# Initial pose of the object
self.X_WObject = X_WObject
# Frames
self.gripper_model = self.plant.GetModelInstanceByName("gripper")
self.world_frame = self.plant.world_frame()
self.gripper_frame = self.plant.GetFrameByName("body", self.gripper_model)
def __init__(self, q_initial, p_goal,
object_file_paths=None,
object_base_link_names=None,
X_WObject_list=None):
"""
:param q_initial: initial config of the robot
:param p_goal: goal point in World space
:param object_file_paths: a list of objects in the world, each a sdf file path
:param object_base_link_names: a list of object base link names
:param X_WObject_list: a list of initial pose of the objects
"""
self.q_initial=q_initial
self.p_goal=p_goal
# Set up station
self.station = ManipulationStation(collision_model=IiwaCollisionModel.kBoxCollision)
self.station.AddCupboard()
self.plant = self.station.get_mutable_multibody_plant()
# Add objects into world
self.objects = []
if object_file_paths is not None:
for i, file_path in enumerate(object_file_paths):
self.objects.append(AddModelFromSdfFile(
file_name=file_path,
model_name="object_{}".format(i),
plant=self.plant,
scene_graph=self.station.get_mutable_scene_graph()
))
self.station.Finalize()
# Object base link names for reference
self.object_base_link_names = object_base_link_names
# Initial pose of the object
self.X_WObject_list = X_WObject_list
# Set up util module
self.util = Util(self.station)
# Set up configuration space
joint_ranges = self.util.get_joint_ranges()
self.cspace = ConfigurationSpace(joint_ranges, self.util)
# Set initial pose of the objects
if self.object_base_link_names is not None:
for link_name, object, X_WObject in zip(self.object_base_link_names,
self.objects,
self.X_WObject_list):
self.util.set_object_pose(link_name, object, X_WObject)
# Set robot initial pose
self.util.set_iiwa_position(self.q_initial)
self.util.set_wsg_position(0.1)
# book keeping
self.rewires = 0
self.edge_evaluation = 0
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 build_station_simulation(builder):
station = builder.AddSystem(ManipulationStation())
station.AddCupboard()
object_file_path = "drake/examples/manipulation_station/models/061_foam_brick.sdf"
brick = AddModelFromSdfFile(FindResourceOrThrow(object_file_path),
station.get_mutable_multibody_plant(),
station.get_mutable_scene_graph())
station.Finalize()
return station, brick
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)
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_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 = AddModelFromSdfFile(
file_name=file_name, plant=plant, scene_graph=None)
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.HasJointNamed(name="ShoulderJoint"))
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._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._test_frame_api(plant.GetFrameByName(name="Link1"))
self.assertIs(
plant.GetFrameByName(name="Link1"),
plant.GetFrameByName(name="Link1", model_instance=model_instance))
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)
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_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 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_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()
instances = []
for i in xrange(num_joints + 1):
instance = AddModelFromSdfFile(instance_file,
"instance_{}".format(i), plant)
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 __init__(self,
time_step,
object_file_path,
object_base_link_name,
is_hardware=False):
self.object_base_link_name = object_base_link_name
self.time_step = time_step
self.is_hardware = is_hardware
# Finalize manipulation station by adding manipuland.
self.station = ManipulationStation(self.time_step)
self.station.AddCupboard()
self.plant = self.station.get_mutable_multibody_plant()
self.object = AddModelFromSdfFile(
file_name=object_file_path,
model_name="object",
plant=self.station.get_mutable_multibody_plant(),
scene_graph=self.station.get_mutable_scene_graph())
self.station.Finalize()
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 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 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, 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()
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)
def __init__(self,
time_step,
object_file_path,
object_base_link_name,
is_hardware=False):
self.object_base_link_name = object_base_link_name
self.time_step = time_step
self.is_hardware = is_hardware
# Finalize manipulation station by adding manipuland.
self.station = ManipulationStation(self.time_step)
self.station.AddCupboard()
self.plant = self.station.get_mutable_multibody_plant()
self.object = AddModelFromSdfFile(
file_name=object_file_path,
model_name="object",
plant=self.station.get_mutable_multibody_plant(),
scene_graph=self.station.get_mutable_scene_graph())
self.station.Finalize()
# Initial pose of the object
X_WObject = Isometry3.Identity()
X_WObject.set_translation([.6, 0, 0])
self.X_WObject = X_WObject
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()
diagram.set_name("graphviz_example")
builder = DiagramBuilder()
if args.hardware:
# TODO(russt): Replace this hard-coded camera serial number with a config
# file.
camera_ids = ["805212060544"]
station = builder.AddSystem(
ManipulationStationHardwareInterface(camera_ids))
station.Connect(wait_for_cameras=False)
else:
station = builder.AddSystem(ManipulationStation())
station.AddCupboard()
object = AddModelFromSdfFile(
FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf"),
"object", station.get_mutable_multibody_plant(),
station.get_mutable_scene_graph())
station.Finalize()
ConnectDrakeVisualizer(builder, station.get_scene_graph(),
station.GetOutputPort("pose_bundle"))
teleop = builder.AddSystem(
JointSliders(station.get_controller_plant(), length=800))
if args.test:
teleop.window.withdraw() # Don't display the window when testing.
builder.Connect(teleop.get_output_port(0),
station.GetInputPort("iiwa_position"))
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):
sliders.set_position(args.position)
elif len(args.joint_position):
if use_hardware:
camera_ids = [
pc_to_pose.camera_configs["left_camera_serial"],
pc_to_pose.camera_configs["middle_camera_serial"],
pc_to_pose.camera_configs["right_camera_serial"]]
station = builder.AddSystem(
ManipulationStationHardwareInterface(camera_ids))
station.Connect()
else:
station = builder.AddSystem(ManipulationStation())
station.AddCupboard()
object_file_path = \
"drake/examples/manipulation_station/models/061_foam_brick.sdf"
brick = AddModelFromSdfFile(
FindResourceOrThrow(object_file_path),
station.get_mutable_multibody_plant(),
station.get_mutable_scene_graph() )
station.Finalize()
# add systems to convert the depth images from ManipulationStation to
# PointClouds
left_camera_info = pc_to_pose.camera_configs["left_camera_info"]
middle_camera_info = pc_to_pose.camera_configs["middle_camera_info"]
right_camera_info = pc_to_pose.camera_configs["right_camera_info"]
left_dut = builder.AddSystem(
mut.DepthImageToPointCloud(camera_info=left_camera_info))
middle_dut = builder.AddSystem(
mut.DepthImageToPointCloud(camera_info=middle_camera_info))
right_dut = builder.AddSystem(
mut.DepthImageToPointCloud(camera_info=right_camera_info))
