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 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 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_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 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 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_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 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 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
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)
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 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())
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()))