def test_inertia_api(self):
UnitInertia()
unit_inertia = UnitInertia(Ixx=2.0, Iyy=2.3, Izz=2.4)
SpatialInertia()
SpatialInertia(mass=2.5,
p_PScm_E=[0.1, -0.2, 0.3],
G_SP_E=unit_inertia)
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 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 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 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 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 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 test_solve(self):
plant = MultibodyPlant(0)
M_AAo_A = SpatialInertia(1, np.zeros(3), UnitInertia(1, 1, 1))
body = plant.AddRigidBody("body", M_AAo_A)
plant.AddJoint(
PrismaticJoint("joint", plant.world_frame(), body.body_frame(),
[1, 0, 0]))
plant.Finalize()
path = PiecewisePolynomial.CubicWithContinuousSecondDerivatives(
[0., 1.], np.array([[0., 1.]]), [0.], [0.])
gridpoints = Toppra.CalcGridPoints(path, CalcGridPointsOptions())
toppra = Toppra(path=path, plant=plant, gridpoints=gridpoints)
toppra.AddJointAccelerationLimit([-1.], [1.])
trajectory = toppra.SolvePathParameterization()
self.assertIsInstance(trajectory, PiecewisePolynomial)
def scene_graph_with_mesh(filename):
builder = DiagramBuilder()
mbp, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
world_body = mbp.world_body()
mesh_shape = Mesh(filename)
mesh_body = mbp.AddRigidBody("mesh", 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(), mesh_body.body_frame(),
RigidTransform())
mbp.RegisterVisualGeometry(
mesh_body, RigidTransform.Identity(), mesh_shape, "mesh_vis",
np.array([0.5, 0.5, 0.5, 1.]))
mbp.Finalize()
return scene_graph
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_goal_region_visual_geometry(mbp, goal_position, goal_delta):
''' Adds a 5cm cube at the specified pose. Uses a planar floating base
in the x-z plane. '''
shape = Box(goal_delta, goal_delta, goal_delta)
no_mass_no_inertia = SpatialInertia(mass=0.,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia(0., 0., 0.))
shape = Sphere(0.05)
model_instance = mbp.AddModelInstance("goal_vis")
vis_origin_frame = mbp.AddFrame(
frame=FixedOffsetFrame(name="goal_vis_origin",
P=mbp.world_frame(),
X_PF=RigidTransform(
p=(goal_position +
np.array([0., 0.5, 0.])))))
body = mbp.AddRigidBody("goal_vis", model_instance, no_mass_no_inertia)
mbp.WeldFrames(vis_origin_frame, body.body_frame())
mbp.RegisterVisualGeometry(body, RigidTransform(), shape, "goal_vis",
[0.4, 0.9, 0.4, 0.35])
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 AddPedestal(plant):
"""
Creates the pedestal.
"""
pedestal_instance = plant.AddModelInstance("pedestal_base")
bodies = []
names = ["left", "right"]
x_positions = [
(PLYWOOD_LENGTH - THICK_PLYWOOD_THICKNESS)/2,
-(PLYWOOD_LENGTH - THICK_PLYWOOD_THICKNESS)/2
]
for name, x_position in zip(names, x_positions):
full_name = "pedestal_" + name + "_body"
body = plant.AddRigidBody(
full_name,
pedestal_instance,
SpatialInertia(mass=1, # Doesn't matter because it's welded
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia.SolidBox(
THICK_PLYWOOD_THICKNESS,
PEDESTAL_Y_DIM,
PEDESTAL_Z_DIM)
))
if plant.geometry_source_is_registered():
plant.RegisterCollisionGeometry(
body,
RigidTransform(),
pydrake.geometry.Box(
THICK_PLYWOOD_THICKNESS,
PEDESTAL_Y_DIM,
PLYWOOD_LENGTH
),
full_name,
pydrake.multibody.plant.CoulombFriction(1, 1)
)
plant.RegisterVisualGeometry(
body,
RigidTransform(),
pydrake.geometry.Box(
THICK_PLYWOOD_THICKNESS,
PEDESTAL_Y_DIM,
PLYWOOD_LENGTH
),
full_name,
[0.4, 0.4, 0.4, 1]) # RGBA color
plant.WeldFrames(
plant.world_frame(),
plant.GetFrameByName(full_name, pedestal_instance),
RigidTransform(RotationMatrix(), [
x_position,
0,
PLYWOOD_LENGTH/2+ bump_z
]
))
# Add bump at the bottom
full_name = "pedestal_bottom_body"
body = plant.AddRigidBody(
full_name,
pedestal_instance,
SpatialInertia(mass=1, # Doesn't matter because it's welded
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia.SolidBox(
PEDESTAL_X_DIM,
PEDESTAL_Y_DIM,
bump_z)
))
if plant.geometry_source_is_registered():
plant.RegisterCollisionGeometry(
body,
RigidTransform(),
pydrake.geometry.Box(
PEDESTAL_X_DIM,
PEDESTAL_Y_DIM,
bump_z
),
full_name,
pydrake.multibody.plant.CoulombFriction(1, 1)
)
plant.RegisterVisualGeometry(
body,
RigidTransform(),
pydrake.geometry.Box(
PEDESTAL_X_DIM,
PEDESTAL_Y_DIM,
bump_z
),
full_name,
[0.4, 0.4, 0.4, 1]) # RGBA color
plant.WeldFrames(
plant.world_frame(),
plant.GetFrameByName(full_name, pedestal_instance),
RigidTransform(RotationMatrix(), [
0,
0,
bump_z/2
]
))
return pedestal_instance
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)
def addSphere(plant, scene_graph=None):
"""Adds the manipulator."""
sphere = plant.AddModelInstance("sphere")
# Initialize sphere body
sphere_body = plant.AddRigidBody(
"sphere_body", sphere,
pydrake.multibody.tree.SpatialInertia(
mass=MASS,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=pydrake.multibody.tree.UnitInertia(1.0, 1.0, 1.0)))
# Initialize false bodies
empty_inertia = SpatialInertia(0, [0, 0, 0], UnitInertia(0, 0, 0))
for i in range(5):
# Add false bodies for control joints
false_body = plant.AddRigidBody("false_body{}".format(i), sphere,
empty_inertia)
# plant.WeldFrames(
# plant.world_frame(),
# false_body.body_frame(),
# RigidTransform()
# )
# Register geometry
if plant.geometry_source_is_registered():
col_geom = plant.RegisterCollisionGeometry(
sphere_body, RigidTransform(), pydrake.geometry.Sphere(RADIUS),
"sphere_body", pydrake.multibody.plant.CoulombFriction(1, 1))
plant.RegisterVisualGeometry(sphere_body, RigidTransform(),
pydrake.geometry.Sphere(RADIUS),
"sphere_body", [.9, .5, .5, 1.0]) # Color
# jnt = plant.AddJoint(pydrake.multibody.tree.Joint(
# "sphere_joint",
# plant.world_frame(),
# sphere_body))
# plant.AddJointActuator("sphere_actuator", jnt)
# Linear x control
sphere_x_translation = plant.AddJoint(
pydrake.multibody.tree.PrismaticJoint(
"sphere_x_translation", plant.world_frame(),
plant.GetFrameByName("false_body0"), [1, 0, 0], -1, 1))
plant.AddJointActuator("sphere_x_translation", sphere_x_translation)
sphere_x_translation.set_default_translation(0)
# Linear y control
sphere_y_translation = plant.AddJoint(
pydrake.multibody.tree.PrismaticJoint(
"sphere_y_translation", plant.GetFrameByName("false_body0"),
plant.GetFrameByName("false_body1"), [0, 1, 0], -1, 1))
plant.AddJointActuator("sphere_y_translation", sphere_y_translation)
sphere_y_translation.set_default_translation(0.2)
# Linear z control
sphere_z_translation = plant.AddJoint(
pydrake.multibody.tree.PrismaticJoint(
"sphere_z", plant.GetFrameByName("false_body1"),
plant.GetFrameByName("false_body2"), [0, 0, 1], -1, 1))
sphere_z_translation.set_default_translation(0.25)
plant.AddJointActuator("sphere_z", sphere_z_translation)
# Rotational x control
sphere_x_rotation = plant.AddJoint(
pydrake.multibody.tree.RevoluteJoint(
"sphere_x_rotation",
plant.GetFrameByName("false_body2"),
plant.GetFrameByName("false_body3"), [1, 0, 0],
damping=0))
sphere_x_rotation.set_default_angle(0)
plant.AddJointActuator("sphere_x_rotation", sphere_x_rotation)
# Rotational y control
sphere_y_rotation = plant.AddJoint(
pydrake.multibody.tree.RevoluteJoint(
"sphere_y_rotation",
plant.GetFrameByName("false_body3"),
plant.GetFrameByName("false_body4"), [0, 1, 0],
damping=0))
sphere_y_rotation.set_default_angle(0)
plant.AddJointActuator("sphere_y_rotation", sphere_y_rotation)
# Rotational z control
sphere_z_rotation = plant.AddJoint(
pydrake.multibody.tree.RevoluteJoint(
"sphere_z_rotation",
plant.GetFrameByName("false_body4"),
plant.GetFrameByName("sphere_body"), [0, 0, 1],
damping=0))
sphere_z_rotation.set_default_angle(0)
plant.AddJointActuator("sphere_z_rotation", sphere_z_rotation)
return sphere
if __name__ == "__main__":
np.random.seed(int(codecs.encode(os.urandom(4), 'hex'), 32) & (2**32 - 1))
random.seed(os.urandom(4))
for scene_k in range(25):
builder = DiagramBuilder()
mbp, scene_graph = AddMultibodyPlantSceneGraph(
builder, MultibodyPlant(time_step=0.005))
# 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
]
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_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"))
controller = builder.AddSystem(QuadrotorController(plant, y_traj, DURATION))
builder.Connect(controller.get_output_port(0), plant.get_input_port(0))
builder.Connect(plant.get_output_port(0), controller.get_input_port(0))
# Set up visualization and env in MeshCat
from pydrake.geometry import Box
from pydrake.common.eigen_geometry import Isometry3
from pydrake.all import AddMultibodyPlantSceneGraph
from pydrake.multibody.tree import SpatialInertia, UnitInertia
env_plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
for idx, object_ in enumerate(OBJECTS):
sz, trans, rot = object_
body = env_plant.AddRigidBody(
str(idx + 1),
SpatialInertia(1.0, np.zeros(3), UnitInertia(1.0, 1.0, 1.0)))
pos = Isometry3()
pos.set_translation(trans) # np.array([0,1,1])
pos.set_rotation(rot) # rm([1,0,0], 90)
env_plant.RegisterVisualGeometry(body, pos, Box(sz[0], sz[1], sz[2]),
str(idx + 1) + 'v',
np.array([1, 1, 1, 1]), scene_graph)
env_plant.Finalize()
plant.RegisterGeometry(scene_graph)
builder.Connect(plant.get_geometry_pose_output_port(),
scene_graph.get_source_pose_port(plant.source_id()))
meshcat = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=None, open_browser=None))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
meshcat.get_input_port(0))
# end setup for visualization
def __init__(self, plant, scene_graph, default_joint_angle=-np.pi/60,
damping=1e-5, stiffness=1e-3):
# Drake objects
self.plant = plant
self.scene_graph = scene_graph
# Geometric and physical quantities
# PROGRAMMING: Refactor constants.FRICTION to be constants.mu
self.mu = constants.FRICTION
self.default_joint_angle = default_joint_angle
self.link_width = PLYWOOD_LENGTH/2
self.y_dim = self.link_width * config.num_links.value
self.link_mass = self.x_dim*self.y_dim*self.z_dim*self.density
# Lists of internal Drake objects
self.link_idxs = []
self.joints = []
self.damping = damping
self.stiffness = stiffness
self.instance = self.plant.AddModelInstance(self.name)
for link_num in range(config.num_links.value):
# Initialize bodies and instances
paper_body = self.plant.AddRigidBody(
self.name + "_body" + str(link_num),
self.instance,
SpatialInertia(mass=self.link_mass,
# CoM at origin of body frame
p_PScm_E=np.array([0., 0., 0.]),
# Default moment of inertia for a solid box
G_SP_E=UnitInertia.SolidBox(
self.x_dim, self.link_width, self.z_dim))
)
if self.plant.geometry_source_is_registered():
# Set a box with link dimensions for collision geometry
self.plant.RegisterCollisionGeometry(
paper_body,
RigidTransform(), # Pose in body frame
pydrake.geometry.Box(
self.x_dim, self.link_width, self.z_dim), # Actual shape
self.name + "_body" + str(link_num), pydrake.multibody.plant.CoulombFriction(
self.mu, self.mu) # Friction parameters
)
# Set Set a box with link dimensions for visual geometry
self.plant.RegisterVisualGeometry(
paper_body,
RigidTransform(),
pydrake.geometry.Box(
self.x_dim, self.link_width, self.z_dim),
self.name + "_body" + str(link_num),
[0, 1, 0, 1]) # RGBA color
# Operations between adjacent links
if link_num > 0:
# Get bodies
paper1_body = self.plant.get_body(
BodyIndex(self.link_idxs[-1]))
paper2_body = self.plant.GetBodyByName(
self.name + "_body" + str(link_num), self.instance)
# Set up joints
paper1_hinge_frame = pydrake.multibody.tree.FixedOffsetFrame(
"paper_hinge_frame",
paper1_body,
RigidTransform(RotationMatrix(),
[
0,
(self.link_width+self.hinge_diameter)/2,
(self.z_dim+self.hinge_diameter)/2
])
)
self.plant.AddFrame(paper1_hinge_frame)
paper2_hinge_frame = pydrake.multibody.tree.FixedOffsetFrame(
"paper_hinge_frame",
paper2_body,
RigidTransform(RotationMatrix(),
[
0,
-(self.link_width+self.hinge_diameter)/2,
(self.z_dim+self.hinge_diameter)/2
])
)
self.plant.AddFrame(paper2_hinge_frame)
joint = self.plant.AddJoint(pydrake.multibody.tree.RevoluteJoint(
"paper_hinge_" + str(link_num),
paper1_hinge_frame,
paper2_hinge_frame,
[1, 0, 0],
damping=damping))
if isinstance(default_joint_angle, list):
joint.set_default_angle(
self.default_joint_angle[link_num])
else:
joint.set_default_angle(self.default_joint_angle)
self.plant.AddForceElement(
RevoluteSpring(joint, 0, self.stiffness))
self.joints.append(joint)
# Ignore collisions between adjacent links
geometries = self.plant.CollectRegisteredGeometries(
[paper1_body, paper2_body])
self.scene_graph.collision_filter_manager().Apply(
CollisionFilterDeclaration()
.ExcludeWithin(geometries))
self.link_idxs.append(int(paper_body.index()))
