def test_multibody_plant_parsing(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant(time_step=0.01) model_instance = AddModelFromSdfFile(file_name=file_name, plant=plant) self.assertIsInstance(model_instance, ModelInstanceIndex) plant = MultibodyPlant(time_step=0.01) model_instance = AddModelFromSdfFile(file_name=file_name, model_name="acrobot", plant=plant)
def test_joint_slider(self): # Simply test to make sure that the UI loads and produces the output. file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant() AddModelFromSdfFile(file_name=file_name, plant=plant) plant.Finalize() slider = JointSliders(robot=plant, lower_limit=-5., upper_limit=5., resolution=0.001, update_period_sec=0.01, title='test', length=300) slider.window.withdraw() # Don't open a window during testing. context = slider.CreateDefaultContext() output = slider.AllocateOutput() q = [.1, .2] slider.set_position(q) slider.set_joint_position(q) slider.CalcOutput(context, output) np.testing.assert_array_equal(output.get_vector_data(0).get_value(), q)
def test_multibody_tree_kinematics(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant() AddModelFromSdfFile(file_name, plant) plant.Finalize() context = plant.CreateDefaultContext() tree = plant.model() world_frame = plant.world_frame() # TODO(eric.cousineau): Replace this with `GetFrameByName`. link1_frame = plant.GetBodyByName("Link1").body_frame() X_WL = tree.CalcRelativeTransform(context, frame_A=world_frame, frame_B=link1_frame) self.assertIsInstance(X_WL, Isometry3) p_AQi = tree.CalcPointsPositions(context=context, frame_B=link1_frame, p_BQi=np.array([[0, 1, 2], [10, 11, 12]]).T, frame_A=world_frame).T self.assertTupleEqual(p_AQi.shape, (2, 3)) Jv_WL = tree.CalcFrameGeometricJacobianExpressedInWorld( context=context, frame_B=link1_frame, p_BoFo_B=[0, 0, 0]) self.assertTupleEqual(Jv_WL.shape, (6, plant.num_velocities()))
def load_station(time_step=0.0): # https://github.com/RobotLocomotion/drake/blob/master/bindings/pydrake/examples/manipulation_station_py.cc object_file_path = FindResourceOrThrow( "drake/external/models_robotlocomotion/ycb_objects/061_foam_brick.sdf") #object_file_path = FOAM_BRICK_PATH station = ManipulationStation(time_step) station.AddCupboard() mbp = station.get_mutable_multibody_plant() scene_graph = station.get_mutable_scene_graph() object = AddModelFromSdfFile( file_name=object_file_path, model_name="object", plant=mbp, scene_graph=scene_graph) station.Finalize() robot = mbp.GetModelInstanceByName('iiwa') gripper = mbp.GetModelInstanceByName('gripper') initial_conf = [0, 0.6 - np.pi / 6, 0, -1.75, 0, 1.0, 0] #initial_conf[1] += np.pi / 6 initial_positions = dict(zip(get_movable_joints(mbp, robot), initial_conf)) initial_poses = { object: create_transform(translation=[.6, 0, 0]), } task = Task(mbp, scene_graph, robot, gripper, movable=[object], surfaces=[], initial_positions=initial_positions, initial_poses=initial_poses, goal_on=[]) return mbp, scene_graph, task
def test_set_free_body_pose(self): file_name = FindResourceOrThrow( "drake/examples/double_pendulum/models/double_pendulum.sdf") plant = MultibodyPlant() plant_model = AddModelFromSdfFile(file_name, plant) plant.Finalize() context = plant.CreateDefaultContext() tree = plant.tree() X_WB_desired = Isometry3.Identity() R_WB = np.array([[0., 1., 0.], [0., 0., 1.], [1., 0., 0.]]) X_WB_desired.set_rotation(R_WB) tree.SetFreeBodyPoseOrThrow( body=plant.GetBodyByName("base", plant_model), X_WB=X_WB_desired, context=context) world_frame = plant.world_frame() base_frame = plant.GetBodyByName("base").body_frame() X_WB = tree.CalcRelativeTransform( context, frame_A=world_frame, frame_B=base_frame) self.assertTrue(np.allclose(X_WB.matrix(), X_WB_desired.matrix()))
def test_scene_graph_queries(self): builder = DiagramBuilder() plant, scene_graph = add_plant_and_scene_graph(builder) AddModelFromSdfFile( FindResourceOrThrow( "drake/bindings/pydrake/multibody/test/two_bodies.sdf"), plant, scene_graph) plant.Finalize(scene_graph) diagram = builder.Build() # The model `two_bodies` has two (implicitly) floating bodies that are # placed in the same position. The default state would be for these two # bodies to be coincident, and thus collide. context = diagram.CreateDefaultContext() sg_context = diagram.GetMutableSubsystemContext(scene_graph, context) query_object = scene_graph.get_query_output_port().Eval(sg_context) # Implicitly require that this should be size 1. point_pair, = query_object.ComputePointPairPenetration() self.assertIsInstance(point_pair, PenetrationAsPointPair) inspector = query_object.inspector() bodies = { plant.GetBodyFromFrameId(inspector.GetFrameId(id_)) for id_ in [point_pair.id_A, point_pair.id_B] } self.assertSetEqual( bodies, {plant.GetBodyByName("body1"), plant.GetBodyByName("body2")})
def test_multibody_state_access(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant() AddModelFromSdfFile(file_name, plant) plant.Finalize() context = plant.CreateDefaultContext() tree = plant.tree() self.assertEqual(plant.num_positions(), 2) self.assertEqual(plant.num_velocities(), 2) q0 = np.array([3.14, 2.]) v0 = np.array([-0.5, 1.]) x0 = np.concatenate([q0, v0]) # The default state is all values set to zero. x = tree.get_multibody_state_vector(context) self.assertTrue(np.allclose(x, np.zeros(4))) # Write into a mutable reference to the state vector. x_reff = tree.get_mutable_multibody_state_vector(context) x_reff[:] = x0 # Verify we did modify the state stored in context. x = tree.get_multibody_state_vector(context) self.assertTrue(np.allclose(x, x0))
def __init__(self, time_step, object_file_paths=None, object_base_link_names=None, X_WObject_list=(X_WObject_default,)): self.time_step = time_step # Finalize manipulation station by adding manipuland. self.station = ManipulationStation(self.time_step) self.station.AddCupboard() self.plant = self.station.get_mutable_multibody_plant() self.objects = [] if object_file_paths is not None: for i, file_path in enumerate(object_file_paths): self.objects.append(AddModelFromSdfFile( file_name=file_path, model_name="object_{}".format(i), plant=self.plant, scene_graph=self.station.get_mutable_scene_graph() )) self.station.Finalize() # Object base link names for reference self.object_base_link_names = object_base_link_names # Initial pose of the object self.X_WObject_list = X_WObject_list
def __init__( self, time_step, object_file_path=None, object_base_link_name=None, X_WObject=X_WObject_default, ): self.object_base_link_name = object_base_link_name self.time_step = time_step # Finalize manipulation station by adding manipuland. self.station = ManipulationStation(self.time_step) self.station.SetupDefaultStation() self.plant = self.station.get_mutable_multibody_plant() if object_file_path is not None: self.object = AddModelFromSdfFile( file_name=object_file_path, model_name="object", plant=self.station.get_mutable_multibody_plant(), scene_graph=self.station.get_mutable_scene_graph()) self.station.Finalize() self.simulator = None self.plan_runner = None # Initial pose of the object self.X_WObject = X_WObject
def __init__(self, time_step, object_file_path=None, object_base_link_name=None, X_WObject=X_WObject_default,): self.object_base_link_name = object_base_link_name self.time_step = time_step # Finalize manipulation station by adding manipuland. self.station = ManipulationStation(self.time_step) self.station.AddCupboard() self.plant = self.station.get_mutable_multibody_plant() if object_file_path is not None: self.object = AddModelFromSdfFile( file_name=object_file_path, model_name="object", plant=self.station.get_mutable_multibody_plant(), scene_graph=self.station.get_mutable_scene_graph() ) self.station.Finalize() self.tree = self.plant.tree() self.simulator = None self.plan_runner = None # Initial pose of the object self.X_WObject = X_WObject # Frames self.gripper_model = self.plant.GetModelInstanceByName("gripper") self.world_frame = self.plant.world_frame() self.gripper_frame = self.plant.GetFrameByName("body", self.gripper_model)
def __init__(self, q_initial, p_goal, object_file_paths=None, object_base_link_names=None, X_WObject_list=None): """ :param q_initial: initial config of the robot :param p_goal: goal point in World space :param object_file_paths: a list of objects in the world, each a sdf file path :param object_base_link_names: a list of object base link names :param X_WObject_list: a list of initial pose of the objects """ self.q_initial=q_initial self.p_goal=p_goal # Set up station self.station = ManipulationStation(collision_model=IiwaCollisionModel.kBoxCollision) self.station.AddCupboard() self.plant = self.station.get_mutable_multibody_plant() # Add objects into world self.objects = [] if object_file_paths is not None: for i, file_path in enumerate(object_file_paths): self.objects.append(AddModelFromSdfFile( file_name=file_path, model_name="object_{}".format(i), plant=self.plant, scene_graph=self.station.get_mutable_scene_graph() )) self.station.Finalize() # Object base link names for reference self.object_base_link_names = object_base_link_names # Initial pose of the object self.X_WObject_list = X_WObject_list # Set up util module self.util = Util(self.station) # Set up configuration space joint_ranges = self.util.get_joint_ranges() self.cspace = ConfigurationSpace(joint_ranges, self.util) # Set initial pose of the objects if self.object_base_link_names is not None: for link_name, object, X_WObject in zip(self.object_base_link_names, self.objects, self.X_WObject_list): self.util.set_object_pose(link_name, object, X_WObject) # Set robot initial pose self.util.set_iiwa_position(self.q_initial) self.util.set_wsg_position(0.1) # book keeping self.rewires = 0 self.edge_evaluation = 0
def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument( "--target_realtime_rate", type=float, default=1.0, help="Desired rate relative to real time. See documentation for " "Simulator::set_target_realtime_rate() for details.") parser.add_argument("--simulation_time", type=float, default=10.0, help="Desired duration of the simulation in seconds.") parser.add_argument( "--time_step", type=float, default=0., help="If greater than zero, the plant is modeled as a system with " "discrete updates and period equal to this time_step. " "If 0, the plant is modeled as a continuous system.") args = parser.parse_args() file_name = FindResourceOrThrow( "drake/examples/multibody/cart_pole/cart_pole.sdf") builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) cart_pole = builder.AddSystem(MultibodyPlant(time_step=args.time_step)) AddModelFromSdfFile(file_name=file_name, plant=cart_pole, scene_graph=scene_graph) cart_pole.AddForceElement(UniformGravityFieldElement([0, 0, -9.81])) cart_pole.Finalize(scene_graph) assert cart_pole.geometry_source_is_registered() builder.Connect( cart_pole.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(cart_pole.get_source_id())) lcm = DrakeLcm() ConnectVisualization(scene_graph=scene_graph, builder=builder, lcm=lcm) diagram = builder.Build() DispatchLoadMessage(scene_graph=scene_graph, lcm=lcm) diagram_context = diagram.CreateDefaultContext() cart_pole_context = diagram.GetMutableSubsystemContext( cart_pole, diagram_context) cart_pole_context.FixInputPort( cart_pole.get_actuation_input_port().get_index(), [0]) cart_slider = cart_pole.GetJointByName("CartSlider") pole_pin = cart_pole.GetJointByName("PolePin") cart_slider.set_translation(context=cart_pole_context, translation=0.) pole_pin.set_angle(context=cart_pole_context, angle=2.) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(args.target_realtime_rate) simulator.Initialize() simulator.StepTo(args.simulation_time)
def build_station_simulation(builder): station = builder.AddSystem(ManipulationStation()) station.AddCupboard() object_file_path = "drake/examples/manipulation_station/models/061_foam_brick.sdf" brick = AddModelFromSdfFile(FindResourceOrThrow(object_file_path), station.get_mutable_multibody_plant(), station.get_mutable_scene_graph()) station.Finalize() return station, brick
def test_deprecated_parsing(self): sdf_file = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant(time_step=0.01) with warnings.catch_warnings(record=True) as w: warnings.simplefilter("default", DrakeDeprecationWarning) result = AddModelFromSdfFile(plant=plant, file_name=sdf_file) self.assertIsInstance(result, ModelInstanceIndex) self.assertEqual(len(w), 1)
def test_multibody_dynamics(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant() AddModelFromSdfFile(file_name, plant) plant.Finalize() context = plant.CreateDefaultContext() tree = plant.tree() H = tree.CalcMassMatrixViaInverseDynamics(context) Cv = tree.CalcBiasTerm(context) self.assertTrue(H.shape == (2, 2)) self.assertTrue(Cv.shape == (2, ))
def test_multibody_plant_api_via_parsing(self): # TODO(eric.cousineau): Decouple this when construction can be done # without parsing. # This a subset of `multibody_plant_sdf_parser_test.cc`. file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant(time_step=0.01) model_instance = AddModelFromSdfFile( file_name=file_name, plant=plant, scene_graph=None) self.assertIsInstance(model_instance, ModelInstanceIndex) plant.Finalize() benchmark = MakeAcrobotPlant(AcrobotParameters(), True) self.assertEqual(plant.num_bodies(), benchmark.num_bodies()) self.assertEqual(plant.num_joints(), benchmark.num_joints()) self.assertEqual(plant.num_actuators(), benchmark.num_actuators()) self.assertEqual( plant.num_model_instances(), benchmark.num_model_instances() + 1) self.assertEqual(plant.num_positions(), benchmark.num_positions()) self.assertEqual( plant.num_positions(model_instance=model_instance), benchmark.num_positions()) self.assertEqual( plant.num_velocities(), benchmark.num_velocities()) self.assertEqual( plant.num_multibody_states(), benchmark.num_multibody_states()) self.assertEqual( plant.num_actuated_dofs(), benchmark.num_actuated_dofs()) self.assertTrue(plant.is_finalized()) self.assertTrue(plant.HasBodyNamed(name="Link1")) self.assertTrue(plant.HasJointNamed(name="ShoulderJoint")) shoulder = plant.GetJointByName(name="ShoulderJoint") self._test_joint_api(shoulder) np.testing.assert_array_equal(shoulder.lower_limits(), [-np.inf]) np.testing.assert_array_equal(shoulder.upper_limits(), [np.inf]) self._test_joint_actuator_api( plant.GetJointActuatorByName(name="ElbowJoint")) self._test_body_api(plant.GetBodyByName(name="Link1")) self.assertIs( plant.GetBodyByName(name="Link1"), plant.GetBodyByName(name="Link1", model_instance=model_instance)) self._test_frame_api(plant.GetFrameByName(name="Link1")) self.assertIs( plant.GetFrameByName(name="Link1"), plant.GetFrameByName(name="Link1", model_instance=model_instance)) self.assertIsInstance( plant.get_actuation_input_port(), InputPort) self.assertIsInstance( plant.get_continuous_state_output_port(), OutputPort) self.assertIsInstance( plant.get_contact_results_output_port(), OutputPort)
def load_tables(time_step=0.0): mbp = MultibodyPlant(time_step=time_step) scene_graph = SceneGraph() # TODO: meshes aren't supported during collision checking robot = AddModelFromSdfFile(file_name=IIWA14_SDF_PATH, model_name='iiwa', scene_graph=scene_graph, plant=mbp) gripper = AddModelFromSdfFile(file_name=WSG50_SDF_PATH, model_name='gripper', scene_graph=scene_graph, plant=mbp) # TODO: sdf frame/link error table = AddModelFromSdfFile(file_name=TABLE_SDF_PATH, model_name='table', scene_graph=scene_graph, plant=mbp) table2 = AddModelFromSdfFile(file_name=TABLE_SDF_PATH, model_name='table2', scene_graph=scene_graph, plant=mbp) sink = AddModelFromSdfFile(file_name=SINK_PATH, model_name='sink', scene_graph=scene_graph, plant=mbp) stove = AddModelFromSdfFile(file_name=STOVE_PATH, model_name='stove', scene_graph=scene_graph, plant=mbp) broccoli = AddModelFromSdfFile(file_name=BROCCOLI_PATH, model_name='broccoli', scene_graph=scene_graph, plant=mbp) #wall = AddModelFromSdfFile(file_name=WALL_PATH, model_name='wall', # scene_graph=scene_graph, plant=mbp) wall = None table2_x = 0.75 table_top_z = get_aabb_z_placement(AABBs['sink'], AABBs['table']) # TODO: use geometry weld_gripper(mbp, robot, gripper) weld_to_world(mbp, robot, create_transform(translation=[0, 0, table_top_z])) weld_to_world(mbp, table, create_transform()) weld_to_world(mbp, table2, create_transform(translation=[table2_x, 0, 0])) weld_to_world(mbp, sink, create_transform(translation=[table2_x, 0.25, table_top_z])) weld_to_world(mbp, stove, create_transform(translation=[table2_x, -0.25, table_top_z])) if wall is not None: weld_to_world(mbp, wall, create_transform(translation=[table2_x / 2, 0, table_top_z])) mbp.Finalize(scene_graph) movable = [broccoli] surfaces = [ VisualElement(sink, 'base_link', 0), # Could also just pass the link index VisualElement(stove, 'base_link', 0), ] initial_poses = { broccoli: create_transform(translation=[table2_x, 0, table_top_z]), } task = Task(mbp, scene_graph, robot, gripper, movable=movable, surfaces=surfaces, initial_poses=initial_poses, goal_cooked=[broccoli]) return mbp, scene_graph, task
def test_multibody_plant_inverse_dynamics(self): sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "iiwa_description/sdf/iiwa14_no_collision.sdf") plant = MultibodyPlant(time_step=0.01) AddModelFromSdfFile(file_name=sdf_path, plant=plant) plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0")) plant.Finalize() # Just test that the constructor doesn't throw. controller = InverseDynamics( plant=plant, mode=InverseDynamics.InverseDynamicsMode.kGravityCompensation)
def test_mbp_overloads(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant() AddModelFromSdfFile(file_name=file_name, plant=plant, scene_graph=None) plant.Finalize() context = plant.CreateDefaultContext() frame = plant.GetFrameByName("Link2") parameters = mut.DifferentialInverseKinematicsParameters(2, 2) mut.DoDifferentialInverseKinematics(plant, context, np.zeros(6), frame, parameters) mut.DoDifferentialInverseKinematics(plant, context, Isometry3.Identity(), frame, parameters)
def testMultibodyPositionToGeometryPose(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant(time_step=0.01) model_instance = AddModelFromSdfFile(file_name=file_name, plant=plant) scene_graph = SceneGraph() plant.RegisterAsSourceForSceneGraph(scene_graph) plant.Finalize() to_pose = MultibodyPositionToGeometryPose(plant) # Check the size of the input. self.assertEqual(to_pose.get_input_port().size(), 2) # Just check the spelling of the output port (size is not meaningful # for Abstract-valued ports). to_pose.get_output_port()
def test_multibody_add_joint(self): """ Tests joint constructors and `AddJoint`. """ instance_file = FindResourceOrThrow( "drake/examples/double_pendulum/models/double_pendulum.sdf") # Add different joints between multiple model instances. # TODO(eric.cousineau): Remove the multiple instances and use # programmatically constructed bodies once this API is exposed in # Python. num_joints = 2 plant = MultibodyPlant() instances = [] for i in xrange(num_joints + 1): instance = AddModelFromSdfFile(instance_file, "instance_{}".format(i), plant) instances.append(instance) proximal_frame = "base" distal_frame = "lower_link" joints = [ RevoluteJoint( name="revolve_things", frame_on_parent=plant.GetBodyByName(distal_frame, instances[1]).body_frame(), frame_on_child=plant.GetBodyByName(proximal_frame, instances[2]).body_frame(), axis=[0, 0, 1], damping=0.), WeldJoint( name="weld_things", parent_frame_P=plant.GetBodyByName(distal_frame, instances[0]).body_frame(), child_frame_C=plant.GetBodyByName(proximal_frame, instances[1]).body_frame(), X_PC=Isometry3.Identity()), ] for joint in joints: joint_out = plant.AddJoint(joint) self.assertIs(joint, joint_out) # The model is now complete. plant.Finalize() for joint in joints: self._test_joint_api(joint)
def __init__(self, time_step, object_file_path, object_base_link_name, is_hardware=False): self.object_base_link_name = object_base_link_name self.time_step = time_step self.is_hardware = is_hardware # Finalize manipulation station by adding manipuland. self.station = ManipulationStation(self.time_step) self.station.AddCupboard() self.plant = self.station.get_mutable_multibody_plant() self.object = AddModelFromSdfFile( file_name=object_file_path, model_name="object", plant=self.station.get_mutable_multibody_plant(), scene_graph=self.station.get_mutable_scene_graph()) self.station.Finalize()
def cartPoleTest(self): file_name = FindResourceOrThrow( "drake/examples/multibody/cart_pole/cart_pole.sdf") builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) cart_pole = builder.AddSystem(MultibodyPlant()) AddModelFromSdfFile(file_name=file_name, plant=cart_pole, scene_graph=scene_graph) cart_pole.AddForceElement(UniformGravityFieldElement([0, 0, -9.81])) cart_pole.Finalize(scene_graph) assert cart_pole.geometry_source_is_registered() builder.Connect( cart_pole.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(cart_pole.get_source_id())) visualizer = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=None)) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) diagram = builder.Build() visualizer.load() diagram_context = diagram.CreateDefaultContext() cart_pole_context = diagram.GetMutableSubsystemContext( cart_pole, diagram_context) cart_pole_context.FixInputPort( cart_pole.get_actuation_input_port().get_index(), [0]) cart_slider = cart_pole.GetJointByName("CartSlider") pole_pin = cart_pole.GetJointByName("PolePin") cart_slider.set_translation(context=cart_pole_context, translation=0.) pole_pin.set_angle(context=cart_pole_context, angle=2.) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(args.target_realtime_rate) simulator.Initialize() simulator.StepTo(args.duration)
def setUp(self): file_name = FindResourceOrThrow( "drake/bindings/pydrake/multibody/test/two_bodies.sdf") self.plant = MultibodyPlant(time_step=0.01) model_instance = AddModelFromSdfFile( file_name=file_name, plant=self.plant, scene_graph=None) self.plant.Finalize() self.body1_frame = self.plant.GetBodyByName("body1").body_frame() self.body2_frame = self.plant.GetBodyByName("body2").body_frame() self.ik_two_bodies = ik.InverseKinematics(self.plant) self.prog = self.ik_two_bodies.get_mutable_prog() self.q = self.ik_two_bodies.q() def squaredNorm(x): return np.array([x[0] ** 2 + x[1] ** 2 + x[2] ** 2 + x[3] ** 2]) self.prog.AddConstraint( squaredNorm, [1], [1], self._body1_quat(self.q)) self.prog.AddConstraint( squaredNorm, [1], [1], self._body2_quat(self.q)) self.prog.SetInitialGuess(self._body1_quat(self.q), [1, 0, 0, 0]) self.prog.SetInitialGuess(self._body2_quat(self.q), [1, 0, 0, 0])
def kukaTest(args): file_name = FindResourceOrThrow( "drake/manipulation/models/iiwa_description/sdf/" "iiwa14_no_collision.sdf") builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) kuka = builder.AddSystem(MultibodyPlant()) AddModelFromSdfFile(file_name=file_name, plant=kuka, scene_graph=scene_graph) kuka.AddForceElement(UniformGravityFieldElement([0, 0, -9.81])) kuka.Finalize(scene_graph) assert kuka.geometry_source_is_registered() builder.Connect(kuka.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(kuka.get_source_id())) visualizer = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=None)) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) diagram = builder.Build() visualizer.load() diagram_context = diagram.CreateDefaultContext() kuka_context = diagram.GetMutableSubsystemContext( kuka, diagram_context) kuka_context.FixInputPort( kuka.get_actuation_input_port().get_index(), np.zeros(kuka.get_actuation_input_port().size())) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(args.target_realtime_rate) simulator.Initialize() simulator.StepTo(args.duration)
def __init__(self, time_step, object_file_path, object_base_link_name, is_hardware=False): self.object_base_link_name = object_base_link_name self.time_step = time_step self.is_hardware = is_hardware # Finalize manipulation station by adding manipuland. self.station = ManipulationStation(self.time_step) self.station.AddCupboard() self.plant = self.station.get_mutable_multibody_plant() self.object = AddModelFromSdfFile( file_name=object_file_path, model_name="object", plant=self.station.get_mutable_multibody_plant(), scene_graph=self.station.get_mutable_scene_graph()) self.station.Finalize() # Initial pose of the object X_WObject = Isometry3.Identity() X_WObject.set_translation([.6, 0, 0]) self.X_WObject = X_WObject
parser = argparse.ArgumentParser() parser.add_argument("--test", action='store_true', help="Causes the script to run without blocking for " "user input.", default=False) args = parser.parse_args() file_name = FindResourceOrThrow( "drake/examples/multibody/cart_pole/cart_pole.sdf") builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) cart_pole = builder.AddSystem(MultibodyPlant()) cart_pole.RegisterAsSourceForSceneGraph(scene_graph) AddModelFromSdfFile(file_name=file_name, plant=cart_pole) cart_pole.AddForceElement(UniformGravityFieldElement([0, 0, -9.81])) cart_pole.Finalize() assert cart_pole.geometry_source_is_registered() builder.Connect(scene_graph.get_query_output_port(), cart_pole.get_geometry_query_input_port()) builder.Connect(cart_pole.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(cart_pole.get_source_id())) builder.ExportInput(cart_pole.get_actuation_input_port()) ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph) diagram = builder.Build() diagram.set_name("graphviz_example")
builder = DiagramBuilder() if args.hardware: # TODO(russt): Replace this hard-coded camera serial number with a config # file. camera_ids = ["805212060544"] station = builder.AddSystem( ManipulationStationHardwareInterface(camera_ids)) station.Connect(wait_for_cameras=False) else: station = builder.AddSystem(ManipulationStation()) station.AddCupboard() object = AddModelFromSdfFile( FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf"), "object", station.get_mutable_multibody_plant(), station.get_mutable_scene_graph()) station.Finalize() ConnectDrakeVisualizer(builder, station.get_scene_graph(), station.GetOutputPort("pose_bundle")) teleop = builder.AddSystem( JointSliders(station.get_controller_plant(), length=800)) if args.test: teleop.window.withdraw() # Don't display the window when testing. builder.Connect(teleop.get_output_port(0), station.GetInputPort("iiwa_position"))
parser.add_argument( "--test", action='store_true', help="Disable opening the gui window for testing." ) # TODO(russt): Add option to weld the base to the world pending the # availability of GetUniqueBaseBody requested in #9747. args = parser.parse_args() builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) # Construct a MultibodyPlant and load the SDF into it. plant = MultibodyPlant() plant.RegisterAsSourceForSceneGraph(scene_graph) AddModelFromSdfFile(args.filename, plant) plant.Finalize(scene_graph) # Add sliders to set positions of the joints. sliders = builder.AddSystem(JointSliders(robot=plant)) 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())) # Connect this to drake_visualizer. ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph) if len(args.position): sliders.set_position(args.position) elif len(args.joint_position):
if use_hardware: camera_ids = [ pc_to_pose.camera_configs["left_camera_serial"], pc_to_pose.camera_configs["middle_camera_serial"], pc_to_pose.camera_configs["right_camera_serial"]] station = builder.AddSystem( ManipulationStationHardwareInterface(camera_ids)) station.Connect() else: station = builder.AddSystem(ManipulationStation()) station.AddCupboard() object_file_path = \ "drake/examples/manipulation_station/models/061_foam_brick.sdf" brick = AddModelFromSdfFile( FindResourceOrThrow(object_file_path), station.get_mutable_multibody_plant(), station.get_mutable_scene_graph() ) station.Finalize() # add systems to convert the depth images from ManipulationStation to # PointClouds left_camera_info = pc_to_pose.camera_configs["left_camera_info"] middle_camera_info = pc_to_pose.camera_configs["middle_camera_info"] right_camera_info = pc_to_pose.camera_configs["right_camera_info"] left_dut = builder.AddSystem( mut.DepthImageToPointCloud(camera_info=left_camera_info)) middle_dut = builder.AddSystem( mut.DepthImageToPointCloud(camera_info=middle_camera_info)) right_dut = builder.AddSystem( mut.DepthImageToPointCloud(camera_info=right_camera_info))