def AddFloatingRpyJoint(plant, frame, instance):
inertia = UnitInertia.SolidSphere(1.0)
x_body = plant.AddRigidBody(
"x", instance,
SpatialInertia(mass=0, p_PScm_E=[0., 0., 0.], G_SP_E=inertia))
plant.AddJoint(
PrismaticJoint("x", plant.world_frame(), x_body.body_frame(),
[1, 0, 0]))
y_body = plant.AddRigidBody(
"y", instance,
SpatialInertia(mass=0, p_PScm_E=[0., 0., 0.], G_SP_E=inertia))
plant.AddJoint(
PrismaticJoint("y", x_body.body_frame(), y_body.body_frame(),
[0, 1, 0]))
z_body = plant.AddRigidBody(
"z", instance,
SpatialInertia(mass=0, p_PScm_E=[0., 0., 0.], G_SP_E=inertia))
plant.AddJoint(
PrismaticJoint("z", y_body.body_frame(), z_body.body_frame(),
[0, 0, 1]))
plant.AddJoint(BallRpyJoint("ball", z_body.body_frame(), frame))
def add_ground(plant):
ground_model = plant.AddModelInstance("ground_model")
ground_box = plant.AddRigidBody(
"ground", ground_model,
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.3, 0.25))
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)
def add_body(plant, name):
# Returns a random body, with an incrementing name.
inertia = SpatialInertia(
mass=1.,
p_PScm_E=[0, 0, 0],
G_SP_E=UnitInertia(
Ixx=1.,
Iyy=1.,
Izz=1.,
),
)
return plant.AddRigidBody(
name=name,
M_BBo_B=inertia,
)
def AddShape(plant, shape, name, mass=1, mu=1, color=[.5, .5, .9, 1.0]):
instance = plant.AddModelInstance(name)
# TODO: Add a method to UnitInertia that accepts a geometry shape (unless
# that dependency is somehow gross) and does this.
if isinstance(shape, pydrake.geometry.Box):
inertia = pydrake.multibody.tree.UnitInertia.SolidBox(
shape.width(), shape.depth(), shape.height())
elif isinstance(shape, pydrake.geometry.Cylinder):
inertia = pydrake.multibody.tree.UnitInertia.SolidCylinder(
shape.radius(), shape.length())
elif isinstance(shape, pydrake.geometry.Sphere):
inertia = pydrake.multibody.tree.UnitInertia.SolidSphere(
shape.radius())
else:
raise RunTimeError(
f"need to write the unit inertia for shapes of type {shape}")
body = plant.AddRigidBody(
name, instance,
SpatialInertia(mass=mass,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=inertia))
if plant.geometry_source_is_registered():
if isinstance(shape, pydrake.geometry.Box):
plant.RegisterCollisionGeometry(
body, RigidTransform(),
pydrake.geometry.Box(shape.width() - 0.001,
shape.depth() - 0.001,
shape.height() - 0.001), name,
pydrake.multibody.plant.CoulombFriction(mu, mu))
i = 0
for x in [-shape.width() / 2.0, shape.width() / 2.0]:
for y in [-shape.depth() / 2.0, shape.depth() / 2.0]:
for z in [-shape.height() / 2.0, shape.height() / 2.0]:
plant.RegisterCollisionGeometry(
body, RigidTransform([x, y, z]),
pydrake.geometry.Sphere(radius=1e-7),
f"contact_sphere{i}",
pydrake.multibody.plant.CoulombFriction(mu, mu))
i += 1
else:
plant.RegisterCollisionGeometry(
body, RigidTransform(), shape, name,
pydrake.multibody.plant.CoulombFriction(mu, mu))
plant.RegisterVisualGeometry(body, RigidTransform(), shape, name,
color)
return instance
def AddPointFinger(plant):
finger = AddShape(plant, Sphere(0.01), "finger", color=[.9, .5, .5, 1.0])
false_body1 = plant.AddRigidBody(
"false_body1", finger, SpatialInertia(0, [0, 0, 0],
UnitInertia(0, 0, 0)))
finger_x = plant.AddJoint(
PrismaticJoint("finger_x", plant.world_frame(),
plant.GetFrameByName("false_body1"), [1, 0, 0], -.3, .3))
finger_x.set_position_limits([-.5], [.5])
finger_x.set_velocity_limits([-2], [2])
plant.AddJointActuator("finger_x", finger_x)
finger_z = plant.AddJoint(
PrismaticJoint("finger_z", plant.GetFrameByName("false_body1"),
plant.GetFrameByName("finger"), [0, 0, 1], 0.0, 0.5))
finger_z.set_default_translation(0.25)
finger_z.set_position_limits([-.1], [.3])
finger_z.set_velocity_limits([-2], [2])
plant.AddJointActuator("finger_z", finger_z)
return finger
# Random seed setup for reproducibility.
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):
# Set up a new Drake scene from scratch.
builder = DiagramBuilder()
mbp, scene_graph = AddMultibodyPlantSceneGraph(
builder, MultibodyPlant(time_step=0.005))
# Add "tabletop" (ground) as a fixed Box welded to the world.
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))
# Figure out what YCB objects we have available to add.
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 build_mbp(seed=0, verts_geom=False, convex_collision_geom=True):
# Make some random lumpy objects
trimeshes = []
np.random.seed(42)
for k in range(3):
# Make a small random number of triangles and chull it
# to get a lumpy object
mesh = trimesh.creation.random_soup(5)
mesh = trimesh.convex.convex_hull(mesh)
trimeshes.append(mesh)
# Create Drake geometry from those objects by adding a small
# sphere at each vertex
sphere_rad = 0.05
cmap = plt.cm.get_cmap('jet')
builder = DiagramBuilder()
mbp, scene_graph = AddMultibodyPlantSceneGraph(
builder, MultibodyPlant(time_step=0.001))
# Add ground
friction = CoulombFriction(0.9, 0.8)
g = pydrake_geom.Box(100., 100., 0.5)
tf = RigidTransform(p=[0., 0., -0.25])
mbp.RegisterVisualGeometry(body=mbp.world_body(),
X_BG=tf,
shape=g,
name="ground",
diffuse_color=[1.0, 1.0, 1.0, 1.0])
mbp.RegisterCollisionGeometry(body=mbp.world_body(),
X_BG=tf,
shape=g,
name="ground",
coulomb_friction=friction)
for i, mesh in enumerate(trimeshes):
inertia = SpatialInertia(mass=1.0,
p_PScm_E=np.zeros(3),
G_SP_E=UnitInertia(0.01, 0.01, 0.01))
body = mbp.AddRigidBody(name="body_%d" % i, M_BBo_B=inertia)
color = cmap(np.random.random())
if verts_geom:
for j, vert in enumerate(mesh.vertices):
g = pydrake_geom.Sphere(radius=sphere_rad)
tf = RigidTransform(p=vert)
mbp.RegisterVisualGeometry(body=body,
X_BG=tf,
shape=g,
name="body_%d_color_%d" % (i, j),
diffuse_color=color)
mbp.RegisterCollisionGeometry(body=body,
X_BG=tf,
shape=g,
name="body_%d_collision_%d" %
(i, j),
coulomb_friction=friction)
# And add mesh itself for vis
path = "/tmp/part_%d.obj" % i
trimesh.exchange.export.export_mesh(mesh, path)
g = pydrake_geom.Convex(path)
mbp.RegisterVisualGeometry(body=body,
X_BG=RigidTransform(),
shape=g,
name="body_%d_base" % i,
diffuse_color=color)
if convex_collision_geom:
mbp.RegisterCollisionGeometry(body=body,
X_BG=RigidTransform(),
shape=g,
name="body_%d_base_col" % i,
coulomb_friction=friction)
mbp.SetDefaultFreeBodyPose(body, RigidTransform(p=[i % 3, i / 3., 1.]))
mbp.Finalize()
return builder, mbp, scene_graph
RigidTransform, RotationMatrix, SpatialForce,
Simulator, SnoptSolver, Solve, SolverOptions,
UnitInertia, Value)
import pydrake.geometry as pydrake_geom
def torch_tf_to_drake_tf(tf):
return RigidTransform(tf.cpu().detach().numpy())
def drake_tf_to_torch_tf(tf):
return torch.tensor(tf.GetAsMatrix4())
default_spatial_inertia = SpatialInertia(mass=1.0,
p_PScm_E=np.zeros(3),
G_SP_E=UnitInertia(0.01, 0.01, 0.01))
default_friction = CoulombFriction(0.9, 0.8)
class PhysicsGeometryInfo():
'''
Container for physics and geometry info, providing simulator and
visualization interoperation.
Args:
- fixed: Whether this geometry is welded to the world (otherwise,
it will be mobilized by a 6DOF floating base).
- spatial_inertia: Spatial inertia of the body. If None,
will adopt a default mass of 1.0kg and 0.01x0.01x0.01 diagonal
rotational inertia.
- is_container: Flag whether this object will function as a