def construct_environment(masses: typing.List, box_sizes: typing.List):
"""
Construct an environment with many free boxes.
@param masses masses[i] is the mass of box i.
@param box_sizes box_sizes[i] is the size of box i.
"""
builder = DiagramBuilder_[AutoDiffXd]()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
# Add the ground as a big box.
ground_box = plant.AddRigidBody(
"ground", SpatialInertia(1, np.array([0, 0, 0]), UnitInertia(1, 1, 1)))
X_WG = RigidTransform([0, 0, -0.05])
ground_geometry_id = plant.RegisterCollisionGeometry(
ground_box, RigidTransform(), Box(10, 10, 0.1), "ground",
CoulombFriction(0.9, 0.8))
plant.RegisterVisualGeometry(
ground_box, RigidTransform(), Box(10, 10, 0.1), "ground",
[0.5, 0.5, 0.5, 1.])
plant.WeldFrames(plant.world_frame(), ground_box.body_frame(), X_WG)
# Add boxes
assert isinstance(masses, list)
assert isinstance(box_sizes, list)
assert len(masses) == len(box_sizes)
num_boxes = len(masses)
boxes = []
boxes_geometry_id = []
for i in range(num_boxes):
box_name = f"box{i}"
boxes.append(plant.AddRigidBody(
box_name, SpatialInertia(
masses[i], np.array([0, 0, 0]), UnitInertia(1, 1, 1))))
box_shape = Box(box_sizes[i][0], box_sizes[i][1], box_sizes[i][2])
boxes_geometry_id.append(plant.RegisterCollisionGeometry(
boxes[i], RigidTransform(), box_shape, f"{box_name}_box",
CoulombFriction(0.9, 0.8)))
plant.RegisterVisualGeometry(
ground_box, RigidTransform(), box_shape, f"{box_name}_box",
[0.5, 0.5, 0.5, 1.])
# Add small spheres at the corners of the box.
vertices = np.array([
[1, 1, 1], [1, 1, -1], [1, -1, 1], [1, -1, -1], [-1, 1, 1],
[-1, -1, 1], [-1, 1, -1], [-1, -1, -1]]) *\
np.tile(box_sizes[i]/2, (8, 1))
sphere_shape = Sphere(0.001)
for j in range(8):
plant.RegisterCollisionGeometry(
boxes[i], RigidTransform(vertices[j]), sphere_shape,
f"{box_name}_sphere{j}", CoulombFriction(0.9, 0.8))
plant.RegisterVisualGeometry(
boxes[i], RigidTransform(vertices[j]), sphere_shape,
f"{box_name}_sphere{j}", [0.5, 0.5, 0.5, 1])
plant.Finalize()
diagram = builder.Build()
return Environment(
plant=plant, scene_graph=scene_graph, diagram=diagram, boxes=boxes,
ground_geometry_id=ground_geometry_id,
boxes_geometry_id=boxes_geometry_id)
def add_rope_and_ground(self, include_ground=True):
if include_ground:
ground_model = add_ground(self.mbp, self.sg)
self._model_ids["ground"] = ground_model
for rope_name, rope_config in iteritems(self._config['env']['ropes']):
X_W = transform_from_dict(rope_config)
rope_model = add_rope(self.mbp, self.sg, self._config['rope'], X_W, rope_name)
self._model_names_to_mask.append(rope_name)
self._model_ids[rope_name] = rope_model
link_length = np.linalg.norm(np.subtract(self._config["env"]["ropes"]["rope_2"]["pos"], self._config["env"]["ropes"]["rope_1"]["pos"]))
lx, ly, lz = self._config["env"]["ropes"]["rope_1"]["pos"]
rx, ry, rz = self._config["env"]["ropes"]["rope_2"]["pos"]
# TODO: not completely right
roll = math.pi / 2 + math.atan2(rz - lz, ry - ly)
pitch = 0
yaw = -math.atan2(rx - lx, ry - ly)
link_I = SpatialInertia(mass=1,
p_PScm_E=np.array([0, 0, 0]),
G_SP_E=UnitInertia(1, 1, 1))
link_name = "middle_link"
link_body = self._mbp.AddRigidBody(link_name, rope_model, link_I)
cylinder_geom = Cylinder(self._config["rope"]["rope_radius"], link_length)
X = RigidTransform()
self._mbp.RegisterVisualGeometry(link_body, X, cylinder_geom,
"middle_vis1", [0.5, 0.5, 0.5, 0.5])
self._mbp.RegisterCollisionGeometry(link_body, X, cylinder_geom,
"middle_collision",
CoulombFriction(0.0, 0.0))
self._mbp.WeldFrames(self._mbp.world_frame(), self._mbp.GetFrameByName(f"middle_link", rope_model), RigidTransform(RollPitchYaw(roll, pitch, yaw), [(lx + rx) / 2.0, (ly + ry) / 2.0, (lz + rz) / 2.0]))
def test_procedural_geometry():
"""
This test ensures we can draw procedurally added primitive
geometry that is added to the world model instance (which has
a slightly different naming scheme than geometry with a
non-default / non-world model instance).
"""
builder = DiagramBuilder()
mbp, scene_graph = AddMultibodyPlantSceneGraph(builder)
world_body = mbp.world_body()
box_shape = Box(1., 2., 3.)
# This rigid body will be added to the world model instance since
# the model instance is not specified.
box_body = mbp.AddRigidBody(
"box",
SpatialInertia(mass=1.0,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia(1.0, 1.0, 1.0)))
mbp.WeldFrames(world_body.body_frame(), box_body.body_frame(),
RigidTransform())
mbp.RegisterVisualGeometry(box_body, RigidTransform.Identity(), box_shape,
"ground_vis", np.array([0.5, 0.5, 0.5, 1.]))
mbp.RegisterCollisionGeometry(box_body, RigidTransform.Identity(),
box_shape, "ground_col",
CoulombFriction(0.9, 0.8))
mbp.Finalize()
# frames_to_draw = {"world": {"box"}}
# visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph))
# builder.Connect(scene_graph.get_pose_bundle_output_port(),
# visualizer.get_input_port(0))
diagram = builder.Build()
def random_geometry(body):
# Creates a random geometry.
i = i_next()
box = Box(
width=random.uniform(0.1, 0.3),
depth=random.uniform(0.1, 0.3),
height=random.uniform(0.1, 0.3),
)
plant.RegisterVisualGeometry(
body=body,
X_BG=random_X(),
shape=box,
name=f"visual_{i}",
diffuse_color=[random.random(), 0, 0, 0.75],
)
static_friction = random.uniform(0.1, 1.)
plant.RegisterCollisionGeometry(body=body,
X_BG=random_X(),
shape=box,
name=f"collision_{i}",
coulomb_friction=CoulombFriction(
static_friction=static_friction,
dynamic_friction=static_friction /
2,
))
def test_procedural_geometry(self):
"""
This test ensures we can draw procedurally added primitive
geometry that is added to the world model instance (which has
a slightly different naming scheme than geometry with a
non-default / non-world model instance).
"""
builder = DiagramBuilder()
mbp, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
world_body = mbp.world_body()
box_shape = Box(1., 2., 3.)
# This rigid body will be added to the world model instance since
# the model instance is not specified.
box_body = mbp.AddRigidBody(
"box",
SpatialInertia(mass=1.0,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia(1.0, 1.0, 1.0)))
mbp.WeldFrames(world_body.body_frame(), box_body.body_frame(),
RigidTransform())
mbp.RegisterVisualGeometry(box_body, RigidTransform.Identity(),
box_shape, "ground_vis",
np.array([0.5, 0.5, 0.5, 1.]))
mbp.RegisterCollisionGeometry(box_body, RigidTransform.Identity(),
box_shape, "ground_col",
CoulombFriction(0.9, 0.8))
mbp.Finalize()
frames_to_draw = {"world": {"box"}}
visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
vis_context = diagram.GetMutableSubsystemContext(
visualizer, diagram_context)
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.AdvanceTo(.1)
visualizer.draw(vis_context)
self.assertEqual(
visualizer.ax.get_title(),
"t = 0.1",
)
def test_multibody_plant_construction_api(self):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
spatial_inertia = SpatialInertia()
body = plant.AddRigidBody(name="new_body",
M_BBo_B=spatial_inertia)
box = Box(width=0.5, depth=1.0, height=2.0)
body_X_BG = RigidTransform()
body_friction = CoulombFriction(static_friction=0.6,
dynamic_friction=0.5)
plant.RegisterVisualGeometry(
body=body, X_BG=body_X_BG, shape=box, name="new_body_visual",
diffuse_color=[1., 0.64, 0.0, 0.5])
plant.RegisterCollisionGeometry(
body=body, X_BG=body_X_BG, shape=box, name="new_body_collision",
coulomb_friction=body_friction)
def add_box_at_location(mbp,
name,
color,
pose,
mass=0.25,
inertia=UnitInertia(3E-3, 3E-3, 3E-3)):
''' Adds a 5cm cube at the specified pose. Uses a planar floating base
in the x-z plane. '''
no_mass_no_inertia = SpatialInertia(mass=0.,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia(0., 0., 0.))
body_mass_and_inertia = SpatialInertia(mass=mass,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=inertia)
shape = Box(0.05, 0.05, 0.05)
model_instance = mbp.AddModelInstance(name)
body = mbp.AddRigidBody(name, model_instance, body_mass_and_inertia)
RegisterVisualAndCollisionGeometry(mbp, body, RigidTransform(), shape,
name, color, CoulombFriction(0.9, 0.8))
body_pre_z = mbp.AddRigidBody("{}_pre_z".format(name), model_instance,
no_mass_no_inertia)
body_pre_theta = mbp.AddRigidBody("{}_pre_theta".format(name),
model_instance, no_mass_no_inertia)
world_carrot_origin = mbp.AddFrame(frame=FixedOffsetFrame(
name="world_{}_origin".format(name), P=mbp.world_frame(), X_PF=pose))
body_joint_x = PrismaticJoint(name="{}_x".format(name),
frame_on_parent=world_carrot_origin,
frame_on_child=body_pre_z.body_frame(),
axis=[1, 0, 0],
damping=0.)
mbp.AddJoint(body_joint_x)
body_joint_z = PrismaticJoint(name="{}_z".format(name),
frame_on_parent=body_pre_z.body_frame(),
frame_on_child=body_pre_theta.body_frame(),
axis=[0, 0, 1],
damping=0.)
mbp.AddJoint(body_joint_z)
body_joint_theta = RevoluteJoint(
name="{}_theta".format(name),
frame_on_parent=body_pre_theta.body_frame(),
frame_on_child=body.body_frame(),
axis=[0, 1, 0],
damping=0.)
mbp.AddJoint(body_joint_theta)
def make_box(mbp, name):
inertia = SpatialInertia.MakeFromCentralInertia(
1, [0, 0, 0], RotationalInertia(1 / 600, 1 / 120, 1 / 120))
body = mbp.AddRigidBody(name, inertia)
shape = Box(1, 0.1, 0.1)
mbp.RegisterVisualGeometry(body=body,
X_BG=RigidTransform(),
shape=shape,
name=f"{name}_visual",
diffuse_color=[1., 0.64, 0.0, 0.5])
body_friction = CoulombFriction(static_friction=0.6, dynamic_friction=0.5)
mbp.RegisterCollisionGeometry(body=body,
X_BG=RigidTransform(),
shape=shape,
name="{name}_collision",
coulomb_friction=body_friction)
return body
def load_atlas(plant, add_ground=False):
atlas_file = FindResourceOrThrow("drake/examples/atlas/urdf/atlas_minimal_contact.urdf")
# atlas_file = FindResourceOrThrow("drake/examples/atlas/urdf/atlas_convex_hull.urdf")
atlas = Parser(plant).AddModelFromFile(atlas_file)
if add_ground:
static_friction = 1.0
green = np.array([0.5, 1.0, 0.5, 1.0])
# plant.RegisterVisualGeometry(plant.world_body(), RigidTransform(), HalfSpace(),
# "GroundVisuaGeometry", green)
ground_friction = CoulombFriction(1.0, 1.0)
plant.RegisterCollisionGeometry(plant.world_body(), RigidTransform(), HalfSpace(),
"GroundCollisionGeometry", ground_friction)
plant.Finalize()
plant.set_penetration_allowance(1.0e-3)
plant.set_stiction_tolerance(1.0e-3)
def test_procedural_geometry_deprecated_api(self):
"""
This test ensures we can draw procedurally added primitive
geometry that is added to the world model instance (which has
a slightly different naming scheme than geometry with a
non-default / non-world model instance).
"""
builder = DiagramBuilder()
mbp, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
world_body = mbp.world_body()
box_shape = Box(1., 2., 3.)
# This rigid body will be added to the world model instance since
# the model instance is not specified.
box_body = mbp.AddRigidBody(
"box",
SpatialInertia(mass=1.0,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia(1.0, 1.0, 1.0)))
mbp.WeldFrames(world_body.body_frame(), box_body.body_frame(),
RigidTransform())
mbp.RegisterVisualGeometry(box_body, RigidTransform.Identity(),
box_shape, "ground_vis",
np.array([0.5, 0.5, 0.5, 1.]))
mbp.RegisterCollisionGeometry(box_body, RigidTransform.Identity(),
box_shape, "ground_col",
CoulombFriction(0.9, 0.8))
mbp.Finalize()
frames_to_draw = {"world": {"box"}}
visualizer = builder.AddSystem(
MeshcatVisualizer(scene_graph,
zmq_url=ZMQ_URL,
open_browser=False,
frames_to_draw=frames_to_draw))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_publish_every_time_step(False)
with catch_drake_warnings(expected_count=1):
simulator.AdvanceTo(.1)
def add_procedurally_generated_table(self):
mbp = self._mbp
dims = self._config['env']['table']['size']
box_shape = Box(*dims)
T_W_B = 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(), T_W_B)
color = np.array(self._config['env']['table']['color'])
mbp.RegisterVisualGeometry(
box_body, RigidTransform(), box_shape, "table_vis", color)
friction_params = self._config['env']['table']['coulomb_friction']
mbp.RegisterCollisionGeometry(
box_body, RigidTransform(), box_shape, "table_collision",
CoulombFriction(*friction_params))
def add_procedurally_generated_table(
mbp, # multi-body plant
table_config, # dict
):
"""
Adds a procedurally generated table to the scene
param table_config: dict with keys ['size', 'color']
"""
world_body = mbp.world_body()
dims = table_config['size']
# box_shape = Box(1., 2., 3.)
box_shape = Box(*dims)
translation = np.zeros(3)
translation[2] = -dims[2] / 2.0
T_W_B = RigidTransform(p=translation)
# 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(world_body.body_frame(), box_body.body_frame(), T_W_B)
# this is a grey color
color = np.array(table_config['color'])
mbp.RegisterVisualGeometry(box_body, RigidTransform.Identity(), box_shape,
"table_vis", color)
# friction
friction_params = table_config['coulomb_friction']
mbp.RegisterCollisionGeometry(box_body, RigidTransform.Identity(),
box_shape, "table_collision",
CoulombFriction(*friction_params))
def test_exploding_iiwa_sim(self):
"""
Shows a simulation of a falling + exploding IIWA which "changes
topology" by being rebuilt without joints.
"""
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder,
time_step=1e-3)
iiwa_file = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/urdf/"
"iiwa14_spheres_dense_elbow_collision.urdf")
Parser(plant).AddModelFromFile(iiwa_file, "iiwa")
# Add ground plane.
X_FH = HalfSpace.MakePose([0, 0, 1], [0, 0, 0])
plant.RegisterCollisionGeometry(plant.world_body(), X_FH, HalfSpace(),
"ground_plane_collision",
CoulombFriction(0.8, 0.3))
plant.Finalize()
# Loosey-goosey - no control.
for model in me.get_model_instances(plant):
util.build_with_no_control(builder, plant, model)
if VISUALIZE:
print("test_exploding_iiwa_sim")
role = Role.kIllustration
DrakeVisualizer.AddToBuilder(
builder, scene_graph, params=DrakeVisualizerParams(role=role))
ConnectContactResultsToDrakeVisualizer(builder, plant, scene_graph)
diagram = builder.Build()
# Set up context.
d_context = diagram.CreateDefaultContext()
context = plant.GetMyContextFromRoot(d_context)
# - Hoist IIWA up in the air.
plant.SetFreeBodyPose(context, plant.GetBodyByName("base"),
RigidTransform([0, 0, 1.]))
# - Set joint velocities to "spin" it in the air.
for joint in me.get_joints(plant):
if isinstance(joint, RevoluteJoint):
me.set_joint_positions(plant, context, joint, 0.7)
me.set_joint_velocities(plant, context, joint, -5.)
def monitor(d_context):
# Stop the simulation once there's any contact.
context = plant.GetMyContextFromRoot(d_context)
query_object = plant.get_geometry_query_input_port().Eval(context)
if query_object.HasCollisions():
return EventStatus.ReachedTermination(plant, "Contact")
else:
return EventStatus.DidNothing()
# Forward simulate.
simulator = Simulator(diagram, d_context)
simulator.Initialize()
simulator.set_monitor(monitor)
if VISUALIZE:
simulator.set_target_realtime_rate(1.)
simulator.AdvanceTo(2.)
# Try to push a bit further.
simulator.clear_monitor()
simulator.AdvanceTo(d_context.get_time() + 0.05)
diagram.Publish(d_context)
# Recreate simulator.
builder_new = DiagramBuilder()
plant_new, scene_graph_new = AddMultibodyPlantSceneGraph(
builder_new, time_step=plant.time_step())
subgraph = mut.MultibodyPlantSubgraph(
mut.get_elements_from_plant(plant, scene_graph))
# Remove all joints; make them floating bodies.
for joint in me.get_joints(plant):
subgraph.remove_joint(joint)
# Remove massless bodies.
# For more info, see: https://stackoverflow.com/a/62035705/7829525
for body in me.get_bodies(plant):
if body is plant.world_body():
continue
if body.default_mass() == 0.:
subgraph.remove_body(body)
# Finalize.
to_new = subgraph.add_to(plant_new, scene_graph_new)
plant_new.Finalize()
if VISUALIZE:
DrakeVisualizer.AddToBuilder(
builder_new,
scene_graph_new,
params=DrakeVisualizerParams(role=role))
ConnectContactResultsToDrakeVisualizer(builder_new, plant_new,
scene_graph_new)
diagram_new = builder_new.Build()
# Remap state.
d_context_new = diagram_new.CreateDefaultContext()
d_context_new.SetTime(d_context.get_time())
context_new = plant_new.GetMyContextFromRoot(d_context_new)
to_new.copy_state(context, context_new)
# Simulate.
simulator_new = Simulator(diagram_new, d_context_new)
simulator_new.Initialize()
diagram_new.Publish(d_context_new)
if VISUALIZE:
simulator_new.set_target_realtime_rate(1.)
simulator_new.AdvanceTo(context_new.get_time() + 2)
if VISUALIZE:
input(" Press enter...")
def test_friction_api(self):
CoulombFriction(static_friction=0.7, dynamic_friction=0.6)
def add_pusher(self):
"""
Adds a cylindrical pusher object
:return:
:rtype:
"""
mbp = self.mbp
parser = self.parser
pusher_model_idx = parser.AddModelFromFile(paths.xy_slide, "pusher")
base_link = mbp.GetBodyByName("base", pusher_model_idx)
# weld it to the world
mbp.WeldFrames(mbp.world_frame(), base_link.body_frame())
self.models['pusher'] = pusher_model_idx
radius = 0.01
length = 0.1
pusher_shape = Cylinder(radius, length)
# This rigid body will be added to the pusher model instance
world_body = mbp.world_body()
pusher_body = mbp.AddRigidBody(
"pusher_body", pusher_model_idx,
SpatialInertia(mass=10.0,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia(1.0, 1.0, 1.0)))
self._rigid_bodies['pusher'] = pusher_body
# weld it to EE frame at particular height
translation = np.zeros(3)
translation[
2] = length / 2.0 + 0.001 # give it a little room for collision stuff
T_EE_P = RigidTransform(p=translation)
ee_link = mbp.GetBodyByName("end_effector", pusher_model_idx)
mbp.WeldFrames(ee_link.body_frame(), pusher_body.body_frame(), T_EE_P)
# color it green
color = np.array([0., 1., 0., 1.])
mbp.RegisterVisualGeometry(pusher_body, RigidTransform.Identity(),
pusher_shape, "pusher_vis", color)
mbp.RegisterCollisionGeometry(pusher_body, RigidTransform.Identity(),
pusher_shape, "pusher_collision",
CoulombFriction(0.9, 0.8))
def export_port_func():
# export relevant ports
actuation_input_port = mbp.get_actuation_input_port(
pusher_model_idx)
state_output_port = mbp.get_state_output_port(pusher_model_idx)
self._port_names["pusher_state_output"] = "pusher_state_output"
self._port_names[
"pusher_actuation_input"] = "pusher_actuation_input"
self.builder.ExportOutput(state_output_port,
self._port_names["pusher_state_output"])
self.builder.ExportInput(
actuation_input_port,
self._port_names["pusher_actuation_input"])
self._export_port_functions.append(export_port_func)
# Add "tabletop" (ground)
world_body = mbp.world_body()
ground_shape = Box(1., 1., 1.)
ground_body = mbp.AddRigidBody(
"ground",
SpatialInertia(mass=10.0,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia(1.0, 1.0, 1.0)))
mbp.WeldFrames(world_body.body_frame(), ground_body.body_frame(),
RigidTransform())
RegisterVisualAndCollisionGeometry(mbp, ground_body,
RigidTransform(p=[0, 0, -0.5]),
ground_shape, "ground",
np.array([0.5, 0.5, 0.5, 1.]),
CoulombFriction(0.9, 0.8))
ycb_object_dir = os.path.join(getDrakePath(),
"manipulation/models/ycb/sdf/")
ycb_object_sdfs = os.listdir(ycb_object_dir)
ycb_object_sdfs = [
os.path.join(ycb_object_dir, path) for path in ycb_object_sdfs
]
# Add random objects
parser = Parser(mbp, scene_graph)
n_objects = np.random.randint(1, 12)
obj_ids = []
for k in range(n_objects):
obj_i = np.random.randint(len(ycb_object_sdfs))
parser.AddModelFromFile(file_name=ycb_object_sdfs[obj_i],
def visualize(theta1, theta2, theta3, theta4=0.0, phi=0.0):
file_name = "res/stair_climb.sdf"
builder = DiagramBuilder()
stair_climb, scene_graph = AddMultibodyPlantSceneGraph(builder)
# stair_climb.RegisterAsSourceForSceneGraph(scene_graph)
Parser(plant=stair_climb).AddModelFromFile(file_name)
# stair_climb.AddForceElement(UniformGravityFieldElement())
front_wheel_x, front_wheel_y = eom.findFrontWheelPosition(
theta1, theta2, theta3)
front_wheel_x = front_wheel_x.Evaluate()
front_wheel_y = front_wheel_y.Evaluate()
step = Box(STEP_DEPTH, STEP_WIDTH, STEP_HEIGHT)
step_pos = RigidTransform(
[STEP_POSITION + STEP_DEPTH / 2.0, 0.0, STEP_HEIGHT / 2.0])
stair_climb.RegisterCollisionGeometry(
stair_climb.world_body(), RigidTransform([0.0, 0.0, 0.0]), HalfSpace(),
"GroundCollision", CoulombFriction(COEFF_FRICTION, COEFF_FRICTION))
stair_climb.RegisterVisualGeometry(stair_climb.world_body(),
RigidTransform([0.0, 0.0, 0.0]),
HalfSpace(), "GroundVisual",
np.array([0.5, 0.5, 0.5, 0.5])) # Color
stair_climb.RegisterCollisionGeometry(
stair_climb.world_body(), step_pos, step, "StepCollision",
CoulombFriction(COEFF_FRICTION, COEFF_FRICTION))
stair_climb.RegisterVisualGeometry(stair_climb.world_body(), step_pos,
step, "StepVisual",
np.array([1.0, 1.0, 0.0, 1.0])) # Color
stair_climb.Finalize()
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
stair_climb_context = diagram.GetMutableSubsystemContext(
stair_climb, diagram_context)
stair_climb_context.FixInputPort(
stair_climb.get_actuation_input_port().get_index(), [0, 0, 0, 0, 0])
theta1_joint = stair_climb.GetJointByName("theta1")
theta2_joint = stair_climb.GetJointByName("theta2")
theta3_joint = stair_climb.GetJointByName("theta3")
theta4_joint = stair_climb.GetJointByName("theta4")
phi_joint = stair_climb.GetJointByName("phi")
theta1_joint.set_angle(context=stair_climb_context, angle=theta1)
theta2_joint.set_angle(context=stair_climb_context, angle=theta2)
theta3_joint.set_angle(context=stair_climb_context, angle=theta3)
theta4_joint.set_angle(context=stair_climb_context, angle=theta4)
phi_joint.set_angle(context=stair_climb_context, angle=phi)
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(0.001)
simulator.Initialize()
simulator.AdvanceTo(0.0)
def test_proximity_properties(self):
"""
Tests the utility functions (not related to hydroelastic contact) for
setting values in ProximityProperties (as defined in
proximity_properties.h).
"""
props = mut.ProximityProperties()
reference_friction = CoulombFriction(0.25, 0.125)
mut.AddContactMaterial(dissipation=2.7,
point_stiffness=3.9,
friction=reference_friction,
properties=props)
self.assertTrue(
props.HasProperty("material", "hunt_crossley_dissipation"))
self.assertEqual(
props.GetProperty("material", "hunt_crossley_dissipation"), 2.7)
self.assertTrue(
props.HasProperty("material", "point_contact_stiffness"))
self.assertEqual(
props.GetProperty("material", "point_contact_stiffness"), 3.9)
self.assertTrue(props.HasProperty("material", "coulomb_friction"))
stored_friction = props.GetProperty("material", "coulomb_friction")
self.assertEqual(stored_friction.static_friction(),
reference_friction.static_friction())
self.assertEqual(stored_friction.dynamic_friction(),
reference_friction.dynamic_friction())
props = mut.ProximityProperties()
res_hint = 0.175
E = 1e8
mut.AddRigidHydroelasticProperties(resolution_hint=res_hint,
properties=props)
self.assertTrue(props.HasProperty("hydroelastic", "compliance_type"))
self.assertFalse(mut_testing.PropertiesIndicateCompliantHydro(props))
self.assertTrue(props.HasProperty("hydroelastic", "resolution_hint"))
self.assertEqual(props.GetProperty("hydroelastic", "resolution_hint"),
res_hint)
props = mut.ProximityProperties()
mut.AddRigidHydroelasticProperties(properties=props)
self.assertTrue(props.HasProperty("hydroelastic", "compliance_type"))
self.assertFalse(mut_testing.PropertiesIndicateCompliantHydro(props))
self.assertFalse(props.HasProperty("hydroelastic", "resolution_hint"))
props = mut.ProximityProperties()
res_hint = 0.275
mut.AddCompliantHydroelasticProperties(resolution_hint=res_hint,
hydroelastic_modulus=E,
properties=props)
self.assertTrue(props.HasProperty("hydroelastic", "compliance_type"))
self.assertTrue(mut_testing.PropertiesIndicateCompliantHydro(props))
self.assertTrue(props.HasProperty("hydroelastic", "resolution_hint"))
self.assertEqual(props.GetProperty("hydroelastic", "resolution_hint"),
res_hint)
self.assertTrue(
props.HasProperty("hydroelastic", "hydroelastic_modulus"))
self.assertEqual(
props.GetProperty("hydroelastic", "hydroelastic_modulus"), E)
props = mut.ProximityProperties()
slab_thickness = 0.275
mut.AddCompliantHydroelasticPropertiesForHalfSpace(
slab_thickness=slab_thickness,
hydroelastic_modulus=E,
properties=props)
self.assertTrue(props.HasProperty("hydroelastic", "compliance_type"))
self.assertTrue(mut_testing.PropertiesIndicateCompliantHydro(props))
self.assertTrue(props.HasProperty("hydroelastic", "slab_thickness"))
self.assertEqual(props.GetProperty("hydroelastic", "slab_thickness"),
slab_thickness)
self.assertTrue(
props.HasProperty("hydroelastic", "hydroelastic_modulus"))
self.assertEqual(
props.GetProperty("hydroelastic", "hydroelastic_modulus"), E)
def add_arbitrary_multibody_stuff(plant,
num_bodies=30,
slight_difference=False):
"""
Deterministic, physically valid, jumble of arbitrary stuff.
The goal of this factory is to:
- Produce a set of elements and cases that exercise each code path in
`MultibodyPlantSubgraph.add_to` and `MultibodyPlantElementsMap`.
- Ensure each element has somewhat "random" but unique properties for each
element produced.
- Allow for a slight difference to show that strict plant comparison can be
falsified.
"""
count = defaultdict(lambda: 0)
def i_next(key=None):
# Increments for a given key.
count[key] += 1
return count[key]
def maybe():
# Returns True or False randomly.
return random.choice([True, False])
def random_model_instance():
# Returns a "random" model instance (by incrementing).
i = i_next()
return plant.AddModelInstance(f"model_{i}")
def random_position():
# Returns a random position.
return [0.2 * random.random(), 0, 0]
def random_X():
# Returns a random pose.
return RigidTransform(random_position())
def random_body():
# Returns a random body, with an incrementing name.
inertia = SpatialInertia(
mass=random.uniform(0.2, 1.),
p_PScm_E=random_position(),
G_SP_E=UnitInertia(
Ixx=random.uniform(0.2, 0.3),
Iyy=random.uniform(0.2, 0.3),
Izz=random.uniform(0.2, 0.3),
),
)
return plant.AddRigidBody(
name=f"body_{i_next()}",
M_BBo_B=inertia,
model_instance=random_model_instance(),
)
def random_frame(parent_frame):
# Returns a random frame, with an incrementing name.
i = i_next()
return plant.AddFrame(
FixedOffsetFrame(
name=f"frame_{i}",
P=parent_frame,
X_PF=random_X(),
model_instance=parent_frame.model_instance(),
))
def random_joint(parent, child):
# Returns a random joint, but with an incrementing name. Note that we
# use a separate index so that we ensure we can loop through all
# joints.
i = i_next("joint")
name = f"joint_{i}"
joint_cls = JOINT_CLS_LIST[i % len(JOINT_CLS_LIST)]
frame_on_parent = random_frame(parent.body_frame())
frame_on_child = random_frame(child.body_frame())
axis = np.zeros(3)
axis[i_next() % 3] = 1
damping = random.random()
if joint_cls == BallRpyJoint:
joint = BallRpyJoint(
name,
frame_on_parent=frame_on_parent,
frame_on_child=frame_on_child,
damping=damping,
)
elif joint_cls == PrismaticJoint:
joint = PrismaticJoint(
name,
frame_on_parent=frame_on_parent,
frame_on_child=frame_on_child,
axis=axis,
damping=damping,
)
elif joint_cls == RevoluteJoint:
joint = RevoluteJoint(
name,
frame_on_parent=frame_on_parent,
frame_on_child=frame_on_child,
axis=axis,
damping=damping,
)
elif joint_cls == UniversalJoint:
joint = UniversalJoint(
name,
frame_on_parent=frame_on_parent,
frame_on_child=frame_on_child,
damping=damping,
)
elif joint_cls == WeldJoint:
joint = WeldJoint(
name,
frame_on_parent_P=frame_on_parent,
frame_on_child_C=frame_on_child,
X_PC=random_X(),
)
else:
assert False
return plant.AddJoint(joint)
def random_joint_actuator(joint):
# Creates a random joint actuator.
assert joint is not None
i = i_next()
return plant.AddJointActuator(f"actuator_{i}",
joint,
effort_limit=random.uniform(1, 2))
def random_geometry(body):
# Creates a random geometry.
i = i_next()
box = Box(
width=random.uniform(0.1, 0.3),
depth=random.uniform(0.1, 0.3),
height=random.uniform(0.1, 0.3),
)
plant.RegisterVisualGeometry(
body=body,
X_BG=random_X(),
shape=box,
name=f"visual_{i}",
diffuse_color=[random.random(), 0, 0, 0.75],
)
static_friction = random.uniform(0.1, 1.)
plant.RegisterCollisionGeometry(body=body,
X_BG=random_X(),
shape=box,
name=f"collision_{i}",
coulomb_friction=CoulombFriction(
static_friction=static_friction,
dynamic_friction=static_friction /
2,
))
# Add ground plane.
X_FH = HalfSpace.MakePose([0, 0, 1], [0, 0, 0])
plant.RegisterCollisionGeometry(plant.world_body(), X_FH, HalfSpace(),
"ground_plane_collision",
CoulombFriction(0.8, 0.3))
grid_rows = 5
prev_body = None
for i in range(num_bodies):
random.seed(i)
body = random_body()
grid_col = i % grid_rows
grid_row = int(i / grid_rows)
if slight_difference:
grid_row += 1
plant.SetDefaultFreeBodyPose(body,
RigidTransform([grid_col, grid_row, 2]))
random_frame(body.body_frame())
# Consider attaching a joint and/or frame to the world.
if maybe() or num_bodies < 3:
prev_body = plant.world_body()
random_frame(plant.world_frame())
if prev_body is not None and (maybe() or num_bodies < 3):
joint = random_joint(prev_body, body)
if joint.num_velocities() == 1 and (maybe() or num_bodies < 3):
random_joint_actuator(joint)
if plant.geometry_source_is_registered():
random_geometry(body)
prev_body = body
def do_main():
rospy.init_node('run_dishrack_interaction', anonymous=False)
#np.random.seed(42)
for outer_iter in range(200):
try:
builder = DiagramBuilder()
mbp, scene_graph = AddMultibodyPlantSceneGraph(
builder, MultibodyPlant(time_step=0.0025))
# Add ground
world_body = mbp.world_body()
ground_shape = Box(2., 2., 2.)
ground_body = mbp.AddRigidBody(
"ground",
SpatialInertia(mass=10.0,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia(1.0, 1.0, 1.0)))
mbp.WeldFrames(world_body.body_frame(), ground_body.body_frame(),
RigidTransform(p=[0, 0, -1]))
mbp.RegisterVisualGeometry(ground_body, RigidTransform.Identity(),
ground_shape, "ground_vis",
np.array([0.5, 0.5, 0.5, 1.]))
mbp.RegisterCollisionGeometry(ground_body,
RigidTransform.Identity(),
ground_shape, "ground_col",
CoulombFriction(0.9, 0.8))
parser = Parser(mbp, scene_graph)
dish_bin_model = "/home/gizatt/projects/scene_generation/models/dish_models/bus_tub_01_decomp/bus_tub_01_decomp.urdf"
cupboard_model = "/home/gizatt/projects/scene_generation/models/dish_models/shelf_two_levels.sdf"
candidate_model_files = {
#"mug": "/home/gizatt/drake/manipulation/models/mug/mug.urdf",
"mug_1":
"/home/gizatt/projects/scene_generation/models/dish_models/mug_1_decomp/mug_1_decomp.urdf",
#"plate_11in": "/home/gizatt/drake/manipulation/models/dish_models/plate_11in_decomp/plate_11in_decomp.urdf",
#"/home/gizatt/drake/manipulation/models/mug_big/mug_big.urdf",
#"/home/gizatt/drake/manipulation/models/dish_models/bowl_6p25in_decomp/bowl_6p25in_decomp.urdf",
#"/home/gizatt/drake/manipulation/models/dish_models/plate_8p5in_decomp/plate_8p5in_decomp.urdf",
}
# Decide how many of each object to add
max_num_objs = 6
num_objs = [
np.random.randint(0, max_num_objs)
for k in range(len(candidate_model_files.keys()))
]
# Actually produce their initial poses + add them to the sim
poses = [] # [quat, pos]
all_object_instances = []
all_manipulable_body_ids = []
total_num_objs = sum(num_objs)
object_ordering = list(range(total_num_objs))
k = 0
random.shuffle(object_ordering)
print("ordering: ", object_ordering)
for class_k, class_entry in enumerate(
candidate_model_files.items()):
for model_instance_k in range(num_objs[class_k]):
class_name, class_path = class_entry
model_name = "%s_%d" % (class_name, model_instance_k)
all_object_instances.append([class_name, model_name])
model_id = parser.AddModelFromFile(class_path,
model_name=model_name)
all_manipulable_body_ids += mbp.GetBodyIndices(model_id)
# Put them in a randomly ordered line, for placing
y_offset = (object_ordering[k] / float(total_num_objs) -
0.5) # RAnge -0.5 to 0.5
poses.append([
RollPitchYaw(np.random.uniform(
0., 2. * np.pi, size=3)).ToQuaternion().wxyz(),
[-0.25, y_offset, 0.1]
])
k += 1
#$poses.append([
# RollPitchYaw(np.random.uniform(0., 2.*np.pi, size=3)).ToQuaternion().wxyz(),
# [np.random.uniform(-0.2, 0.2), np.random.uniform(-0.1, 0.1), np.random.uniform(0.1, 0.3)]])
# Build a desk
parser.AddModelFromFile(cupboard_model)
mbp.WeldFrames(world_body.body_frame(),
mbp.GetBodyByName("shelf_origin_body").body_frame(),
RigidTransform(p=[0.0, 0, 0.0]))
#parser.AddModelFromFile(dish_bin_model)
#mbp.WeldFrames(world_body.body_frame(), mbp.GetBodyByName("bus_tub_01_decomp_body_link").body_frame(),
# RigidTransform(p=[0.0, 0., 0.], rpy=RollPitchYaw(np.pi/2., 0., 0.)))
mbp.AddForceElement(UniformGravityFieldElement())
mbp.Finalize()
hydra_sg_spy = builder.AddSystem(
HydraInteractionLeafSystem(
mbp,
scene_graph,
all_manipulable_body_ids=all_manipulable_body_ids))
#builder.Connect(scene_graph.get_query_output_port(),
# hydra_sg_spy.get_input_port(0))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
hydra_sg_spy.get_input_port(0))
builder.Connect(mbp.get_state_output_port(),
hydra_sg_spy.get_input_port(1))
builder.Connect(hydra_sg_spy.get_output_port(0),
mbp.get_applied_spatial_force_input_port())
visualizer = builder.AddSystem(
MeshcatVisualizer(scene_graph,
zmq_url="tcp://127.0.0.1:6000",
draw_period=0.01))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
mbp_context = diagram.GetMutableSubsystemContext(
mbp, diagram_context)
sg_context = diagram.GetMutableSubsystemContext(
scene_graph, diagram_context)
q0 = mbp.GetPositions(mbp_context).copy()
for k in range(len(poses)):
offset = k * 7
q0[(offset):(offset + 4)] = poses[k][0]
q0[(offset + 4):(offset + 7)] = poses[k][1]
mbp.SetPositions(mbp_context, q0)
simulator = Simulator(diagram, diagram_context)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
simulator.Initialize()
ik = InverseKinematics(mbp, mbp_context)
q_dec = ik.q()
prog = ik.prog()
def squaredNorm(x):
return np.array([x[0]**2 + x[1]**2 + x[2]**2 + x[3]**2])
for k in range(len(poses)):
# Quaternion norm
prog.AddConstraint(squaredNorm, [1], [1],
q_dec[(k * 7):(k * 7 + 4)])
# Trivial quaternion bounds
prog.AddBoundingBoxConstraint(-np.ones(4), np.ones(4),
q_dec[(k * 7):(k * 7 + 4)])
# Conservative bounds on on XYZ
prog.AddBoundingBoxConstraint(np.array([-2., -2., -2.]),
np.array([2., 2., 2.]),
q_dec[(k * 7 + 4):(k * 7 + 7)])
def vis_callback(x):
vis_diagram_context = diagram.CreateDefaultContext()
mbp.SetPositions(
diagram.GetMutableSubsystemContext(mbp,
vis_diagram_context), x)
pose_bundle = scene_graph.get_pose_bundle_output_port().Eval(
diagram.GetMutableSubsystemContext(scene_graph,
vis_diagram_context))
context = visualizer.CreateDefaultContext()
context.FixInputPort(0, AbstractValue.Make(pose_bundle))
visualizer.Publish(context)
prog.AddVisualizationCallback(vis_callback, q_dec)
prog.AddQuadraticErrorCost(np.eye(q0.shape[0]) * 1.0, q0, q_dec)
ik.AddMinimumDistanceConstraint(0.001, threshold_distance=1.0)
prog.SetInitialGuess(q_dec, q0)
print("Solving")
# print "Initial guess: ", q0
start_time = time.time()
solver = SnoptSolver()
#solver = NloptSolver()
sid = solver.solver_type()
# SNOPT
prog.SetSolverOption(sid, "Print file", "test.snopt")
prog.SetSolverOption(sid, "Major feasibility tolerance", 1e-3)
prog.SetSolverOption(sid, "Major optimality tolerance", 1e-2)
prog.SetSolverOption(sid, "Minor feasibility tolerance", 1e-3)
prog.SetSolverOption(sid, "Scale option", 0)
#prog.SetSolverOption(sid, "Elastic weight", 1e1)
#prog.SetSolverOption(sid, "Elastic mode", "Yes")
# NLOPT
#prog.SetSolverOption(sid, "initial_step", 0.1)
#prog.SetSolverOption(sid, "xtol_rel", 1E-2)
#prog.SetSolverOption(sid, "xtol_abs", 1E-2)
#prog.SetSolverOption(sid, "Major step limit", 2)
print("Solver opts: ", prog.GetSolverOptions(solver.solver_type()))
result = mp.Solve(prog)
print("Solve info: ", result)
print("Solved in %f seconds" % (time.time() - start_time))
#print(IpoptSolver().Solve(prog))
print(result.get_solver_id().name())
q0_proj = result.GetSolution(q_dec)
# print "Final: ", q0_proj
mbp.SetPositions(mbp_context, q0_proj)
simulator.StepTo(1000)
raise StopIteration()
except StopIteration:
print(colored("Stopped, saving and restarting", 'yellow'))
qf = mbp.GetPositions(mbp_context)
# Decide whether to accept: all objs within bounds
save = True
for k in range(len(all_object_instances)):
offset = k * 7
q = qf[offset:(offset + 7)]
if q[4] <= -0.25 or q[4] >= 0.25 or q[5] <= -0.2 or q[
5] >= 0.2 or q[6] <= -0.1:
save = False
break
if save:
print(colored("Saving", "green"))
save_config(all_object_instances, qf,
"mug_rack_environments_human.yaml")
else:
print(
colored("Not saving due to bounds violation: " + str(q),
"yellow"))
except Exception as e:
print(colored("Suffered other exception " + str(e), "red"))
sys.exit(-1)
except:
print(
colored("Suffered totally unknown exception! Probably sim.",
"red"))
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 do_exploding_iiwa_sim(self, plant_src, scene_graph_src, context_src):
# Show a simulation which "changes state" by being rebuilt.
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder,
time_step=1e-3)
subgraph_src = mut.MultibodyPlantSubgraph(
mut.get_elements_from_plant(plant_src, scene_graph_src))
to_plant = subgraph_src.add_to(plant, scene_graph)
# Add ground plane.
X_FH = HalfSpace.MakePose([0, 0, 1], [0, 0, 0])
plant.RegisterCollisionGeometry(plant.world_body(), X_FH, HalfSpace(),
"ground_plane_collision",
CoulombFriction(0.8, 0.3))
plant.Finalize()
# Loosey-goosey - no control.
for model in me.get_model_instances(plant):
util.no_control(builder, plant, model)
if VISUALIZE:
print("do_exploding_iiwa_sim")
role = Role.kIllustration
ConnectDrakeVisualizer(builder, scene_graph, role=role)
ConnectContactResultsToDrakeVisualizer(builder, plant)
diagram = builder.Build()
# Set up context.
d_context = diagram.CreateDefaultContext()
context = plant.GetMyContextFromRoot(d_context)
to_plant.copy_state(context_src, context)
# - Hoist IIWA up in the air.
plant.SetFreeBodyPose(context, plant.GetBodyByName("base"),
RigidTransform([0, 0, 1.]))
# - Set joint velocities to "spin" it in the air.
for joint in me.get_joints(plant):
if isinstance(joint, RevoluteJoint):
me.set_joint_positions(plant, context, joint, 0.7)
me.set_joint_velocities(plant, context, joint, -5.)
def monitor(d_context):
# Stop the simulation once there's any contact.
context = plant.GetMyContextFromRoot(d_context)
query_object = plant.get_geometry_query_input_port().Eval(context)
if query_object.HasCollisions():
return EventStatus.ReachedTermination(plant, "Contact")
else:
return EventStatus.DidNothing()
# Forward simulate.
simulator = Simulator(diagram, d_context)
simulator.Initialize()
simulator.set_monitor(monitor)
if VISUALIZE:
simulator.set_target_realtime_rate(1.)
simulator.AdvanceTo(2.)
# Try to push a bit further.
simulator.clear_monitor()
simulator.AdvanceTo(d_context.get_time() + 0.05)
diagram.Publish(d_context)
# Recreate simulator.
builder_new = DiagramBuilder()
plant_new, scene_graph_new = AddMultibodyPlantSceneGraph(
builder_new, time_step=plant.time_step())
subgraph = mut.MultibodyPlantSubgraph(
mut.get_elements_from_plant(plant, scene_graph))
# Remove all joints; make them floating bodies.
for joint in me.get_joints(plant):
subgraph.remove_joint(joint)
# Remove massless bodies.
for body in me.get_bodies(plant):
if body is plant.world_body():
continue
if body.default_mass() == 0.:
subgraph.remove_body(body)
# Finalize.
to_new = subgraph.add_to(plant_new, scene_graph_new)
plant_new.Finalize()
if VISUALIZE:
ConnectDrakeVisualizer(builder_new, scene_graph_new, role=role)
ConnectContactResultsToDrakeVisualizer(builder_new, plant_new)
diagram_new = builder_new.Build()
# Remap state.
d_context_new = diagram_new.CreateDefaultContext()
d_context_new.SetTime(d_context.get_time())
context_new = plant_new.GetMyContextFromRoot(d_context_new)
to_new.copy_state(context, context_new)
# Simulate.
simulator_new = Simulator(diagram_new, d_context_new)
simulator_new.Initialize()
diagram_new.Publish(d_context_new)
if VISUALIZE:
simulator_new.set_target_realtime_rate(1.)
simulator_new.AdvanceTo(context_new.get_time() + 2)
if VISUALIZE:
input(" Press enter...")
