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 random_geometry(body): # Creates a random geometry. i = i_next() box = Box( width=random.uniform(0.1, 0.3), depth=random.uniform(0.1, 0.3), height=random.uniform(0.1, 0.3), ) plant.RegisterVisualGeometry( body=body, X_BG=random_X(), shape=box, name=f"visual_{i}", diffuse_color=[random.random(), 0, 0, 0.75], ) static_friction = random.uniform(0.1, 1.) plant.RegisterCollisionGeometry(body=body, X_BG=random_X(), shape=box, name=f"collision_{i}", coulomb_friction=CoulombFriction( static_friction=static_friction, dynamic_friction=static_friction / 2, ))
def test_custom_geometry_name_parsing(self): """ Ensure that name parsing does not fail on programmatically added anchored geometries. """ # Make a minimal example to ensure MeshcatVisualizer loads anchored # geometry. builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) meshcat = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=ZMQ_URL, open_browser=False)) builder.Connect( scene_graph.get_pose_bundle_output_port(), meshcat.get_input_port(0)) source_id = scene_graph.RegisterSource() geom_inst = GeometryInstance(RigidTransform(), Box(1., 1., 1.), "box") geom_id = scene_graph.RegisterAnchoredGeometry(source_id, geom_inst) # Illustration properties required to ensure geometry is parsed scene_graph.AssignRole(source_id, geom_id, IllustrationProperties()) diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize()
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 test_multibody_plant_construction_api(self, T): # SceneGraph does not support `Expression` type. DiagramBuilder = DiagramBuilder_[T] SpatialInertia = SpatialInertia_[float] RigidTransform = RigidTransform_[T] CoulombFriction = CoulombFriction_[T] builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder) spatial_inertia = SpatialInertia() body = plant.AddRigidBody(name="new_body", M_BBo_B=spatial_inertia) new_model_instance = plant.AddModelInstance("new_model_instance") body = plant.AddRigidBody(name="new_body_2", M_BBo_B=spatial_inertia, model_instance=new_model_instance) 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) if T == float: 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 _convert_geom(self, geom): """Given an lcmt_viewer_geometry_data, parse it into a tuple of (Shape, Rgba, RigidTransform).""" shape = None if geom.type == lcmt_viewer_geometry_data.BOX: (width, depth, height) = geom.float_data shape = Box(width=width, depth=depth, height=height) elif geom.type == lcmt_viewer_geometry_data.CAPSULE: (radius, length) = geom.float_data shape = Capsule(radius=radius, length=length) elif geom.type == lcmt_viewer_geometry_data.CYLINDER: (radius, length) = geom.float_data shape = Cylinder(radius=radius, length=length) elif geom.type == lcmt_viewer_geometry_data.ELLIPSOID: (a, b, c) = geom.float_data shape = Ellipsoid(a=a, b=b, c=c) elif geom.type == lcmt_viewer_geometry_data.MESH: (scale_x, scale_y, scale_z) = geom.float_data filename = geom.string_data assert scale_x == scale_y and scale_y == scale_z shape = Mesh(absolute_filename=filename, scale=scale_x) elif geom.type == lcmt_viewer_geometry_data.SPHERE: (radius,) = geom.float_data shape = Sphere(radius=radius) else: _logger.warning(f"Unknown geom.type of {geom.type}") return (None, None, None) rgba = Rgba(*geom.color) pose = self._to_pose(geom.position, geom.quaternion) return (shape, rgba, pose)
def test_procedural_geometry(self): """ This test ensures we can draw procedurally added primitive geometry that is added to the world model instance (which has a slightly different naming scheme than geometry with a non-default / non-world model instance). """ builder = DiagramBuilder() mbp, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) world_body = mbp.world_body() box_shape = Box(1., 2., 3.) # This rigid body will be added to the world model instance since # the model instance is not specified. box_body = mbp.AddRigidBody( "box", SpatialInertia(mass=1.0, p_PScm_E=np.array([0., 0., 0.]), G_SP_E=UnitInertia(1.0, 1.0, 1.0))) mbp.WeldFrames(world_body.body_frame(), box_body.body_frame(), RigidTransform()) mbp.RegisterVisualGeometry(box_body, RigidTransform.Identity(), box_shape, "ground_vis", np.array([0.5, 0.5, 0.5, 1.])) mbp.RegisterCollisionGeometry(box_body, RigidTransform.Identity(), box_shape, "ground_col", CoulombFriction(0.9, 0.8)) mbp.Finalize() frames_to_draw = {"world": {"box"}} visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph)) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() vis_context = diagram.GetMutableSubsystemContext( visualizer, diagram_context) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.AdvanceTo(.1) visualizer.draw(vis_context) self.assertEqual( visualizer.ax.get_title(), "t = 0.1", )
def test_multibody_plant_construction_api(self): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder) spatial_inertia = SpatialInertia() body = plant.AddRigidBody(name="new_body", M_BBo_B=spatial_inertia) box = Box(width=0.5, depth=1.0, height=2.0) body_X_BG = RigidTransform() body_friction = CoulombFriction(static_friction=0.6, dynamic_friction=0.5) plant.RegisterVisualGeometry( body=body, X_BG=body_X_BG, shape=box, name="new_body_visual", diffuse_color=[1., 0.64, 0.0, 0.5]) plant.RegisterCollisionGeometry( body=body, X_BG=body_X_BG, shape=box, name="new_body_collision", coulomb_friction=body_friction)
def add_box_at_location(mbp, name, color, pose, mass=0.25, inertia=UnitInertia(3E-3, 3E-3, 3E-3)): ''' Adds a 5cm cube at the specified pose. Uses a planar floating base in the x-z plane. ''' no_mass_no_inertia = SpatialInertia(mass=0., p_PScm_E=np.array([0., 0., 0.]), G_SP_E=UnitInertia(0., 0., 0.)) body_mass_and_inertia = SpatialInertia(mass=mass, p_PScm_E=np.array([0., 0., 0.]), G_SP_E=inertia) shape = Box(0.05, 0.05, 0.05) model_instance = mbp.AddModelInstance(name) body = mbp.AddRigidBody(name, model_instance, body_mass_and_inertia) RegisterVisualAndCollisionGeometry(mbp, body, RigidTransform(), shape, name, color, CoulombFriction(0.9, 0.8)) body_pre_z = mbp.AddRigidBody("{}_pre_z".format(name), model_instance, no_mass_no_inertia) body_pre_theta = mbp.AddRigidBody("{}_pre_theta".format(name), model_instance, no_mass_no_inertia) world_carrot_origin = mbp.AddFrame(frame=FixedOffsetFrame( name="world_{}_origin".format(name), P=mbp.world_frame(), X_PF=pose)) body_joint_x = PrismaticJoint(name="{}_x".format(name), frame_on_parent=world_carrot_origin, frame_on_child=body_pre_z.body_frame(), axis=[1, 0, 0], damping=0.) mbp.AddJoint(body_joint_x) body_joint_z = PrismaticJoint(name="{}_z".format(name), frame_on_parent=body_pre_z.body_frame(), frame_on_child=body_pre_theta.body_frame(), axis=[0, 0, 1], damping=0.) mbp.AddJoint(body_joint_z) body_joint_theta = RevoluteJoint( name="{}_theta".format(name), frame_on_parent=body_pre_theta.body_frame(), frame_on_child=body.body_frame(), axis=[0, 1, 0], damping=0.) mbp.AddJoint(body_joint_theta)
def make_box(mbp, name): inertia = SpatialInertia.MakeFromCentralInertia( 1, [0, 0, 0], RotationalInertia(1 / 600, 1 / 120, 1 / 120)) body = mbp.AddRigidBody(name, inertia) shape = Box(1, 0.1, 0.1) mbp.RegisterVisualGeometry(body=body, X_BG=RigidTransform(), shape=shape, name=f"{name}_visual", diffuse_color=[1., 0.64, 0.0, 0.5]) body_friction = CoulombFriction(static_friction=0.6, dynamic_friction=0.5) mbp.RegisterCollisionGeometry(body=body, X_BG=RigidTransform(), shape=shape, name="{name}_collision", coulomb_friction=body_friction) return body
def 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 _convert_geom(self, geom): """Given an lcmt_viewer_geometry_data, parse it into a tuple of (Shape, Rgba, RigidTransform).""" shape = None if geom.type == lcmt_viewer_geometry_data.BOX: (width, depth, height) = geom.float_data shape = Box(width=width, depth=depth, height=height) elif geom.type == lcmt_viewer_geometry_data.CAPSULE: (radius, length) = geom.float_data shape = Capsule(radius=radius, length=length) elif geom.type == lcmt_viewer_geometry_data.CYLINDER: (radius, length) = geom.float_data shape = Cylinder(radius=radius, length=length) elif geom.type == lcmt_viewer_geometry_data.ELLIPSOID: (a, b, c) = geom.float_data shape = Ellipsoid(a=a, b=b, c=c) elif geom.type == lcmt_viewer_geometry_data.MESH and geom.string_data: # A mesh to be loaded from a file. (scale_x, scale_y, scale_z) = geom.float_data filename = geom.string_data assert scale_x == scale_y and scale_y == scale_z shape = Mesh(absolute_filename=filename, scale=scale_x) elif geom.type == lcmt_viewer_geometry_data.MESH: assert not geom.string_data # A mesh with the data inline, i.e., # V | T | v0 | v1 | ... vN | t0 | t1 | ... | tM # where # V: The number of vertices. # T: The number of triangles. # N: 3V, the number of floating point values for the V vertices. # M: 3T, the number of vertex indices for the T triangles. _logger.warning("Meldis cannot yet display hydroelastic collision " "meshes; that geometry will be ignored.") elif geom.type == lcmt_viewer_geometry_data.SPHERE: (radius,) = geom.float_data shape = Sphere(radius=radius) else: _logger.warning(f"Unknown geom.type of {geom.type}") return (None, None, None) rgba = Rgba(*geom.color) pose = self._to_pose(geom.position, geom.quaternion) return (shape, rgba, pose)
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 test_make_from_scene_graph_and_iris(self): """ Tests the make from scene graph and iris functionality together as the Iris code makes obstacles from geometries registered in SceneGraph. """ scene_graph = SceneGraph() source_id = scene_graph.RegisterSource("source") frame_id = scene_graph.RegisterFrame( source_id=source_id, frame=GeometryFrame("frame")) box_geometry_id = scene_graph.RegisterGeometry( source_id=source_id, frame_id=frame_id, geometry=GeometryInstance(X_PG=RigidTransform(), shape=Box(1., 1., 1.), name="box")) cylinder_geometry_id = scene_graph.RegisterGeometry( source_id=source_id, frame_id=frame_id, geometry=GeometryInstance(X_PG=RigidTransform(), shape=Cylinder(1., 1.), name="cylinder")) sphere_geometry_id = scene_graph.RegisterGeometry( source_id=source_id, frame_id=frame_id, geometry=GeometryInstance(X_PG=RigidTransform(), shape=Sphere(1.), name="sphere")) capsule_geometry_id = scene_graph.RegisterGeometry( source_id=source_id, frame_id=frame_id, geometry=GeometryInstance(X_PG=RigidTransform(), shape=Capsule(1., 1.0), name="capsule")) context = scene_graph.CreateDefaultContext() pose_vector = FramePoseVector() pose_vector.set_value(frame_id, RigidTransform()) scene_graph.get_source_pose_port(source_id).FixValue( context, pose_vector) query_object = scene_graph.get_query_output_port().Eval(context) H = mut.HPolyhedron( query_object=query_object, geometry_id=box_geometry_id, reference_frame=scene_graph.world_frame_id()) self.assertEqual(H.ambient_dimension(), 3) C = mut.CartesianProduct( query_object=query_object, geometry_id=cylinder_geometry_id, reference_frame=scene_graph.world_frame_id()) self.assertEqual(C.ambient_dimension(), 3) E = mut.Hyperellipsoid( query_object=query_object, geometry_id=sphere_geometry_id, reference_frame=scene_graph.world_frame_id()) self.assertEqual(E.ambient_dimension(), 3) S = mut.MinkowskiSum( query_object=query_object, geometry_id=capsule_geometry_id, reference_frame=scene_graph.world_frame_id()) self.assertEqual(S.ambient_dimension(), 3) P = mut.Point( query_object=query_object, geometry_id=sphere_geometry_id, reference_frame=scene_graph.world_frame_id(), maximum_allowable_radius=1.5) self.assertEqual(P.ambient_dimension(), 3) V = mut.VPolytope( query_object=query_object, geometry_id=box_geometry_id, reference_frame=scene_graph.world_frame_id()) self.assertEqual(V.ambient_dimension(), 3) obstacles = mut.MakeIrisObstacles( query_object=query_object, reference_frame=scene_graph.world_frame_id()) options = mut.IrisOptions() options.require_sample_point_is_contained = True options.iteration_limit = 1 options.termination_threshold = 0.1 options.relative_termination_threshold = 0.01 self.assertNotIn("object at 0x", repr(options)) region = mut.Iris( obstacles=obstacles, sample=[2, 3.4, 5], domain=mut.HPolyhedron.MakeBox( lb=[-5, -5, -5], ub=[5, 5, 5]), options=options) self.assertIsInstance(region, mut.HPolyhedron) obstacles = [ mut.HPolyhedron.MakeUnitBox(3), mut.Hyperellipsoid.MakeUnitBall(3), mut.Point([0, 0, 0]), mut.VPolytope.MakeUnitBox(3)] region = mut.Iris( obstacles=obstacles, sample=[2, 3.4, 5], domain=mut.HPolyhedron.MakeBox( lb=[-5, -5, -5], ub=[5, 5, 5]), options=options) self.assertIsInstance(region, mut.HPolyhedron)
def visualize(theta1, theta2, theta3, theta4=0.0, phi=0.0): file_name = "res/stair_climb.sdf" builder = DiagramBuilder() stair_climb, scene_graph = AddMultibodyPlantSceneGraph(builder) # stair_climb.RegisterAsSourceForSceneGraph(scene_graph) Parser(plant=stair_climb).AddModelFromFile(file_name) # stair_climb.AddForceElement(UniformGravityFieldElement()) front_wheel_x, front_wheel_y = eom.findFrontWheelPosition( theta1, theta2, theta3) front_wheel_x = front_wheel_x.Evaluate() front_wheel_y = front_wheel_y.Evaluate() step = Box(STEP_DEPTH, STEP_WIDTH, STEP_HEIGHT) step_pos = RigidTransform( [STEP_POSITION + STEP_DEPTH / 2.0, 0.0, STEP_HEIGHT / 2.0]) stair_climb.RegisterCollisionGeometry( stair_climb.world_body(), RigidTransform([0.0, 0.0, 0.0]), HalfSpace(), "GroundCollision", CoulombFriction(COEFF_FRICTION, COEFF_FRICTION)) stair_climb.RegisterVisualGeometry(stair_climb.world_body(), RigidTransform([0.0, 0.0, 0.0]), HalfSpace(), "GroundVisual", np.array([0.5, 0.5, 0.5, 0.5])) # Color stair_climb.RegisterCollisionGeometry( stair_climb.world_body(), step_pos, step, "StepCollision", CoulombFriction(COEFF_FRICTION, COEFF_FRICTION)) stair_climb.RegisterVisualGeometry(stair_climb.world_body(), step_pos, step, "StepVisual", np.array([1.0, 1.0, 0.0, 1.0])) # Color stair_climb.Finalize() ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() stair_climb_context = diagram.GetMutableSubsystemContext( stair_climb, diagram_context) stair_climb_context.FixInputPort( stair_climb.get_actuation_input_port().get_index(), [0, 0, 0, 0, 0]) theta1_joint = stair_climb.GetJointByName("theta1") theta2_joint = stair_climb.GetJointByName("theta2") theta3_joint = stair_climb.GetJointByName("theta3") theta4_joint = stair_climb.GetJointByName("theta4") phi_joint = stair_climb.GetJointByName("phi") theta1_joint.set_angle(context=stair_climb_context, angle=theta1) theta2_joint.set_angle(context=stair_climb_context, angle=theta2) theta3_joint.set_angle(context=stair_climb_context, angle=theta3) theta4_joint.set_angle(context=stair_climb_context, angle=theta4) phi_joint.set_angle(context=stair_climb_context, angle=phi) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(0.001) simulator.Initialize() simulator.AdvanceTo(0.0)
def __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())
friction): mbp.RegisterVisualGeometry(body, pose, shape, name + "_vis", color) mbp.RegisterCollisionGeometry(body, pose, shape, name + "_col", friction) 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/")
# 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 diagram = builder.Build() simulator = Simulator(diagram)
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"))