def do_collision_mesh_simplification(input_obj_path, show=False): # Returns a list of new meshs for collision. # These'll be the convex decomp components of the original # mesh. # Create a subdir for the convex decomp parts, as # there might be many. dirname, objname = os.path.split(input_obj_path) piece_dirname = objname[:-4] + "_parts" out_dir = os.path.join(dirname, piece_dirname) os.makedirs(out_dir, exist_ok=True) mesh = trimesh.load(input_obj_path) if show: mesh.show() try: convex_pieces = trimesh.decomposition.convex_decomposition( mesh) # TODO: args if not isinstance(convex_pieces, list): convex_pieces = [convex_pieces] except Exception as e: print("Problem in decomp: ", e) # Give them random colors for display for part in convex_pieces: this_color = trimesh.visual.random_color() part.visual.face_colors[:] = this_color scene = trimesh.scene.scene.Scene() for part in convex_pieces: scene.add_geometry(part) if show: scene.show() mesh.density = 2000 I = mesh.moment_inertia inertia = SpatialInertia.MakeFromCentralInertia( mass=mesh.mass, p_PScm_E=mesh.center_mass, I_SScm_E=RotationalInertia( Ixx=I[0, 0], Ixy=I[0, 1], Ixz=I[0, 2], Iyy=I[1, 1], Iyz=I[1, 2], Izz=I[2, 2]).ShiftFromCenterOfMass(mass=mesh.mass, p_BcmQ_E=-mesh.center_mass) ) # Save them each out out_paths = [] for k, part in enumerate(convex_pieces): piece_name = '%s_convex_piece_%03d.obj' % (objname[:-4], k) full_path = os.path.join(out_dir, piece_name) trimesh.exchange.export.export_mesh(part, full_path) out_paths.append(full_path) return out_paths, inertia
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