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
