def _create_plant(self, urdf): self.plant = MultibodyPlant(time_step=0.0) self.scenegraph = SceneGraph() self.plant.RegisterAsSourceForSceneGraph(self.scenegraph) self.model_index = Parser(self.plant).AddModelFromFile(FindResource(urdf)) self.builder = DiagramBuilder() self.builder.AddSystem(self.scenegraph)
def __init__(self, builder, dt=5e-4, N=150, params=None, trj_decay=0.7, x_w_cov=1e-5, door_angle_ref=1.0, visualize=False): self.plant_derivs = MultibodyPlant(time_step=dt) parser = Parser(self.plant_derivs) self.derivs_iiwa, _, _ = self.add_models(self.plant_derivs, parser, params=params) self.plant_derivs.Finalize() self.plant_derivs_context = self.plant_derivs.CreateDefaultContext() self.plant_derivs.get_actuation_input_port().FixValue( self.plant_derivs_context, [0., 0., 0., 0., 0., 0., 0.]) null_force = BasicVector([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) self.plant_derivs.GetInputPort("applied_generalized_force").FixValue( self.plant_derivs_context, null_force) self.plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=dt) parser = Parser(self.plant, scene_graph) self.iiwa, self.hinge, self.bushing = self.add_models(self.plant, parser, params=params) self.plant.Finalize( ) # Finalize will assign ports for compatibility w/ the scene_graph; could be cause of the issue w/ first order taylor. self.meshcat = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url) self.sym_derivs = False # If system should use symbolic derivatives; if false, autodiff self.custom_sim = False # If rollouts should be gathered with sys.dyn() calls nq = self.plant.num_positions() nv = self.plant.num_velocities() self.n_x = nq + nv self.n_u = self.plant.num_actuators() self.n_y = self.plant.get_state_output_port(self.iiwa).size() self.N = N self.dt = dt self.decay = trj_decay self.V = 1e-2 * np.ones(self.n_y) self.W = np.concatenate((1e-7 * np.ones(nq), 1e-4 * np.ones(nv))) self.W0 = np.concatenate((1e-9 * np.ones(nq), 1e-6 * np.ones(nv))) self.x_w_cov = x_w_cov self.door_angle_ref = door_angle_ref self.q0 = np.array( [-3.12, -0.17, 0.52, -3.11, 1.22, -0.75, -1.56, 0.55]) #self.q0 = np.array([-3.12, -0.27, 0.52, -3.11, 1.22, -0.75, -1.56, 0.55]) self.x0 = np.concatenate((self.q0, np.zeros(nv))) self.door_index = None self.phi = {}
def visualize(urdf, xtraj): plant = MultibodyPlant(time_step=0.0) scenegraph = SceneGraph() plant.RegisterAsSourceForSceneGraph(self.scenegraph) model_index = Parser(plant).AddModelFromFile(FindResource(urdf)) builder = DiagramBuilder() builder.AddSystem(scenegraph) plant.Finalize()
def setUp(self): self.plant = MultibodyPlant(mbp_time_step) load_atlas(self.plant, add_ground=False) self.context = self.plant.CreateDefaultContext() upright_context = self.plant.CreateDefaultContext() set_atlas_initial_pose(self.plant, upright_context) self.q_nom = self.plant.GetPositions(upright_context) self.planner = HumanoidPlanner(self.plant, Atlas.CONTACTS_PER_FRAME, self.q_nom) self.num_contacts = 16 self.contact_dim = 3*16 self.N_d = 4
def num_positions_velocities_example(): plant = MultibodyPlant(time_step=0.0) Parser(plant).AddModelFromFile( FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf")) plant.Finalize() context = plant.CreateDefaultContext() print(context) print(f"plant.num_positions() = {plant.num_positions()}") print(f"plant.num_velocities() = {plant.num_velocities()}")
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 compile_scene_tree_clearance_geometry_to_mbp_and_sg( scene_tree, timestep=0.001, alpha=0.25): 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() free_body_poses = [] # For generating colors. node_class_to_color_dict = {} cmap = plt.cm.get_cmap('jet') cmap_counter = 0. for node in scene_tree.nodes: 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 has_clearance_geometry = len(phys_geom_info.clearance_geometry) > 0 if not has_clearance_geometry: continue # Add a body for this node and register the clearance geometry. # 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, M_BBo_B=phys_geom_info.spatial_inertia) tf = torch_tf_to_drake_tf(node.tf) mbp.SetDefaultFreeBodyPose(body, tf) # Pick out a color for this class. node_type_string = node.__class__.__name__ if node_type_string in node_class_to_color_dict.keys(): color = node_class_to_color_dict[node_type_string] else: color = list(cmap(cmap_counter)) color[3] = alpha node_class_to_color_dict[node_type_string] = color cmap_counter = np.fmod(cmap_counter + np.pi * 2., 1.) # Handle adding primitive geometry by adding it all to one # mbp. if len(phys_geom_info.clearance_geometry) > 0: for k, (tf, geometry) in enumerate( phys_geom_info.clearance_geometry): mbp.RegisterCollisionGeometry( body=body, X_BG=torch_tf_to_drake_tf(tf), shape=geometry, name=node.name + "_col_%03d" % k, coulomb_friction=default_friction) mbp.RegisterVisualGeometry(body=body, X_BG=torch_tf_to_drake_tf(tf), shape=geometry, name=node.name + "_vis_%03d" % k, diffuse_color=color) return builder, mbp, scene_graph
def visualize(q, dt=None): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, MultibodyPlant(0.001)) load_atlas(plant, add_ground=True) plant_context = plant.CreateDefaultContext() ConnectContactResultsToDrakeVisualizer(builder=builder, plant=plant) ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph) diagram = builder.Build() if len(q.shape) == 1: q = np.reshape(q, (1, -1)) for i in range(q.shape[0]): print(f"knot point: {i}") diagram_context = diagram.CreateDefaultContext() plant_context = diagram.GetMutableSubsystemContext(plant, diagram_context) set_null_input(plant, plant_context) plant.SetPositions(plant_context, q[i]) simulator = Simulator(diagram, diagram_context) simulator.set_target_realtime_rate(0.0) simulator.AdvanceTo(0.0001) if not dt is None: time.sleep(5/(np.sum(dt))*dt[i]) else: time.sleep(0.5)
def plant_system(self): ''' Implements the plant_system method in DrakeEnv by constructing a RigidBodyPlant ''' # Add Systems builder = DiagramBuilder() self.scene_graph = builder.AddSystem(SceneGraph()) self.mbp = builder.AddSystem(MultibodyPlant()) # Load the model from the file AddModelFromSdfFile(file_name=self.model_path, plant=self.mbp, scene_graph=self.scene_graph) self.mbp.AddForceElement(UniformGravityFieldElement([0, 0, -9.81])) self.mbp.Finalize(self.scene_graph) assert self.mbp.geometry_source_is_registered() # Visualizer must be initialized after Finalize() and before CreateDefaultContext() self.init_visualizer() builder.Connect( self.mbp.get_geometry_poses_output_port(), self.scene_graph.get_source_pose_port(self.mbp.get_source_id())) # Expose the inputs and outputs and build the diagram self._input_port_index_action = builder.ExportInput( self.mbp.get_actuation_input_port()) self._output_port_index_state = builder.ExportOutput( self.mbp.get_continuous_state_output_port()) self.diagram = builder.Build() self._output = self.mbp.AllocateOutput() return self.diagram
def __init__(self): pydrake.systems.framework.LeafSystem.__init__(self) # Physical parameters self.manipulator_plant = MultibodyPlant(config.DT) manipulator.data["add_plant_function"](self.manipulator_plant) self.manipulator_plant.Finalize() self.manipulator_plant_context = \ self.manipulator_plant.CreateDefaultContext() self.nq_manipulator = \ self.manipulator_plant.get_actuation_input_port().size() self.DeclareVectorInputPort("state", BasicVector(self.nq_manipulator * 2)) self.DeclareVectorOutputPort( "q", pydrake.systems.framework.BasicVector(self.nq_manipulator), self.output_q) self.DeclareVectorOutputPort( "v", pydrake.systems.framework.BasicVector(self.nq_manipulator), self.output_v) self.DeclareVectorOutputPort( "tau_g", pydrake.systems.framework.BasicVector(self.nq_manipulator), self.output_tau_g) self.DeclareVectorOutputPort( "M", pydrake.systems.framework.BasicVector( self.nq_manipulator * self.nq_manipulator), self.output_M) self.DeclareVectorOutputPort( "Cv", pydrake.systems.framework.BasicVector(self.nq_manipulator), self.output_Cv) self.DeclareVectorOutputPort( "J", pydrake.systems.framework.BasicVector(6 * self.nq_manipulator), self.output_J) self.DeclareVectorOutputPort( "J_translational", pydrake.systems.framework.BasicVector(3 * self.nq_manipulator), self.output_J_translational) self.DeclareVectorOutputPort( "J_rotational", pydrake.systems.framework.BasicVector(3 * self.nq_manipulator), self.output_J_rotational) self.DeclareVectorOutputPort("Jdot_qdot", pydrake.systems.framework.BasicVector(6), self.output_Jdot_qdot)
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 setUpClass(cls): """Find and load the sliding_block urdf file""" urdf_file = FindResource("systems/urdf/sliding_block.urdf") cls.plant = MultibodyPlant(0.0) cls.block = Parser(cls.plant).AddModelFromFile(urdf_file) body_inds = cls.plant.GetBodyIndices(cls.block) base_frame = cls.plant.get_body(body_inds[0]).body_frame() world_frame = cls.plant.world_frame() cls.plant.WeldFrames(world_frame, base_frame, RigidTransform()) cls.plant.Finalize()
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 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 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 test_simple_trajectory(self): plant = MultibodyPlant(0.001) load_atlas(plant, add_ground=False) T = 2.0 l_foot_pos_traj = PiecewisePolynomial.FirstOrderHold( np.linspace(0, T, 5), np.array([[0.00, 0.09, 0.00], [0.25, 0.09, 0.10], [0.50, 0.09, 0.00], [0.50, 0.09, 0.00], [0.50, 0.09, 0.00]]).T) r_foot_pos_traj = PiecewisePolynomial.FirstOrderHold( np.linspace(0, T, 5), np.array([[0.00, -0.09, 0.00], [0.00, -0.09, 0.00], [0.00, -0.09, 0.00], [0.25, -0.09, 0.10], [0.50, -0.09, 0.00]]).T) pelvis_pos_traj = PiecewisePolynomial.FirstOrderHold( [0.0, T], np.array([[0.00, 0.00, Atlas.PELVIS_HEIGHT - 0.05], [0.50, 0.00, Atlas.PELVIS_HEIGHT - 0.05]]).T) null_orientation_traj = PiecewiseQuaternionSlerp([0.0, T], [ Quaternion([1.0, 0.0, 0.0, 0.0]), Quaternion([1.0, 0.0, 0.0, 0.0]) ]) generator = PoseGenerator(plant, [ Trajectory('l_foot', Atlas.FOOT_OFFSET, l_foot_pos_traj, 1e-3, null_orientation_traj, 0.05), Trajectory('r_foot', Atlas.FOOT_OFFSET, r_foot_pos_traj, 1e-3, null_orientation_traj, 0.05), Trajectory('pelvis', np.zeros(3), pelvis_pos_traj, 1e-2, null_orientation_traj, 0.2) ]) for t in np.linspace(0, T, 50): q = generator.get_ik(t) if q is not None: visualize(q) else: self.fail("Failed to find IK solution!") time.sleep(0.2)
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)
drake_theta_pub_echo.publish(x_cmd[1,times]) times=times+1 #'current x state is :{}'.format(statex) rospy.loginfo(u_cmd[:,times]) rate.sleep() if __name__ == '__main__': rospy.init_node('drake_control', anonymous=True) #fig, ax = plt.subplots(2,1,figsize=(8,8)) i=0 sum_x=0 #x_pub = rospy.Publisher('/curretx', catpolexMsg, queue_size=100) drake_cmd_pub = rospy.Publisher('/chassis_world_effort_controller/command', Float64, queue_size=100) #drake_cmd_pub = rospy.Publisher('/gazebo/set_link_state', LinkState, queue_size=100) drake_cmd_pub_echo = rospy.Publisher('/drake_cmdu0_pub_echo', Float64, queue_size=100) drake_state_pub_echo = rospy.Publisher('/drake_x_pub_echo', Float64, queue_size=100) drake_theta_pub_echo = rospy.Publisher('/drake_theta_pub_echo', Float64, queue_size=100) #/chassis_world_effort_controller/command # sub = rospy.Subscriber("/curretx",catpolexMsg,currentCallback, queue_size=1, buff_size=100)#1000HZ plant = MultibodyPlant(time_step=0.0) scene_graph = SceneGraph() plant.RegisterAsSourceForSceneGraph(scene_graph) file_name = FindResource("/home/cby/catkin_ws/src/cartpole/urdf/cartpole2.urdf") xu_cmd=drake_trajectory_generation(file_name)#u_cmd with 400points effort input u_cmd=u_cmd_drake x_cmd=x_cmd_drake rospy.loginfo("trajectory complete!! sending") drake_cmd_publisher(u_cmd,x_cmd) #rospy.spin()
import pydrake from pydrake.all import (AddMultibodyPlantSceneGraph, DiagramBuilder, Meshcat, 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)
In this notebook we will use the compass gait `MultibodyPlant` only as a support for the computation of the robot kinematics and dynamics. In particular, we will not use it for simulation. The reason for this resides in the scuffing issue described above: when the swing foot is sliding on the ground at zero height, any good physic simulator detects a collision and applies a friction force to the foot. This would cause the compass gait ot stumble and fall down, no matter the trajectory we found. Note however that more complex robots, such as the [kneed walker](http://underactuated.mit.edu/simple_legs.html#kneed_walker), do not have this issue, and, for these, the workflow we use in this notebook can be used both for the detection of the limit cycle and for simulation. **An important implementation detail.** Behind the scenes, the optimization solvers we use require the knowledge of the derivaties of the cost function and the constraint values with respect to the decision variables. This is needed to understand in which direction the current solution should be corrected to find a feasible point or reduce the cost. This process used to be very tedious some years ago, when graduate students had to spend many hours writing down these derivatives "by hand". Nowadays, we use [automatic differentiation](https://en.wikipedia.org/wiki/Automatic_differentiation), which through the construction of a computational graph is able to evaluate a function and its derivatives very quickly and exactly (cf. [finite difference](https://en.wikipedia.org/wiki/Finite_difference)). To allow the evaluation of the `MultibodyPlant` functions (e.g. the mass matrix method) with `AutoDiffXd` variables, we need to call the `MultibodyPlant.ToAutoDiffXd()` function which returns a copy of the `MultibodyPlant` that can work with autodiff variables. """ # parse urdf and create the MultibodyPlant compass_gait = MultibodyPlant(time_step=0) file_name = FindResource('models/compass_gait_limit_cycle.urdf') Parser(compass_gait).AddModelFromFile(file_name) compass_gait.Finalize() # overwrite MultibodyPlant with its autodiff copy compass_gait = compass_gait.ToAutoDiffXd() # number of configuration variables nq = compass_gait.num_positions() # number of components of the contact forces nf = 2 """## Helper Functions for the `MathematicalProgram`
def make_box_flipup(generator, observations="state", meshcat=None, time_limit=10): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.001) # TODO(russt): randomize parameters. box = AddPlanarBinAndSimpleBox(plant) finger = AddPointFinger(plant) plant.Finalize() plant.set_name("plant") SetTransparency(scene_graph, alpha=0.5, source_id=plant.get_source_id()) controller_plant = MultibodyPlant(time_step=0.005) AddPointFinger(controller_plant) if meshcat: MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph, meshcat) meshcat.Set2dRenderMode(xmin=-.35, xmax=.35, ymin=-0.1, ymax=0.3) ContactVisualizer.AddToBuilder( builder, plant, meshcat, ContactVisualizerParams(radius=0.005, newtons_per_meter=40.0)) # Use the controller plant to visualize the set point geometry. controller_scene_graph = builder.AddSystem(SceneGraph()) controller_plant.RegisterAsSourceForSceneGraph(controller_scene_graph) SetColor(controller_scene_graph, color=[1.0, 165.0 / 255, 0.0, 1.0], source_id=controller_plant.get_source_id()) controller_vis = MeshcatVisualizerCpp.AddToBuilder( builder, controller_scene_graph, meshcat, MeshcatVisualizerParams(prefix="controller")) controller_vis.set_name("controller meshcat") controller_plant.Finalize() # Stiffness control. (For a point finger with unit mass, the # InverseDynamicsController is identical) N = controller_plant.num_positions() kp = [100] * N ki = [1] * N kd = [2 * np.sqrt(kp[0])] * N controller = builder.AddSystem( InverseDynamicsController(controller_plant, kp, ki, kd, False)) builder.Connect(plant.get_state_output_port(finger), controller.get_input_port_estimated_state()) actions = builder.AddSystem(PassThrough(N)) positions_to_state = builder.AddSystem(Multiplexer([N, N])) builder.Connect(actions.get_output_port(), positions_to_state.get_input_port(0)) zeros = builder.AddSystem(ConstantVectorSource([0] * N)) builder.Connect(zeros.get_output_port(), positions_to_state.get_input_port(1)) builder.Connect(positions_to_state.get_output_port(), controller.get_input_port_desired_state()) builder.Connect(controller.get_output_port(), plant.get_actuation_input_port()) if meshcat: positions_to_poses = builder.AddSystem( MultibodyPositionToGeometryPose(controller_plant)) builder.Connect( positions_to_poses.get_output_port(), controller_scene_graph.get_source_pose_port( controller_plant.get_source_id())) builder.ExportInput(actions.get_input_port(), "actions") if observations == "state": builder.ExportOutput(plant.get_state_output_port(), "observations") # TODO(russt): Add 'time', and 'keypoints' else: raise ValueError("observations must be one of ['state']") class RewardSystem(LeafSystem): def __init__(self): LeafSystem.__init__(self) self.DeclareVectorInputPort("box_state", 6) self.DeclareVectorInputPort("finger_state", 4) self.DeclareVectorInputPort("actions", 2) self.DeclareVectorOutputPort("reward", 1, self.CalcReward) def CalcReward(self, context, output): box_state = self.get_input_port(0).Eval(context) finger_state = self.get_input_port(1).Eval(context) actions = self.get_input_port(2).Eval(context) angle_from_vertical = (box_state[2] % np.pi) - np.pi / 2 cost = 2 * angle_from_vertical**2 # box angle cost += 0.1 * box_state[5]**2 # box velocity effort = actions - finger_state[:2] cost += 0.1 * effort.dot(effort) # effort # finger velocity cost += 0.1 * finger_state[2:].dot(finger_state[2:]) # Add 10 to make rewards positive (to avoid rewarding simulator # crashes). output[0] = 10 - cost reward = builder.AddSystem(RewardSystem()) builder.Connect(plant.get_state_output_port(box), reward.get_input_port(0)) builder.Connect(plant.get_state_output_port(finger), reward.get_input_port(1)) builder.Connect(actions.get_output_port(), reward.get_input_port(2)) builder.ExportOutput(reward.get_output_port(), "reward") # Set random state distributions. uniform_random = Variable(name="uniform_random", type=Variable.Type.RANDOM_UNIFORM) box_joint = plant.GetJointByName("box") x, y = box_joint.get_default_translation() box_joint.set_random_pose_distribution([.2 * uniform_random - .1 + x, y], 0) diagram = builder.Build() simulator = Simulator(diagram) # Termination conditions: def monitor(context): if context.get_time() > time_limit: return EventStatus.ReachedTermination(diagram, "time limit") return EventStatus.Succeeded() simulator.set_monitor(monitor) return simulator
def build_mbp(seed=0, verts_geom=False, convex_collision_geom=True): # Make some random lumpy objects trimeshes = [] np.random.seed(42) for k in range(3): # Make a small random number of triangles and chull it # to get a lumpy object mesh = trimesh.creation.random_soup(5) mesh = trimesh.convex.convex_hull(mesh) trimeshes.append(mesh) # Create Drake geometry from those objects by adding a small # sphere at each vertex sphere_rad = 0.05 cmap = plt.cm.get_cmap('jet') builder = DiagramBuilder() mbp, scene_graph = AddMultibodyPlantSceneGraph( builder, MultibodyPlant(time_step=0.001)) # Add ground friction = CoulombFriction(0.9, 0.8) g = pydrake_geom.Box(100., 100., 0.5) tf = RigidTransform(p=[0., 0., -0.25]) mbp.RegisterVisualGeometry(body=mbp.world_body(), X_BG=tf, shape=g, name="ground", diffuse_color=[1.0, 1.0, 1.0, 1.0]) mbp.RegisterCollisionGeometry(body=mbp.world_body(), X_BG=tf, shape=g, name="ground", coulomb_friction=friction) for i, mesh in enumerate(trimeshes): inertia = SpatialInertia(mass=1.0, p_PScm_E=np.zeros(3), G_SP_E=UnitInertia(0.01, 0.01, 0.01)) body = mbp.AddRigidBody(name="body_%d" % i, M_BBo_B=inertia) color = cmap(np.random.random()) if verts_geom: for j, vert in enumerate(mesh.vertices): g = pydrake_geom.Sphere(radius=sphere_rad) tf = RigidTransform(p=vert) mbp.RegisterVisualGeometry(body=body, X_BG=tf, shape=g, name="body_%d_color_%d" % (i, j), diffuse_color=color) mbp.RegisterCollisionGeometry(body=body, X_BG=tf, shape=g, name="body_%d_collision_%d" % (i, j), coulomb_friction=friction) # And add mesh itself for vis path = "/tmp/part_%d.obj" % i trimesh.exchange.export.export_mesh(mesh, path) g = pydrake_geom.Convex(path) mbp.RegisterVisualGeometry(body=body, X_BG=RigidTransform(), shape=g, name="body_%d_base" % i, diffuse_color=color) if convex_collision_geom: mbp.RegisterCollisionGeometry(body=body, X_BG=RigidTransform(), shape=g, name="body_%d_base_col" % i, coulomb_friction=friction) mbp.SetDefaultFreeBodyPose(body, RigidTransform(p=[i % 3, i / 3., 1.])) mbp.Finalize() return builder, mbp, scene_graph
mbp.RegisterVisualGeometry(body, pose, shape, name + "_vis", color) mbp.RegisterCollisionGeometry(body, pose, shape, name + "_col", friction) if __name__ == "__main__": # Random seed setup for reproducibility. np.random.seed(int(codecs.encode(os.urandom(4), 'hex'), 32) & (2**32 - 1)) random.seed(os.urandom(4)) for scene_k in range(25): # Set up a new Drake scene from scratch. builder = DiagramBuilder() mbp, scene_graph = AddMultibodyPlantSceneGraph( builder, MultibodyPlant(time_step=0.005)) # Add "tabletop" (ground) as a fixed Box welded to the world. world_body = mbp.world_body() ground_shape = Box(1., 1., 1.) ground_body = mbp.AddRigidBody("ground", SpatialInertia( mass=10.0, p_PScm_E=np.array([0., 0., 0.]), G_SP_E=UnitInertia(1.0, 1.0, 1.0))) mbp.WeldFrames(world_body.body_frame(), ground_body.body_frame(), RigidTransform()) RegisterVisualAndCollisionGeometry( mbp, ground_body, RigidTransform(p=[0, 0, -0.5]), ground_shape, "ground", np.array([0.5, 0.5, 0.5, 1.]), CoulombFriction(0.9, 0.8))
class ProprioceptionSystem(pydrake.systems.framework.LeafSystem): """ Calculates J, M, Cv, etc. based on arm state. """ def __init__(self): pydrake.systems.framework.LeafSystem.__init__(self) # Physical parameters self.manipulator_plant = MultibodyPlant(config.DT) manipulator.data["add_plant_function"](self.manipulator_plant) self.manipulator_plant.Finalize() self.manipulator_plant_context = \ self.manipulator_plant.CreateDefaultContext() self.nq_manipulator = \ self.manipulator_plant.get_actuation_input_port().size() self.DeclareVectorInputPort("state", BasicVector(self.nq_manipulator * 2)) self.DeclareVectorOutputPort( "q", pydrake.systems.framework.BasicVector(self.nq_manipulator), self.output_q) self.DeclareVectorOutputPort( "v", pydrake.systems.framework.BasicVector(self.nq_manipulator), self.output_v) self.DeclareVectorOutputPort( "tau_g", pydrake.systems.framework.BasicVector(self.nq_manipulator), self.output_tau_g) self.DeclareVectorOutputPort( "M", pydrake.systems.framework.BasicVector( self.nq_manipulator * self.nq_manipulator), self.output_M) self.DeclareVectorOutputPort( "Cv", pydrake.systems.framework.BasicVector(self.nq_manipulator), self.output_Cv) self.DeclareVectorOutputPort( "J", pydrake.systems.framework.BasicVector(6 * self.nq_manipulator), self.output_J) self.DeclareVectorOutputPort( "J_translational", pydrake.systems.framework.BasicVector(3 * self.nq_manipulator), self.output_J_translational) self.DeclareVectorOutputPort( "J_rotational", pydrake.systems.framework.BasicVector(3 * self.nq_manipulator), self.output_J_rotational) self.DeclareVectorOutputPort("Jdot_qdot", pydrake.systems.framework.BasicVector(6), self.output_Jdot_qdot) # =========================== CALC FUNCTIONS ========================== def calc_q(self, context): state = self.GetInputPort("state").Eval(context) q = state[:self.nq_manipulator] return q def calc_v(self, context): state = self.GetInputPort("state").Eval(context) v = state[self.nq_manipulator:] return v def calc_J(self, context): self.update_plant(context) contact_body = self.manipulator_plant.GetBodyByName( manipulator.data["contact_body_name"]) J = self.manipulator_plant.CalcJacobianSpatialVelocity( self.manipulator_plant_context, JacobianWrtVariable.kV, contact_body.body_frame(), [0, 0, 0], self.manipulator_plant.world_frame(), self.manipulator_plant.world_frame()) return J # ========================== OUTPUT FUNCTIONS ========================= def output_q(self, context, output): q = self.calc_q(context) output.SetFromVector(q.flatten()) def output_v(self, context, output): v = self.calc_v(context) output.SetFromVector(v.flatten()) def output_tau_g(self, context, output): self.update_plant(context) tau_g = self.manipulator_plant.CalcGravityGeneralizedForces( self.manipulator_plant_context) output.SetFromVector(tau_g.flatten()) def output_M(self, context, output): self.update_plant(context) M = self.manipulator_plant.CalcMassMatrixViaInverseDynamics( self.manipulator_plant_context) output.SetFromVector(M.flatten()) def output_Cv(self, context, output): self.update_plant(context) Cv = self.manipulator_plant.CalcBiasTerm( self.manipulator_plant_context) output.SetFromVector(Cv.flatten()) def output_J(self, context, output): J = self.calc_J(context) output.SetFromVector(J.flatten()) def output_J_translational(self, context, output): J = self.calc_J(context) J_translational = J[3:, :] output.SetFromVector(J_translational.flatten()) def output_J_rotational(self, context, output): J = self.calc_J(context) J_rotational = J[:3, :] output.SetFromVector(J_rotational.flatten()) def output_Jdot_qdot(self, context, output): self.update_plant(context) contact_body = self.manipulator_plant.GetBodyByName( manipulator.data["contact_body_name"]) Jdot_qdot_raw = self.manipulator_plant.CalcBiasSpatialAcceleration( self.manipulator_plant_context, JacobianWrtVariable.kV, contact_body.body_frame(), [0, 0, 0], self.manipulator_plant.world_frame(), self.manipulator_plant.world_frame()) Jdot_qdot = list(Jdot_qdot_raw.rotational()) + \ list(Jdot_qdot_raw.translational()) output.SetFromVector(Jdot_qdot) # ========================== OTHER FUNCTIONS ========================== def update_plant(self, context): self.manipulator_plant.SetPositions(self.manipulator_plant_context, self.calc_q(context)) self.manipulator_plant.SetVelocities(self.manipulator_plant_context, self.calc_v(context))
# Here we demonstrate how to access body-fixed frames from MultibodyPlant and how to use the frames to perform forward kinematics in pyDrake. We use an acrobot, an underactuated two-link pendulum, as our example system. The description of the acrobot is in a URDF file included in the Drake installation. # To begin, we'll need to: # 1. import a few resources from Drake # 2. Create the MultibodyPlant using the Acrobot URDF # 3. Create a default *context* to store the model parameters from math import pi from pydrake.common import FindResourceOrThrow from pydrake.all import MultibodyPlant, JacobianWrtVariable from pydrake.multibody.parsing import Parser # Create the MultibodyPlant from the URDF # Find and load the Acrobot URDF acro_file = FindResourceOrThrow("drake/examples/acrobot/Acrobot.urdf") # Create a Multibody plant model from the acrobot plant = MultibodyPlant(0.0) acrobot = Parser(plant).AddModelFromFile(acro_file) plant.Finalize() # Get the default context context = plant.CreateDefaultContext() #%% [markdown] # # The acrobot as 4 states: the two relative angles of the links and their respective joint rates. The default context sets all state to zero. To make the kinematics more interesting, we can set a nonzer state vector. plant.SetPositionsAndVelocities(context, [pi/4, pi/4, 0.1, -0.1 ]) #%% [markdown] # # ### Kinematics # We can get the world body and world frame
from math import pi # Import utilities from pydrake from pydrake.common import FindResourceOrThrow from pydrake.all import MultibodyPlant from pydrake.multibody.parsing import Parser #from pydrake.autodiffutils import AutoDiffXd from pydrake.multibody.tree import MultibodyForces #%% [markdown] # # Loading a the acrobot model from a URDF # # The Acrobot model is supplied with the Drake docker image. We can load it and directly create a Drake MultibodyPlant. To fully utilitze the MultibodyPlant, we will also need to create a Context, which stores values associated with the MultibodyPlant such as the configuration and velocity variables. # Find and load the Acrobot URDF acro_file = FindResourceOrThrow("drake/examples/acrobot/Acrobot.urdf") # Create a Multibody plant model from the acrobot plant = MultibodyPlant(0.0) acrobot = Parser(plant).AddModelFromFile(acro_file) plant.Finalize() # The CONTEXT of the system stores information, such as the state, about the multibody plant. The context is necessary to calculate values such as the inertia matrix context = plant.CreateDefaultContext() ##% [markdown] # # The default context is not very interesting, so let's set some nonzero values for the configuration and velocity variables q = [pi / 4, pi / 4] v = [0.1, -0.1] plant.SetPositions(context, q) plant.SetVelocities(context, v) #%% [markdown] # # DYNAMICS # We can get the mass matrix via inverse dynamics (note that CalcMassMatrix is not available in pyDrake):
class Visualizer(): def __init__(self, urdf): self._create_plant(urdf) def _create_plant(self, urdf): self.plant = MultibodyPlant(time_step=0.0) self.scenegraph = SceneGraph() self.plant.RegisterAsSourceForSceneGraph(self.scenegraph) self.model_index = Parser(self.plant).AddModelFromFile(FindResource(urdf)) self.builder = DiagramBuilder() self.builder.AddSystem(self.scenegraph) def _finalize_plant(self): self.plant.Finalize() def _add_trajectory_source(self, traj): # Create the trajectory source source = self.builder.AddSystem(TrajectorySource(traj)) pos2pose = self.builder.AddSystem(MultibodyPositionToGeometryPose(self.plant, input_multibody_state=True)) # Wire the source to the scene graph self.builder.Connect(source.get_output_port(0), pos2pose.get_input_port()) self.builder.Connect(pos2pose.get_output_port(), self.scenegraph.get_source_pose_port(self.plant.get_source_id())) def _add_renderer(self): renderer_name = "renderer" self.scenegraph.AddRenderer(renderer_name, MakeRenderEngineVtk(RenderEngineVtkParams())) # Add a camera depth_camera = DepthRenderCamera( RenderCameraCore( renderer_name, CameraInfo(width=640, height=480, fov_y=np.pi/4), ClippingRange(0.01, 10.0), RigidTransform()), DepthRange(0.01,10.0) ) world_id = self.plant.GetBodyFrameIdOrThrow(self.plant.world_body().index()) X_WB = xyz_rpy_deg([4,0,0],[-90,0,90]) sensor = RgbdSensor(world_id, X_PB=X_WB, depth_camera=depth_camera) self.builder.AddSystem(sensor) self.builder.Connect(self.scenegraph.get_query_output_port(), sensor.query_object_input_port()) def _connect_visualizer(self): DrakeVisualizer.AddToBuilder(self.builder, self.scenegraph) self.meshcat = ConnectMeshcatVisualizer(self.builder, self.scenegraph, zmq_url="new", open_browser=False) self.meshcat.vis.jupyter_cell() def _make_visualization(self, stop_time): simulator = Simulator(self.builder.Build()) simulator.Initialize() self.meshcat.vis.render_static() # Set simulator context simulator.get_mutable_context().SetTime(0.0) # Start recording and simulate the diagram self.meshcat.reset_recording() self.meshcat.start_recording() simulator.AdvanceTo(stop_time) # Publish the recording self.meshcat.publish_recording() # Render self.meshcat.vis.render_static() input("View visualization. Press <ENTER> to end") print("Finished") def visualize_trajectory(self, xtraj=None): self._finalize_plant() print("Adding trajectory source") xtraj = self._check_trajectory(xtraj) self._add_trajectory_source(xtraj) print("Adding renderer") self._add_renderer() print("Connecting to MeshCat") self._connect_visualizer() print("Making visualization") self._make_visualization(xtraj.end_time()) def _check_trajectory(self, traj): if traj is None: plant_context = self.plant.CreateDefaultContext() pose = self.plant.GetPositions(plant_context) pose = np.column_stack((pose, pose)) pose = zero_pad_rows(pose, self.plant.num_positions() + self.plant.num_velocities()) return PiecewisePolynomial.FirstOrderHold([0., 1.], pose) elif type(traj) is np.ndarray: if traj.ndim == 1: traj = np.column_stack(traj, traj) traj = zero_pad_rows(traj, self.plant.num_positions() + self.plant.num_velocities()) return PiecewisePolynomial.FirstOrderHold([0.,1.], traj) elif type(traj) is PiecewisePolynomial: breaks = traj.get_segment_times() values = traj.vector_values(breaks) values = zero_pad_rows(values, self.plant.num_positions() + self.plant.num_velocities()) return PiecewisePolynomial.FirstOrderHold(breaks, values) else: raise ValueError("Trajectory must be a piecewise polynomial, an ndarray, or None")
def MakeManipulationStation(time_step=0.002, plant_setup_callback=None, camera_prefix="camera"): """ Sets up a manipulation station with the iiwa + wsg + controllers [+ cameras]. Cameras will be added to each body with a name starting with "camera". Args: time_step: the standard MultibodyPlant time step. plant_setup_callback: should be a python callable that takes one argument: `plant_setup_callback(plant)`. It will be called after the iiwa and WSG are added to the plant, but before the plant is finalized. Use this to add additional geometry. camera_prefix: Any bodies in the plant (created during the plant_setup_callback) starting with this prefix will get a camera attached. """ builder = DiagramBuilder() # Add (only) the iiwa, WSG, and cameras to the scene. plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=time_step) iiwa = AddIiwa(plant) wsg = AddWsg(plant, iiwa) if plant_setup_callback: plant_setup_callback(plant) plant.Finalize() num_iiwa_positions = plant.num_positions(iiwa) # I need a PassThrough system so that I can export the input port. iiwa_position = builder.AddSystem(PassThrough(num_iiwa_positions)) builder.ExportInput(iiwa_position.get_input_port(), "iiwa_position") builder.ExportOutput(iiwa_position.get_output_port(), "iiwa_position_command") # Export the iiwa "state" outputs. demux = builder.AddSystem( Demultiplexer(2 * num_iiwa_positions, num_iiwa_positions)) builder.Connect(plant.get_state_output_port(iiwa), demux.get_input_port()) builder.ExportOutput(demux.get_output_port(0), "iiwa_position_measured") builder.ExportOutput(demux.get_output_port(1), "iiwa_velocity_estimated") builder.ExportOutput(plant.get_state_output_port(iiwa), "iiwa_state_estimated") # Make the plant for the iiwa controller to use. controller_plant = MultibodyPlant(time_step=time_step) controller_iiwa = AddIiwa(controller_plant) AddWsg(controller_plant, controller_iiwa, welded=True) controller_plant.Finalize() # Add the iiwa controller iiwa_controller = builder.AddSystem( InverseDynamicsController(controller_plant, kp=[100] * num_iiwa_positions, ki=[1] * num_iiwa_positions, kd=[20] * num_iiwa_positions, has_reference_acceleration=False)) iiwa_controller.set_name("iiwa_controller") builder.Connect(plant.get_state_output_port(iiwa), iiwa_controller.get_input_port_estimated_state()) # Add in the feed-forward torque adder = builder.AddSystem(Adder(2, num_iiwa_positions)) builder.Connect(iiwa_controller.get_output_port_control(), adder.get_input_port(0)) # Use a PassThrough to make the port optional (it will provide zero values # if not connected). torque_passthrough = builder.AddSystem(PassThrough([0] * num_iiwa_positions)) builder.Connect(torque_passthrough.get_output_port(), adder.get_input_port(1)) builder.ExportInput(torque_passthrough.get_input_port(), "iiwa_feedforward_torque") builder.Connect(adder.get_output_port(), plant.get_actuation_input_port(iiwa)) # Add discrete derivative to command velocities. desired_state_from_position = builder.AddSystem( StateInterpolatorWithDiscreteDerivative( num_iiwa_positions, time_step, suppress_initial_transient=True)) desired_state_from_position.set_name("desired_state_from_position") builder.Connect(desired_state_from_position.get_output_port(), iiwa_controller.get_input_port_desired_state()) builder.Connect(iiwa_position.get_output_port(), desired_state_from_position.get_input_port()) # Export commanded torques. builder.ExportOutput(adder.get_output_port(), "iiwa_torque_commanded") builder.ExportOutput(adder.get_output_port(), "iiwa_torque_measured") builder.ExportOutput(plant.get_generalized_contact_forces_output_port(iiwa), "iiwa_torque_external") # Wsg controller. wsg_controller = builder.AddSystem(SchunkWsgPositionController()) wsg_controller.set_name("wsg_controller") builder.Connect(wsg_controller.get_generalized_force_output_port(), plant.get_actuation_input_port(wsg)) builder.Connect(plant.get_state_output_port(wsg), wsg_controller.get_state_input_port()) builder.ExportInput(wsg_controller.get_desired_position_input_port(), "wsg_position") builder.ExportInput(wsg_controller.get_force_limit_input_port(), "wsg_force_limit") wsg_mbp_state_to_wsg_state = builder.AddSystem( MakeMultibodyStateToWsgStateSystem()) builder.Connect(plant.get_state_output_port(wsg), wsg_mbp_state_to_wsg_state.get_input_port()) builder.ExportOutput(wsg_mbp_state_to_wsg_state.get_output_port(), "wsg_state_measured") builder.ExportOutput(wsg_controller.get_grip_force_output_port(), "wsg_force_measured") # Cameras. AddRgbdSensors(builder, plant, scene_graph, model_instance_prefix=camera_prefix) # Export "cheat" ports. builder.ExportOutput(scene_graph.get_query_output_port(), "geometry_query") builder.ExportOutput(plant.get_contact_results_output_port(), "contact_results") builder.ExportOutput(plant.get_state_output_port(), "plant_continuous_state") builder.ExportOutput(plant.get_body_poses_output_port(), "body_poses") diagram = builder.Build() diagram.set_name("ManipulationStation") return diagram
import math import numpy as np import matplotlib.pyplot as plt from pydrake.all import (DiagramBuilder, DirectCollocation, MultibodyPlant, MultibodyPositionToGeometryPose, Parser, PiecewisePolynomial, PlanarSceneGraphVisualizer, SceneGraph, Simulator, Solve, TrajectorySource) from underactuated import FindResource plant = MultibodyPlant(time_step=0.0) scene_graph = SceneGraph() plant.RegisterAsSourceForSceneGraph(scene_graph) file_name = FindResource("models/cartpole.urdf") Parser(plant).AddModelFromFile(file_name) plant.Finalize() context = plant.CreateDefaultContext() dircol = DirectCollocation( plant, context, num_time_samples=21, minimum_timestep=0.1, maximum_timestep=0.4, input_port_index=plant.get_actuation_input_port().get_index()) dircol.AddEqualTimeIntervalsConstraints() initial_state = (0., 0., 0., 0.) dircol.AddBoundingBoxConstraint(initial_state, initial_state, dircol.initial_state())
from pydrake.all import MultibodyPlant, Parser, UniformGravityFieldElement from underactuated import FindResource, ManipulatorDynamics plant = MultibodyPlant() parser = Parser(plant) parser.AddModelFromFile(FindResource("double_pendulum/double_pendulum.urdf")) plant.AddForceElement(UniformGravityFieldElement()) plant.Finalize() q = [0.1, 0.1] v = [1, 1] (M, Cv, tauG, B, tauExt) = ManipulatorDynamics(plant, q, v) print("M = \n" + str(M)) print("Cv = " + str(Cv)) print("tau_G = " + str(tauG)) print("B = " + str(B)) print("tau_ext = " + str(tauExt)) # TODO(russt): add symbolic version pending resolution of drake #11240