def CreateIiwaControllerPlant(): """creates plant that includes only the robot and gripper, used for controllers.""" robot_sdf_path = FindResourceOrThrow( "drake/manipulation/models/iiwa_description/iiwa7/iiwa7_no_collision.sdf") gripper_sdf_path = FindResourceOrThrow( "drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf") sim_timestep = 1e-3 plant_robot = MultibodyPlant(sim_timestep) parser = Parser(plant=plant_robot) parser.AddModelFromFile(robot_sdf_path) parser.AddModelFromFile(gripper_sdf_path) plant_robot.WeldFrames( A=plant_robot.world_frame(), B=plant_robot.GetFrameByName("iiwa_link_0")) plant_robot.WeldFrames( A=plant_robot.GetFrameByName("iiwa_link_7"), B=plant_robot.GetFrameByName("body"), X_AB=RigidTransform(RollPitchYaw(np.pi/2, 0, np.pi/2), np.array([0, 0, 0.114])) ) plant_robot.mutable_gravity_field().set_gravity_vector([0, 0, 0]) plant_robot.Finalize() link_frame_indices = [] for i in range(8): link_frame_indices.append( plant_robot.GetFrameByName("iiwa_link_" + str(i)).index()) return plant_robot, link_frame_indices
def setup_manipulation_station(T_world_objectInitial, zmq_url, T_world_targetBin, manipuland="foam", include_bin=True, include_hoop=False): builder = DiagramBuilder() station = builder.AddSystem(ManipulationStation(time_step=1e-3)) station.SetupClutterClearingStation() if manipuland is "foam": station.AddManipulandFromFile( "drake/examples/manipulation_station/models/061_foam_brick.sdf", T_world_objectInitial) elif manipuland is "ball": station.AddManipulandFromFile( "drake/examples/manipulation_station/models/sphere.sdf", T_world_objectInitial) elif manipuland is "bball": station.AddManipulandFromFile( "drake/../../../../../../manipulation/sdfs/bball.sdf", # this is some path hackery T_world_objectInitial) elif manipuland is "rod": station.AddManipulandFromFile( "drake/examples/manipulation_station/models/rod.sdf", T_world_objectInitial) station_plant = station.get_multibody_plant() parser = Parser(station_plant) if include_bin: parser.AddModelFromFile("extra_bin.sdf") station_plant.WeldFrames(station_plant.world_frame(), station_plant.GetFrameByName("extra_bin_base"), T_world_targetBin) if include_hoop: parser.AddModelFromFile("sdfs/hoop.sdf") station_plant.WeldFrames(station_plant.world_frame(), station_plant.GetFrameByName("base_link_hoop"), T_world_targetBin) station.Finalize() frames_to_draw = {"gripper": {"body"}} meshcat = None if zmq_url is not None: meshcat = ConnectMeshcatVisualizer(builder, station.get_scene_graph(), output_port=station.GetOutputPort("pose_bundle"), delete_prefix_on_load=False, frames_to_draw=frames_to_draw, zmq_url=zmq_url) diagram = builder.Build() plant = station.get_multibody_plant() context = plant.CreateDefaultContext() gripper = plant.GetBodyByName("body") initial_pose = plant.EvalBodyPoseInWorld(context, gripper) simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) simulator.AdvanceTo(0.01) return initial_pose, meshcat
def test_thigh_torque_return_type(self): """Verify the signature of ChooseThighTorque""" from hopper_2d import Hopper2dController builder = DiagramBuilder() plant = builder.AddSystem(MultibodyPlant(0.0005)) parser = Parser(plant) parser.AddModelFromFile("raibert_hopper_2d.sdf") plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("ground")) plant.Finalize() controller = Hopper2dController(plant, desired_lateral_velocity=0.0) x0 = np.zeros(10) x0[1] = 4. # in air x0[4] = 0.5 # feasible leg length x0[5] = 0.1 # initial speed torquedes = controller.ChooseThighTorque(x0) self.assertIsInstance(torquedes, float, "ChooseThighTorque returned a type other than "\ "float for X0 = %s, desired_lateral_velocity = %f" % (np.array_str(x0), controller.desired_lateral_velocity)) # Try from another desired velocity controller.desired_lateral_velocity = -1.0 torquedes = controller.ChooseThighTorque(x0) self.assertIsInstance(torquedes, float, "ChooseThighTorque returned a type other than "\ "float for X0 = %s, desired_lateral_velocity = %f" % (np.array_str(x0), controller.desired_lateral_velocity))
def AddPlanarBinAndSimpleBox(plant, mass=1.0, mu=1.0, width=0.2, depth=0.05, height=0.3): parser = Parser(plant) bin = parser.AddModelFromFile(FindResource("models/planar_bin.sdf")) plant.WeldFrames( plant.world_frame(), plant.GetFrameByName("bin_base", bin), RigidTransform(RotationMatrix.MakeZRotation(np.pi / 2.0), [0, 0, -0.015])) planar_joint_frame = plant.AddFrame( FixedOffsetFrame( "planar_joint_frame", plant.world_frame(), RigidTransform(RotationMatrix.MakeXRotation(np.pi / 2)))) # TODO(russt): make this a *random* box? # TODO(russt): move random box to a shared py file. box_instance = AddShape(plant, Box(width, depth, height), "box", mass, mu) box_joint = plant.AddJoint( PlanarJoint("box", planar_joint_frame, plant.GetFrameByName("box", box_instance))) box_joint.set_position_limits([-.5, -.1, -np.pi], [.5, .3, np.pi]) box_joint.set_velocity_limits([-2, -2, -2], [2, 2, 2]) box_joint.set_default_translation([0, depth / 2.0]) return box_instance
def runPendulumExample(args): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder) parser = Parser(plant) parser.AddModelFromFile(FindResource("pendulum/pendulum.urdf")) plant.Finalize() pose_bundle_output_port = scene_graph.get_pose_bundle_output_port() Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]], dtype=np.float64) visualizer = builder.AddSystem( PlanarSceneGraphVisualizer(scene_graph, Tview=Tview, xlim=[-1.2, 1.2], ylim=[-1.2, 1.2])) builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() simulator.set_target_realtime_rate(1.0) # Fix the input port to zero. plant_context = diagram.GetMutableSubsystemContext( plant, simulator.get_mutable_context()) plant_context.FixInputPort(plant.get_actuation_input_port().get_index(), np.zeros(plant.num_actuators())) plant_context.SetContinuousState([0.5, 0.1]) simulator.StepTo(args.duration)
def kinematic_tree_example(): plant = MultibodyPlant(time_step=0.0) parser = Parser(plant) parser.AddModelFromFile( FindResourceOrThrow( "drake/manipulation/models/allegro_hand_description/sdf/allegro_hand_description_right.sdf" )) parser.AddModelFromFile( FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf")) plant.Finalize() g = Source(plant.GetTopologyGraphvizString()) print(g.format) g.view()
def build_block_diagram(actuators_off=False, desired_lateral_velocity=0.0, desired_height=3.0, print_period=0.0): builder = DiagramBuilder() # Build the plant plant = builder.AddSystem(MultibodyPlant(0.0005)) scene_graph = builder.AddSystem(SceneGraph()) plant.RegisterAsSourceForSceneGraph(scene_graph) builder.Connect(plant.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(plant.get_source_id())) builder.Connect(scene_graph.get_query_output_port(), plant.get_geometry_query_input_port()) # Build the robot parser = Parser(plant) parser.AddModelFromFile("raibert_hopper_2d.sdf") plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("ground")) plant.Finalize() plant.set_name('plant') # Create a logger to log at 30hz state_dim = plant.num_positions() + plant.num_velocities() state_log = builder.AddSystem(SignalLogger(state_dim)) state_log.DeclarePeriodicPublish(0.0333, 0.0) # 30hz logging builder.Connect(plant.get_state_output_port(), state_log.get_input_port(0)) state_log.set_name('state_log') # The controller controller = builder.AddSystem( Hopper2dController(plant, desired_lateral_velocity=desired_lateral_velocity, desired_height=desired_height, actuators_off=actuators_off, print_period=print_period)) builder.Connect(plant.get_state_output_port(), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), plant.get_actuation_input_port()) controller.set_name('controller') # Create visualizer visualizer = builder.AddSystem( PlanarSceneGraphVisualizer(scene_graph, xlim=[-1, 10], ylim=[-.2, 4.5], show=False)) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) visualizer.set_name('visualizer') diagram = builder.Build() return diagram
def grasp_poses_example(): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0) parser = Parser(plant, scene_graph) grasp = parser.AddModelFromFile( FindResourceOrThrow( "drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf" ), "grasp") brick = parser.AddModelFromFile( FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf")) plant.Finalize() meshcat = ConnectMeshcatVisualizer(builder, scene_graph) diagram = builder.Build() context = diagram.CreateDefaultContext() plant_context = plant.GetMyContextFromRoot(context) B_O = plant.GetBodyByName("base_link", brick) X_WO = plant.EvalBodyPoseInWorld(plant_context, B_O) B_Ggrasp = plant.GetBodyByName("body", grasp) p_GgraspO = [0, 0.12, 0] R_GgraspO = RotationMatrix.MakeXRotation(np.pi / 2.0).multiply( RotationMatrix.MakeZRotation(np.pi / 2.0)) X_GgraspO = RigidTransform(R_GgraspO, p_GgraspO) X_OGgrasp = X_GgraspO.inverse() X_WGgrasp = X_WO.multiply(X_OGgrasp) plant.SetFreBodyPose(plant_context, B_Ggrasp, X_WGgrasp) # Open the fingers, too plant.GetJointByName("left_finger_sliding_joint", grasp).set_translation(plant_context, -0.054) plant.GetJointByName("right_finger_sliding_joint", grasp).set_translation(plant_context, 0.054) meshcat.load() diagram.Publish(context)
def AddWsg(plant, iiwa_model_instance, roll=np.pi / 2.0, welded=False): parser = Parser(plant) if welded: parser.package_map().Add( "wsg_50_description", os.path.dirname( FindResourceOrThrow( "drake/manipulation/models/wsg_50_description/package.xml")) ) gripper = parser.AddModelFromFile( FindResource("models/schunk_wsg_50_welded_fingers.sdf"), "gripper") else: gripper = parser.AddModelFromFile( FindResourceOrThrow( "drake/manipulation/models/" "wsg_50_description/sdf/schunk_wsg_50_with_tip.sdf")) X_7G = RigidTransform(RollPitchYaw(np.pi / 2.0, 0, roll), [0, 0, 0.114]) plant.WeldFrames(plant.GetFrameByName("iiwa_link_7", iiwa_model_instance), plant.GetFrameByName("body", gripper), X_7G) return gripper
def RunSimulation(self, real_time_rate=1.0): ''' Here we test using the NNSystem in a Simulator to drive an acrobot. ''' sdf_file = "assets/acrobot.sdf" urdf_file = "assets/acrobot.urdf" builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder) parser = Parser(plant=plant, scene_graph=scene_graph) parser.AddModelFromFile(sdf_file) plant.Finalize(scene_graph) # Add nn_system = NNSystem(self.pytorch_nn_object) builder.AddSystem(nn_system) # NN -> plant builder.Connect(nn_system.NN_out_output_port, plant.get_actuation_input_port()) # plant -> NN builder.Connect(plant.get_continuous_state_output_port(), nn_system.NN_in_input_port) # Add meshcat visualizer meshcat = MeshcatVisualizer(scene_graph) builder.AddSystem(meshcat) # builder.Connect(scene_graph.GetOutputPort("lcm_visualization"), builder.Connect(scene_graph.get_pose_bundle_output_port(), meshcat.GetInputPort("lcm_visualization")) # build diagram diagram = builder.Build() meshcat.load() # time.sleep(2.0) RenderSystemWithGraphviz(diagram) # construct simulator simulator = Simulator(diagram) # context = diagram.GetMutableSubsystemContext( # self.station, simulator.get_mutable_context()) simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(real_time_rate) simulator.Initialize() sim_duration = 5. simulator.StepTo(sim_duration) print("stepping complete")
def Simulate2dHopper(x0, duration, desired_lateral_velocity=0.0, print_period=0.0): builder = DiagramBuilder() plant = builder.AddSystem(MultibodyPlant(0.0005)) scene_graph = builder.AddSystem(SceneGraph()) plant.RegisterAsSourceForSceneGraph(scene_graph) builder.Connect(plant.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(plant.get_source_id())) builder.Connect(scene_graph.get_query_output_port(), plant.get_geometry_query_input_port()) # Build the plant parser = Parser(plant) parser.AddModelFromFile("raibert_hopper_2d.sdf") plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("ground")) plant.AddForceElement(UniformGravityFieldElement()) plant.Finalize() # Create a logger to log at 30hz state_dim = plant.num_positions() + plant.num_velocities() state_log = builder.AddSystem(SignalLogger(state_dim)) state_log.DeclarePeriodicPublish(0.0333, 0.0) # 30hz logging builder.Connect(plant.get_continuous_state_output_port(), state_log.get_input_port(0)) # The controller controller = builder.AddSystem( Hopper2dController(plant, desired_lateral_velocity=desired_lateral_velocity, print_period=print_period)) builder.Connect(plant.get_continuous_state_output_port(), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), plant.get_actuation_input_port()) # The diagram diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() plant_context = diagram.GetMutableSubsystemContext( plant, simulator.get_mutable_context()) plant_context.get_mutable_discrete_state_vector().SetFromVector(x0) simulator.StepTo(duration) return plant, controller, state_log
def do_generation_and_simulation(sim_time=10): vis = meshcat.Visualizer(zmq_url="tcp://127.0.0.1:6000") scene_tree, satisfied_clearance = rejection_sample_feasible_tree(num_attempts=1000) scene_tree, satisfied_feasibility = project_tree_to_feasibility(scene_tree, num_attempts=3) serialize_scene_tree_to_yamls_and_sdfs(scene_tree, package_name='save', package_dir=".", remove_directory=True) # Draw generated tree in meshcat. #draw_scene_tree_meshcat(scene_tree, alpha=1.0, node_sphere_size=0.1) # Draw its clearance geometry for debugging. #draw_clearance_geometry_meshcat(scene_tree, alpha=0.3) # Simulate the resulting scene, with a PR2 for scale. builder, mbp, scene_graph = compile_scene_tree_to_mbp_and_sg( scene_tree, timestep=0.001) # Add PR2 and shift it back in front of where I know the table will be. parser = Parser(mbp) pr2_model_path = "/home/gizatt/drake/build/install/share/drake/examples/pr2/models/pr2_description/urdf/pr2_simplified.urdf" parser.package_map().PopulateUpstreamToDrake(pr2_model_path); pr2_model_id = parser.AddModelFromFile( file_name=pr2_model_path, model_name="PR2_for_scale") # Get the tf of the robot spawn node, and put the PR2 at that xy location. robot_spawn_tf = scene_tree.find_nodes_by_type(RobotSpawnLocation)[0].tf.numpy() mbp.GetJointByName("x", model_instance=pr2_model_id).set_default_translation(robot_spawn_tf[0, 3]) mbp.GetJointByName("y", model_instance=pr2_model_id).set_default_translation(robot_spawn_tf[1, 3]) mbp.Finalize() visualizer = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url="default") diagram = builder.Build() diag_context = diagram.CreateDefaultContext() mbp_context = diagram.GetMutableSubsystemContext(mbp, diag_context) # Fix input port for PR2 to zero. actuation_port = mbp.get_actuation_input_port(model_instance=pr2_model_id) nu = mbp.num_actuated_dofs(model_instance=pr2_model_id) actuation_port.FixValue(mbp_context, np.zeros(nu)) sim = Simulator(diagram, diag_context) sim.set_target_realtime_rate(1.0) if not satisfied_clearance: print("WARNING: SCENE TREE NOT SATISFYING CLEARANCE") if not satisfied_feasibility: print("WARNING: SCENE TREE NOT SATISFYING FEASIBILITY, SIM MAY FAIL") try: sim.AdvanceTo(sim_time) except RuntimeError as e: print("Encountered error in sim: ", e)
def build_iiwa7_plant(): plant = MultibodyPlant(1e-3) parser = Parser(plant=plant) iiwa_drake_path = ( "drake/manipulation/models/iiwa_description/iiwa7/iiwa7_no_collision.sdf" ) iiwa_path = FindResourceOrThrow(iiwa_drake_path) robot_model = parser.AddModelFromFile(iiwa_path) # weld robot to world frame. plant.WeldFrames(frame_on_parent_P=plant.world_frame(), frame_on_child_C=plant.GetFrameByName("iiwa_link_0"), X_PC=RigidTransform.Identity()) plant.Finalize() return plant
def setup_dot_diagram(builder, args): ''' Using an existing DiagramBuilder, adds a sim of the Dot robot. Args comes from argparse. The returned controller will need its first port connected to a setpoint source.''' print( "TODO: load in servo calibration dict to a servo calibration object that gets shared" ) with open(args.yaml_path, "r") as f: config_dict = yaml.load(f, Loader=Loader) sdf_path = os.path.join(os.path.dirname(args.yaml_path), config_dict["sdf_path"]) plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0005) parser = Parser(plant) model = parser.AddModelFromFile(sdf_path) # Set initial pose floating above the ground. plant.SetDefaultFreeBodyPose(plant.GetBodyByName("body"), RigidTransform(p=[0., 0., 0.25])) if args.welded: plant.WeldFrames(plant.world_frame(), plant.GetBodyByName("body").body_frame()) else: add_ground(plant) plant.Finalize() controller = builder.AddSystem(ServoController(plant, config_dict)) # Fixed control-rate controller with a low pass filter on its torque output. zoh = builder.AddSystem( ZeroOrderHold(period_sec=0.001, vector_size=controller.n_servos)) filter = builder.AddSystem( FirstOrderLowPassFilter(time_constant=0.02, size=controller.n_servos)) builder.Connect(plant.get_state_output_port(), controller.get_input_port(1)) builder.Connect(controller.get_output_port(0), zoh.get_input_port(0)) builder.Connect(zoh.get_output_port(0), filter.get_input_port(0)) builder.Connect(filter.get_output_port(0), plant.get_actuation_input_port()) return plant, scene_graph, controller
def AddPlanarIiwa(plant): urdf = FindResourceOrThrow( "drake/manipulation/models/iiwa_description/urdf/" "planar_iiwa14_spheres_dense_elbow_collision.urdf") parser = Parser(plant) iiwa = parser.AddModelFromFile(urdf) plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0")) # Set default positions: q0 = [0.1, -1.2, 1.6] index = 0 for joint_index in plant.GetJointIndices(iiwa): joint = plant.get_mutable_joint(joint_index) if isinstance(joint, RevoluteJoint): joint.set_default_angle(q0[index]) index += 1 return iiwa
def AddIiwa(plant, collision_model="no_collision"): sdf_path = FindResourceOrThrow( "drake/manipulation/models/iiwa_description/iiwa7/" f"iiwa7_{collision_model}.sdf") parser = Parser(plant) iiwa = parser.AddModelFromFile(sdf_path) plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0")) # Set default positions: q0 = [0.0, 0.1, 0, -1.2, 0, 1.6, 0] index = 0 for joint_index in plant.GetJointIndices(iiwa): joint = plant.get_mutable_joint(joint_index) if isinstance(joint, RevoluteJoint): joint.set_default_angle(q0[index]) index += 1 return iiwa
def AddTwoLinkIiwa(plant, q0=[0.1, -1.2]): urdf = FindResource("models/two_link_iiwa14.urdf") parser = Parser(plant) parser.package_map().Add( "iiwa_description", os.path.dirname( FindResourceOrThrow( "drake/manipulation/models/iiwa_description/package.xml"))) iiwa = parser.AddModelFromFile(urdf) plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0")) # Set default positions: index = 0 for joint_index in plant.GetJointIndices(iiwa): joint = plant.get_mutable_joint(joint_index) if isinstance(joint, RevoluteJoint): joint.set_default_angle(q0[index]) index += 1 return iiwa
def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument("sdf_path", help="path to sdf") parser.add_argument("--interactive", action='store_true') MeshcatVisualizer.add_argparse_argument(parser) args = parser.parse_args() builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) plant = MultibodyPlant(time_step=0.01) plant.RegisterAsSourceForSceneGraph(scene_graph) parser = Parser(plant) model = parser.AddModelFromFile(args.sdf_path) plant.Finalize() if args.meshcat: meshcat = ConnectMeshcatVisualizer( builder, output_port=scene_graph.get_query_output_port(), zmq_url=args.meshcat, open_browser=args.open_browser) if args.interactive: # Add sliders to set positions of the joints. sliders = builder.AddSystem(JointSliders(robot=plant)) else: q0 = plant.GetPositions(plant.CreateDefaultContext()) sliders = builder.AddSystem(ConstantVectorSource(q0)) to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(plant)) builder.Connect(sliders.get_output_port(0), to_pose.get_input_port()) builder.Connect( to_pose.get_output_port(), scene_graph.get_source_pose_port(plant.get_source_id())) diagram = builder.Build() simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) simulator.AdvanceTo(1E6)
from pydrake.all import (AddMultibodyPlantSceneGraph, DiagramBuilder, Parser, Simulator) from underactuated import FindResource, PlanarSceneGraphVisualizer # Set up a block diagram with the robot (dynamics) and a visualization block. builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder) # Load the double pendulum from Universal Robot Description Format parser = Parser(plant, scene_graph) parser.AddModelFromFile(FindResource("double_pendulum/double_pendulum.urdf")) plant.Finalize() builder.ExportInput(plant.get_actuation_input_port()) visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph, xlim=[-2.8, 2.8], ylim=[-2.8, 2.8])) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) diagram = builder.Build() # Set up a simulator to run this diagram simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) # Set the initial conditions context = simulator.get_mutable_context() # state is (theta1, theta2, theta1dot, theta2dot) context.SetContinuousState([1., 1., 0., 0.])
default=1.0) args = parser.parse_args() builder = DiagramBuilder() plant = builder.AddSystem(MultibodyPlant()) scene_graph = builder.AddSystem(SceneGraph()) plant.RegisterAsSourceForSceneGraph(scene_graph) builder.Connect(plant.get_geometry_poses_output_port(), scene_graph.get_source_pose_port( plant.get_source_id())) builder.Connect(scene_graph.get_query_output_port(), plant.get_geometry_query_input_port()) parser = Parser(plant) parser.AddModelFromFile(FindResource("pendulum/pendulum.urdf")) plant.AddForceElement(UniformGravityFieldElement()) plant.Finalize() Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]], dtype=np.float64) visualizer = builder.AddSystem(PlanarSceneGraphVisualizer( scene_graph, Tview=Tview, xlim=[-1.2, 1.2], ylim=[-1.2, 1.2])) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize()
#Settings duration = 3. #Setup simulator elements builder = DiagramBuilder() plant = builder.AddSystem(MultibodyPlant(0.0005)) scene_graph = builder.AddSystem(SceneGraph()) plant.RegisterAsSourceForSceneGraph(scene_graph) builder.Connect(plant.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(plant.get_source_id())) builder.Connect(scene_graph.get_query_output_port(), plant.get_geometry_query_input_port()) #Setup plant parser = Parser(plant, scene_graph) parser.AddModelFromFile("diff_drive_real.urdf") plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("ground")) '''#Uncomment these three lines if using a urdf with obstacles plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("obstacle_1")) plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("obstacle_2")) plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("obstacle_3")) ''' plant.AddForceElement(UniformGravityFieldElement()) plant.Finalize() #context = plant.CreateDefaultContext() #diff_drive = builder.AddSystem(plant) # Set up visualization in MeshCat #Meshcat Visualization #scene_graph = builder.AddSystem(SceneGraph())
def BuildAndSimulateTrajectory( q_traj, g_traj, get_gripper_controller, T_world_objectInitial, T_world_targetBin, zmq_url, time_step, include_target_bin=True, include_hoop=False, manipuland="foam" ): """Simulate trajectory for manipulation station. @param q_traj: Trajectory class used to initialize TrajectorySource for joints. @param g_traj: Trajectory class used to initialize TrajectorySource for gripper. """ builder = DiagramBuilder() station = builder.AddSystem(ManipulationStation(time_step=time_step)) #1e-3 or 1e-4 probably station.SetupClutterClearingStation() if manipuland is "foam": station.AddManipulandFromFile( "drake/examples/manipulation_station/models/061_foam_brick.sdf", T_world_objectInitial) elif manipuland is "ball": station.AddManipulandFromFile( "drake/examples/manipulation_station/models/sphere.sdf", T_world_objectInitial) elif manipuland is "bball": station.AddManipulandFromFile( "drake/../../../../../../manipulation/sdfs/bball.sdf", # this is some path hackery T_world_objectInitial) elif manipuland is "rod": station.AddManipulandFromFile( "drake/examples/manipulation_station/models/rod.sdf", T_world_objectInitial) station_plant = station.get_multibody_plant() parser = Parser(station_plant) if include_target_bin: parser.AddModelFromFile("sdfs/extra_bin.sdf") station_plant.WeldFrames(station_plant.world_frame(), station_plant.GetFrameByName("extra_bin_base"), T_world_targetBin) if include_hoop: parser.AddModelFromFile("sdfs/hoop.sdf") station_plant.WeldFrames(station_plant.world_frame(), station_plant.GetFrameByName("base_link_hoop"), T_world_targetBin) station.Finalize() # iiwa joint trajectory - predetermined trajectory q_traj_system = builder.AddSystem(TrajectorySource(q_traj)) builder.Connect(q_traj_system.get_output_port(), station.GetInputPort("iiwa_position")) # gripper - closed loop controller gctlr = builder.AddSystem(get_gripper_controller(station_plant)) gctlr.set_name("GripperControllerUsingIiwaState") builder.Connect(station.GetOutputPort("iiwa_position_measured"), gctlr.GetInputPort("iiwa_position")) builder.Connect(station.GetOutputPort("iiwa_velocity_estimated"), gctlr.GetInputPort("iiwa_velocity")) builder.Connect(gctlr.get_output_port(), station.GetInputPort("wsg_position")) loggers = dict( state=builder.AddSystem(SignalLogger(31)), v_est=builder.AddSystem(SignalLogger(7)) ) builder.Connect(station.GetOutputPort("plant_continuous_state"), loggers["state"].get_input_port()) builder.Connect(station.GetOutputPort("iiwa_velocity_estimated"), loggers["v_est"].get_input_port()) meshcat = None if zmq_url is not None: meshcat = ConnectMeshcatVisualizer(builder, station.get_scene_graph(), output_port=station.GetOutputPort("pose_bundle"), delete_prefix_on_load=True, frames_to_draw={"gripper":{"body"}}, zmq_url=zmq_url) diagram = builder.Build() simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) return simulator, station_plant, meshcat, loggers
# 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] # Add random objects to the scene. parser = Parser(mbp, scene_graph) n_objects = np.random.randint(1, 12) obj_ids = [] for k in range(n_objects): obj_i = np.random.randint(len(ycb_object_sdfs)) parser.AddModelFromFile( file_name=ycb_object_sdfs[obj_i], model_name="obj_ycb_%03d" % k) obj_ids.append(k+2) mbp.Finalize() # Optional: set up a meshcat visualizer for the scene. #meshcat = builder.AddSystem( # MeshcatVisualizer(scene_graph, draw_period=0.0333)) #builder.Connect(scene_graph.get_pose_bundle_output_port(), # meshcat.get_input_port(0)) # Figure out where we're putting the camera for the scene by # pointing it inwards, and then applying a random yaw around # the origin. cam_quat_base = RollPitchYaw(
from pydrake.math import RigidTransform #%% station = ManipulationStation() station.SetupClutterClearingStation() parser = Parser(station.get_mutable_multibody_plant(), station.get_mutable_scene_graph()) model_names = ["Cucumber", "Mango", "Lime"] models_object = {} for name in model_names: models_object[name] = [] for i in range(3): models_object[name].append( parser.AddModelFromFile( os.path.join("cad_files", name + '_simplified.sdf'), name + str(i))) station.Finalize() builder = DiagramBuilder() builder.AddSystem(station) ConnectMeshcatVisualizer(builder, scene_graph=station.get_scene_graph(), output_port=station.GetOutputPort("query_object")) diagram = builder.Build() #%% context = diagram.CreateDefaultContext()
MeshcatVisualizerParams, MeshcatVisualizerCpp, Parser, Role, Simulator, MultibodyPlant) if __name__ == "__main__": parser = argparse.ArgumentParser( description='Do interactive placement of objects.') parser.add_argument('model_path', help='Path to model SDF/URDF.') args = parser.parse_args() # Build MBP builder = DiagramBuilder() mbp, scene_graph = AddMultibodyPlantSceneGraph( builder, MultibodyPlant(time_step=1E-3)) # Parse requested file parser = Parser(mbp, scene_graph) model_id = parser.AddModelFromFile(args.model_path) mbp.Finalize() # Visualizer meshcat = Meshcat() vis = MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph, meshcat=meshcat) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() mbp_context = diagram.GetSubsystemContext(mbp, diagram_context) simulator = Simulator(diagram, diagram_context) simulator.Initialize() simulator.set_target_realtime_rate(1.0)
def compile_scene_tree_to_mbp_and_sg(scene_tree, timestep=0.001): builder = DiagramBuilder() mbp, scene_graph = AddMultibodyPlantSceneGraph( builder, MultibodyPlant(time_step=timestep)) parser = Parser(mbp) parser.package_map().PopulateFromEnvironment("ROS_PACKAGE_PATH") world_body = mbp.world_body() node_to_free_body_ids_map = {} body_id_to_node_map = {} free_body_poses = [] for node in scene_tree.nodes: node_to_free_body_ids_map[node] = [] if node.tf is not None and node.physics_geometry_info is not None: # Don't have to do anything if this does not introduce geometry. sanity_check_node_tf_and_physics_geom_info(node) phys_geom_info = node.physics_geometry_info # Don't have to do anything if this does not introduce geometry. has_models = len(phys_geom_info.model_paths) > 0 has_prim_geometry = (len(phys_geom_info.visual_geometry) + len(phys_geom_info.collision_geometry)) > 0 if not has_models and not has_prim_geometry: continue node_model_ids = [] # Handle adding primitive geometry by adding it all to one # mbp. if has_prim_geometry: # Contain this primitive geometry in a model instance. model_id = mbp.AddModelInstance(node.name + "::model_%d" % len(node_model_ids)) node_model_ids.append(model_id) # Add a body for this node, and register any of the # visual and collision geometry available. # TODO(gizatt) This tree body index is built in to disambiguate names. # But I forsee a name-to-stuff resolution crisis when inference time comes... # this might get resolved by the solution to that. body = mbp.AddRigidBody(name=node.name, model_instance=model_id, M_BBo_B=phys_geom_info.spatial_inertia) body_id_to_node_map[body.index()] = node tf = torch_tf_to_drake_tf(node.tf) if phys_geom_info.fixed: weld = mbp.WeldFrames(world_body.body_frame(), body.body_frame(), tf) else: node_to_free_body_ids_map[node].append(body.index()) mbp.SetDefaultFreeBodyPose(body, tf) for k, (tf, geometry, color) in enumerate(phys_geom_info.visual_geometry): mbp.RegisterVisualGeometry(body=body, X_BG=torch_tf_to_drake_tf(tf), shape=geometry, name=node.name + "_vis_%03d" % k, diffuse_color=color) for k, (tf, geometry, friction) in enumerate( phys_geom_info.collision_geometry): mbp.RegisterCollisionGeometry( body=body, X_BG=torch_tf_to_drake_tf(tf), shape=geometry, name=node.name + "_col_%03d" % k, coulomb_friction=friction) # Handle adding each model from sdf/urdf. if has_models: for local_tf, model_path, root_body_name, q0_dict in phys_geom_info.model_paths: model_id = parser.AddModelFromFile( resolve_catkin_package_path(parser.package_map(), model_path), node.name + "::" "model_%d" % len(node_model_ids)) node_model_ids.append(model_id) if root_body_name is None: root_body_ind_possibilities = mbp.GetBodyIndices( model_id) assert len(root_body_ind_possibilities) == 1, \ "Please supply root_body_name for model with path %s" % model_path root_body = mbp.get_body( root_body_ind_possibilities[0]) else: root_body = mbp.GetBodyByName(name=root_body_name, model_instance=model_id) body_id_to_node_map[root_body.index()] = node node_tf = torch_tf_to_drake_tf(node.tf) full_model_tf = node_tf.multiply( torch_tf_to_drake_tf(local_tf)) if phys_geom_info.fixed: mbp.WeldFrames(world_body.body_frame(), root_body.body_frame(), full_model_tf) else: node_to_free_body_ids_map[node].append( root_body.index()) mbp.SetDefaultFreeBodyPose(root_body, full_model_tf) # Handle initial joint state if q0_dict is not None: for joint_name in list(q0_dict.keys()): q0_this = q0_dict[joint_name] joint = mbp.GetMutableJointByName( joint_name, model_instance=model_id) # Reshape to make Drake happy. q0_this = q0_this.reshape(joint.num_positions(), 1) joint.set_default_positions(q0_this) return builder, mbp, scene_graph, node_to_free_body_ids_map, body_id_to_node_map