# Imports
import numpy as np
import altair as alt
import pydot
from IPython.display import display, SVG, clear_output
import plotly.express as px
from pydrake.all import (
AddMultibodyPlantSceneGraph, AngleAxis, BasicVector,
DiagramBuilder, FindResourceOrThrow, Integrator, JacobianWrtVariable,
LeafSystem, MeshcatVisualizerCpp, MultibodyPlant, MultibodyPositionToGeometryPose, Parser,
PiecewisePose, PiecewisePolynomial, Quaternion, RigidTransform,
RollPitchYaw, RotationMatrix, SceneGraph, Simulator, TrajectorySource
)
from pydrake.examples.manipulation_station import ManipulationStation
from manipulation import running_as_notebook
from manipulation.meshcat_cpp_utils import (
StartMeshcat, MeshcatJointSlidersThatPublish)
from manipulation.scenarios import AddMultibodyTriad, SetColor
# TODO(russt): Move this to drake (adding the element name support to the base class).
import pandas as pd
def dataframe(trajectory, times, names):
assert trajectory.rows() == len(names)
values = trajectory.vector_values(times)
data = {'t': times }
for i in range(len(names)):
data[names[i]] = values[i,:]
return pd.DataFrame(data)
# Start the visualizer.
meshcat = StartMeshcat()
Meshcat is now available at https://65aad364-ef1c-45f5-a796-fac7c122e274.deepnoteproject.com
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()
# TODO(russt): Add floating base connections
# TODO(russt): Consider a more interactive javascript rendering?
display(SVG(pydot.graph_from_dot_data(plant.GetTopologyGraphvizString())[0].create_svg()))
kinematic_tree_example()
def gripper_forward_kinematics_example():
builder = DiagramBuilder()
# TODO: Replace this with a simple model directive of iiwa+wsg (no clutter bins)
station = builder.AddSystem(ManipulationStation())
station.SetupClutterClearingStation()
station.Finalize()
plant = station.get_multibody_plant()
scene_graph = station.get_scene_graph()
# Draw the frames
for body_name in ["iiwa_link_1", "iiwa_link_2", "iiwa_link_3", "iiwa_link_4", "iiwa_link_5", "iiwa_link_6", "iiwa_link_7", "body"]:
AddMultibodyTriad(plant.GetFrameByName(body_name), scene_graph)
meshcat.Delete()
visualizer = MeshcatVisualizerCpp.AddToBuilder(
builder, station.GetOutputPort("query_object"), meshcat)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
gripper = plant.GetBodyByName("body")
def pose_callback(context):
pose = plant.EvalBodyPoseInWorld(context, gripper) ## This is the important line
clear_output(wait=True)
print("gripper position (m): " + np.array2string(
pose.translation(), formatter={
'float': lambda x: "{:3.2f}".format(x)}))
print("gripper roll-pitch-yaw (rad):" + np.array2string(
RollPitchYaw(pose.rotation()).vector(),
formatter={'float': lambda x: "{:3.2f}".format(x)}))
sliders = MeshcatJointSlidersThatPublish(
meshcat, plant, visualizer, context)
sliders.Run(pose_callback)
gripper_forward_kinematics_example()
Unsupported output type: clearOutput
gripper position (m): [0.00 -0.47 0.66]
gripper roll-pitch-yaw (rad):[-1.81 -0.00 0.00]
meshcat.DeleteAddedControls()
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()}")
num_positions_velocities_example()
::_ Context
------------
Time: 0
States:
13 continuous states
1 0 0 0 0 0 0 0 0 0 0 0 0
Parameters:
3 numeric parameter groups with
10 parameters
nan nan nan nan nan nan nan nan nan nan
10 parameters
0.028 0 0 0.025 0.00104286 0.00130357 0.000678571 0 0 0
12 parameters
1 0 0 0 1 0 0 0 1 0 0 0
2 abstract parameters
plant.num_positions() = 7
plant.num_velocities() = 6
def pick_and_place_jacobians_example():
builder = DiagramBuilder()
# TODO: Replace this with a simple model directive of iiwa+wsg (no clutter bins)
station = builder.AddSystem(ManipulationStation())
station.SetupClutterClearingStation()
station.Finalize()
meshcat.Delete()
visualizer = MeshcatVisualizerCpp.AddToBuilder(
builder, station.GetOutputPort("query_object"), meshcat)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant = station.get_multibody_plant()
G = plant.GetBodyByName("body").body_frame()
W = plant.world_frame()
def pose_callback(context):
J_G = plant.CalcJacobianSpatialVelocity(context, JacobianWrtVariable.kQDot, G, [0,0,0], W, W) ## This is the important line
clear_output(wait=True)
print("J_G:")
print(np.array2string(J_G, formatter={
'float': lambda x: "{:5.1f}".format(x)}))
print(
f"smallest singular value(J_G): {np.min(np.linalg.svd(J_G, compute_uv=False))}")
# If you want to set the initial positions manually, use this:
#plant.SetPositions(plant.GetMyContextFromRoot(context),
# plant.GetModelInstanceByName("iiwa"),
# [0, 0, 0, 0, 0, 0, 0])
sliders = MeshcatJointSlidersThatPublish(
meshcat, plant, visualizer, context)
sliders.Run(pose_callback)
pick_and_place_jacobians_example()
Unsupported output type: clearOutput
J_G:
[[ 0.0 1.0 0.0 -1.0 0.0 1.0 0.0 0.0 0.0]
[ 0.0 0.0 -0.1 -0.0 -1.0 0.0 -0.2 0.0 0.0]
[ 1.0 0.0 1.0 -0.0 0.3 0.0 -1.0 0.0 0.0]
[ 0.5 0.0 0.4 0.0 0.2 -0.0 -0.0 0.0 0.0]
[ 0.0 -0.3 0.0 -0.1 0.0 0.2 -0.0 0.0 0.0]
[ 0.0 -0.5 0.0 0.4 0.0 -0.0 -0.0 0.0 0.0]]
smallest singular value(J_G): 0.17931039621645184
meshcat.DeleteAddedControls()
# We can write a new System by deriving from the LeafSystem class.
# There is a little bit of boiler plate, but hopefully this example makes sense.
class PseudoInverseController(LeafSystem):
def __init__(self, plant):
LeafSystem.__init__(self)
self._plant = plant
self._plant_context = plant.CreateDefaultContext()
self._iiwa = plant.GetModelInstanceByName("iiwa")
self._G = plant.GetBodyByName("body").body_frame()
self._W = plant.world_frame()
self.DeclareVectorInputPort("iiwa_position", BasicVector(7))
self.DeclareVectorOutputPort("iiwa_velocity", BasicVector(7),
self.CalcOutput)
def CalcOutput(self, context, output):
q = self.get_input_port().Eval(context)
self._plant.SetPositions(self._plant_context, self._iiwa, q)
J_G = self._plant.CalcJacobianSpatialVelocity(
self._plant_context, JacobianWrtVariable.kQDot,
self._G, [0,0,0], self._W, self._W)
J_G = J_G[:,0:7] # Ignore gripper terms
V_G_desired = np.array([0, # rotation about x
-.1, # rotation about y
0, # rotation about z
0, # x
-.05, # y
-.1]) # z
v = np.linalg.pinv(J_G).dot(V_G_desired)
output.SetFromVector(v)
def jacobian_controller_example():
builder = DiagramBuilder()
station = builder.AddSystem(ManipulationStation())
station.SetupClutterClearingStation()
station.Finalize()
controller = builder.AddSystem(PseudoInverseController(
station.get_multibody_plant()))
integrator = builder.AddSystem(Integrator(7))
builder.Connect(controller.get_output_port(),
integrator.get_input_port())
builder.Connect(integrator.get_output_port(),
station.GetInputPort("iiwa_position"))
builder.Connect(station.GetOutputPort("iiwa_position_measured"),
controller.get_input_port())
meshcat.Delete()
visualizer = MeshcatVisualizerCpp.AddToBuilder(
builder, station.GetOutputPort("query_object"), meshcat)
diagram = builder.Build()
simulator = Simulator(diagram)
station_context = station.GetMyContextFromRoot(simulator.get_mutable_context())
station.GetInputPort("iiwa_feedforward_torque").FixValue(station_context, np.zeros((7,1)))
station.GetInputPort("wsg_position").FixValue(station_context, [0.1])
integrator.set_integral_value(
integrator.GetMyContextFromRoot(simulator.get_mutable_context()),
station.GetIiwaPosition(station_context))
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(0.01);
return simulator
simulator = jacobian_controller_example()
# Wait for the model to load in the visualizer, then run this cell to see the interesting part...
simulator.AdvanceTo(5.0 if running_as_notebook else 0.1);
def grasp_poses_example():
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step = 0.0)
parser = Parser(plant, scene_graph)
grasp = parser.AddModelFromFile(FindResourceOrThrow(
"drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf"), "grasp")
pregrasp = parser.AddModelFromFile(FindResourceOrThrow(
"drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf"), "pregrasp")
brick = parser.AddModelFromFile(FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf"))
plant.Finalize()
B_O = plant.GetBodyByName("base_link", brick)
B_Ggrasp = plant.GetBodyByName("body", grasp)
B_Gpregrasp = plant.GetBodyByName("body", pregrasp)
# Set the pregrasp to be green and slightly transparent.
inspector = scene_graph.model_inspector()
for body_index in plant.GetBodyIndices(pregrasp):
SetColor(
scene_graph, [0, .6, 0, .5], plant.get_source_id(),
inspector.GetGeometries(plant.GetBodyFrameIdOrThrow(body_index)))
meshcat.Delete()
meshcat.SetProperty("/Background", "visible", False)
visualizer = MeshcatVisualizerCpp.AddToBuilder(
builder, scene_graph, meshcat)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant_context = plant.GetMyContextFromRoot(context)
# TODO(russt): Set a random pose of the object.
# Get the current object, O, pose
X_WO = plant.EvalBodyPoseInWorld(plant_context, B_O)
p_GgraspO = [0, 0.12, 0]
R_GgraspO = RotationMatrix.MakeXRotation(np.pi/2.0).multiply(
RotationMatrix.MakeZRotation(np.pi/2.0))
X_GgraspO = RigidTransform(R_GgraspO, p_GgraspO)
X_OGgrasp = X_GgraspO.inverse()
X_WGgrasp = X_WO.multiply(X_OGgrasp)
# pregrasp is negative y in the gripper frame (see the figure!).
X_GgraspGpregrasp = RigidTransform([0, -0.08, 0])
X_WGpregrasp = X_WGgrasp @ X_GgraspGpregrasp
plant.SetFreeBodyPose(plant_context, B_Ggrasp, X_WGgrasp)
# Open the fingers, too.
plant.GetJointByName("left_finger_sliding_joint", grasp).set_translation(plant_context, -0.054)
plant.GetJointByName("right_finger_sliding_joint", grasp).set_translation(plant_context, 0.054)
plant.SetFreeBodyPose(plant_context, B_Gpregrasp, X_WGpregrasp)
# Open the fingers, too.
plant.GetJointByName("left_finger_sliding_joint", pregrasp).set_translation(plant_context, -0.054)
plant.GetJointByName("right_finger_sliding_joint", pregrasp).set_translation(plant_context, 0.054)
diagram.Publish(context)
grasp_poses_example()
def make_gripper_frames(X_G, X_O):
"""
Takes a partial specification with X_G["initial"] and X_O["initial"] and X_0["goal"], and
returns a X_G and times with all of the pick and place frames populated.
"""
# Define (again) the gripper pose relative to the object when in grasp.
p_GgraspO = [0, 0.12, 0]
R_GgraspO = RotationMatrix.MakeXRotation(np.pi/2.0).multiply(
RotationMatrix.MakeZRotation(np.pi/2.0))
X_GgraspO = RigidTransform(R_GgraspO, p_GgraspO)
X_OGgrasp = X_GgraspO.inverse()
# pregrasp is negative y in the gripper frame (see the figure!).
X_GgraspGpregrasp = RigidTransform([0, -0.08, 0])
X_G["pick"] = X_O["initial"].multiply(X_OGgrasp)
X_G["prepick"] = X_G["pick"].multiply(X_GgraspGpregrasp)
X_G["place"] = X_O["goal"].multiply(X_OGgrasp)
X_G["preplace"] = X_G["place"].multiply(X_GgraspGpregrasp)
# I'll interpolate a halfway orientation by converting to axis angle and halving the angle.
X_GprepickGpreplace = X_G["prepick"].inverse().multiply(X_G["preplace"])
angle_axis = X_GprepickGpreplace.rotation().ToAngleAxis()
X_GprepickGclearance = RigidTransform(AngleAxis(angle=angle_axis.angle()/2.0, axis=angle_axis.axis()),
X_GprepickGpreplace.translation()/2.0 + np.array([0, -0.3, 0]))
X_G["clearance"] = X_G["prepick"].multiply(X_GprepickGclearance)
# Now let's set the timing
times = {"initial": 0}
X_GinitialGprepick = X_G["initial"].inverse().multiply(X_G["prepick"])
times["prepick"] = times["initial"] + 10.0*np.linalg.norm(X_GinitialGprepick.translation())
# Allow some time for the gripper to close.
times["pick_start"] = times["prepick"] + 2.0
times["pick_end"] = times["pick_start"] + 2.0
X_G["pick_start"] = X_G["pick"]
X_G["pick_end"] = X_G["pick"]
times["postpick"] = times["pick_end"] + 2.0
X_G["postpick"] = X_G["prepick"]
time_to_from_clearance = 10.0*np.linalg.norm(X_GprepickGclearance.translation())
times["clearance"] = times["postpick"] + time_to_from_clearance
times["preplace"] = times["clearance"] + time_to_from_clearance
times["place_start"] = times["preplace"] + 2.0
times["place_end"] = times["place_start"] + 2.0
X_G["place_start"] = X_G["place"]
X_G["place_end"] = X_G["place"]
times["postplace"] = times["place_end"] + 2.0
X_G["postplace"] = X_G["preplace"]
return X_G, times
X_G = {"initial": RigidTransform(RotationMatrix.MakeXRotation(-np.pi/2.0), [0, -0.25, 0.25])}
X_O = {"initial": RigidTransform(RotationMatrix.MakeZRotation(np.pi/2.0), [-.2, -.75, 0.025]),
"goal": RigidTransform(RotationMatrix.MakeZRotation(np.pi),[.75, 0, 0.025])}
X_G, times = make_gripper_frames(X_G, X_O)
print(f"Sanity check: The entire maneuver will take {times['postplace']} seconds to execute.")
Sanity check: The entire maneuver will take 30.900220749318255 seconds to execute.
def visualize_gripper_frames(X_G, X_O):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step = 0.0)
parser = Parser(plant, scene_graph)
gripper = FindResourceOrThrow(
"drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf")
brick = FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf")
for key, pose in X_G.items():
g = parser.AddModelFromFile(gripper, f"gripper_{key}")
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("body", g), pose)
for key, pose in X_O.items():
o = parser.AddModelFromFile(brick, f"object_{key}")
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base_link", o), pose)
plant.Finalize()
meshcat.Delete()
visualizer = MeshcatVisualizerCpp.AddToBuilder(
builder, scene_graph, meshcat)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
diagram.Publish(context)
visualize_gripper_frames(X_G, X_O)
def make_gripper_trajectory(X_G, times):
"""
Constructs a gripper position trajectory from the plan "sketch".
"""
sample_times = []
poses = []
for name in ["initial", "prepick", "pick_start", "pick_end", "postpick",
"clearance", "preplace", "place_start", "place_end",
"postplace"]:
sample_times.append(times[name])
poses.append(X_G[name])
return PiecewisePose.MakeLinear(sample_times, poses)
traj = make_gripper_trajectory(X_G, times)
traj_p_G = traj.get_position_trajectory()
data = dataframe(traj_p_G, traj_p_G.get_segment_times(), ['x','y','z'])
alt.vconcat(
alt.Chart(data).mark_line().encode(x='t', y='x').properties(height=80),
alt.Chart(data).mark_line().encode(x='t', y='y').properties(height=80),
alt.Chart(data).mark_line().encode(x='t', y='z').properties(height=80),
)
data = dataframe(traj_p_G, np.linspace(traj_p_G.start_time(), traj_p_G.end_time(), 50), ['x','y','z'])
fig = px.line_3d(data, x='x', y='y', z='z')
fig.update_traces(mode='lines+markers',
marker=dict(size=3, color="rgb(153, 51, 51)"),
line=dict(color="rgb(153, 51, 51)"))
if running_as_notebook:
fig.show()
traj_R_G = traj.get_orientation_trajectory()
data = dataframe(traj_R_G, traj_R_G.get_segment_times(), ['x','y','z','w'])
alt.vconcat(
alt.Chart(data).mark_line().encode(x='t', y='x').properties(height=80),
alt.Chart(data).mark_line().encode(x='t', y='y').properties(height=80),
alt.Chart(data).mark_line().encode(x='t', y='z').properties(height=80),
alt.Chart(data).mark_line().encode(x='t', y='w').properties(height=80)
)
opened = np.array([0.107]);
closed = np.array([0.0]);
def make_wsg_command_trajectory(times):
traj_wsg_command = PiecewisePolynomial.FirstOrderHold(
[times["initial"], times["pick_start"]], np.hstack([[opened], [opened]]))
traj_wsg_command.AppendFirstOrderSegment(times["pick_end"], closed)
traj_wsg_command.AppendFirstOrderSegment(times["place_start"], closed)
traj_wsg_command.AppendFirstOrderSegment(times["place_end"], opened)
traj_wsg_command.AppendFirstOrderSegment(times["postplace"], opened)
return traj_wsg_command
traj_wsg_command = make_wsg_command_trajectory(times)
data = dataframe(traj_wsg_command, traj_wsg_command.get_segment_times(), ['wsg'])
alt.Chart(data).mark_line().encode(x='t', y='wsg').properties(height=80)
class GripperTrajectoriesToPosition(LeafSystem):
def __init__(self, plant, traj_p_G, traj_R_G, traj_wsg_command):
LeafSystem.__init__(self)
self.plant = plant
self.gripper_body = plant.GetBodyByName("body")
self.left_finger_joint = plant.GetJointByName("left_finger_sliding_joint")
self.right_finger_joint = plant.GetJointByName("right_finger_sliding_joint")
self.traj_p_G = traj_p_G
self.traj_R_G = traj_R_G
self.traj_wsg_command = traj_wsg_command
self.plant_context = plant.CreateDefaultContext()
self.DeclareVectorOutputPort("position",
BasicVector(plant.num_positions()),
self.CalcPositionOutput)
def CalcPositionOutput(self, context, output):
t = context.get_time()
X_G = RigidTransform(Quaternion(self.traj_R_G.value(t)), self.traj_p_G.value(t))
self.plant.SetFreeBodyPose(self.plant_context, self.gripper_body, X_G)
wsg = self.traj_wsg_command.value(t)
self.left_finger_joint.set_translation(self.plant_context, -wsg/2.0)
self.right_finger_joint.set_translation(self.plant_context, wsg/2.0)
output.SetFromVector(self.plant.GetPositions(self.plant_context))
def visualize_pick_and_place_trajectory(traj_p_G, traj_R_G, traj_wsg_command, X_O):
builder = DiagramBuilder()
# Note: Don't use AddMultibodyPlantSceneGraph because we are only using
# MultibodyPlant for parsing, then wiring directly to SceneGraph.
scene_graph = builder.AddSystem(SceneGraph())
plant = MultibodyPlant(time_step=0.0)
plant.RegisterAsSourceForSceneGraph(scene_graph)
parser = Parser(plant, scene_graph)
gripper = parser.AddModelFromFile(FindResourceOrThrow(
"drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf"), "gripper")
brick = FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf")
for key, pose in X_O.items():
o = parser.AddModelFromFile(brick, f"object_{key}")
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base_link", o), pose)
plant.Finalize()
to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(plant))
builder.Connect(to_pose.get_output_port(), scene_graph.get_source_pose_port(plant.get_source_id()))
traj_to_position = builder.AddSystem(GripperTrajectoriesToPosition(plant, traj_p_G, traj_R_G, traj_wsg_command))
builder.Connect(traj_to_position.get_output_port(), to_pose.get_input_port())
meshcat.Delete()
visualizer = MeshcatVisualizerCpp.AddToBuilder(
builder, scene_graph, meshcat)
diagram = builder.Build()
simulator = Simulator(diagram)
# TODO(russt): get recording working again with new Meshcat (and remove realtime rate)
#meshcat.start_recording()
if running_as_notebook:
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(traj_p_G.end_time() if running_as_notebook else 0.1)
#meshcat.publish_recording()
visualize_pick_and_place_trajectory(traj_p_G, traj_R_G, traj_wsg_command, X_O)
traj_v_G = traj_p_G.MakeDerivative()
traj_w_G = traj_R_G.MakeDerivative()
data = dataframe(traj_v_G, np.linspace(traj_v_G.start_time(), traj_v_G.end_time()), ['x','y','z'])
alt.vconcat(
alt.Chart(data).mark_line().encode(x='t', y='x').properties(height=80),
alt.Chart(data).mark_line().encode(x='t', y='y').properties(height=80),
alt.Chart(data).mark_line().encode(x='t', y='z').properties(height=80),
)
# We can write a new System by deriving from the LeafSystem class.
# There is a little bit of boiler plate, but hopefully this example makes sense.
class PseudoInverseController(LeafSystem):
def __init__(self, plant):
LeafSystem.__init__(self)
self._plant = plant
self._plant_context = plant.CreateDefaultContext()
self._iiwa = plant.GetModelInstanceByName("iiwa")
self._G = plant.GetBodyByName("body").body_frame()
self._W = plant.world_frame()
self.w_G_port = self.DeclareVectorInputPort("omega_WG", BasicVector(3))
self.v_G_port = self.DeclareVectorInputPort("v_WG", BasicVector(3))
self.q_port = self.DeclareVectorInputPort("iiwa_position", BasicVector(7))
self.DeclareVectorOutputPort("iiwa_velocity", BasicVector(7),
self.CalcOutput)
# TODO(russt): Add missing binding
#joint_indices = plant.GetJointIndices(self._iiwa)
#self.position_start = plant.get_joint(joint_indices[0]).position_start()
#self.position_end = plant.get_joint(joint_indices[-1]).position_start()
self.iiwa_start = plant.GetJointByName("iiwa_joint_1").velocity_start()
self.iiwa_end = plant.GetJointByName("iiwa_joint_7").velocity_start()
def CalcOutput(self, context, output):
w_G = self.w_G_port.Eval(context)
v_G = self.v_G_port.Eval(context)
V_G = np.hstack([w_G, v_G])
q = self.q_port.Eval(context)
self._plant.SetPositions(self._plant_context, self._iiwa, q)
J_G = self._plant.CalcJacobianSpatialVelocity(
self._plant_context, JacobianWrtVariable.kV,
self._G, [0,0,0], self._W, self._W)
J_G = J_G[:,self.iiwa_start:self.iiwa_end+1] # Only iiwa terms.
v = np.linalg.pinv(J_G).dot(V_G)
output.SetFromVector(v)
X_O = {"initial": RigidTransform(RotationMatrix.MakeZRotation(np.pi/2.0), [-.2, -.65, 0.09]),
"goal": RigidTransform(RotationMatrix.MakeZRotation(np.pi),[.5, 0, 0.09])}
builder = DiagramBuilder()
station = builder.AddSystem(ManipulationStation())
station.SetupClutterClearingStation()
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/061_foam_brick.sdf",
X_O["initial"])
station.Finalize()
# Find the initial pose of the gripper (as set in the default Context)
temp_context = station.CreateDefaultContext()
plant = station.get_multibody_plant()
temp_plant_context = plant.GetMyContextFromRoot(temp_context)
X_G = {"initial": plant.EvalBodyPoseInWorld(temp_plant_context, plant.GetBodyByName("body"))}
X_G, times = make_gripper_frames(X_G, X_O)
print(f"Sanity check: The entire maneuver will take {times['postplace']} seconds to execute.")
# Make the trajectories
traj = make_gripper_trajectory(X_G, times)
# TODO(russt): Offer PiecewisePose get_velocity_trajectory().
traj_v_G = traj.get_position_trajectory().MakeDerivative()
traj_w_G = traj.get_orientation_trajectory().MakeDerivative()
v_G_source = builder.AddSystem(TrajectorySource(traj_v_G))
v_G_source.set_name("v_WG")
w_G_source = builder.AddSystem(TrajectorySource(traj_w_G))
w_G_source.set_name("omega_WG")
controller = builder.AddSystem(PseudoInverseController(plant))
controller.set_name("PseudoInverseController")
builder.Connect(v_G_source.get_output_port(), controller.GetInputPort("v_WG"))
builder.Connect(w_G_source.get_output_port(), controller.GetInputPort("omega_WG"))
integrator = builder.AddSystem(Integrator(7))
integrator.set_name("integrator")
builder.Connect(controller.get_output_port(),
integrator.get_input_port())
builder.Connect(integrator.get_output_port(),
station.GetInputPort("iiwa_position"))
builder.Connect(station.GetOutputPort("iiwa_position_measured"),
controller.GetInputPort("iiwa_position"))
traj_wsg_command = make_wsg_command_trajectory(times)
wsg_source = builder.AddSystem(TrajectorySource(traj_wsg_command))
wsg_source.set_name("wsg_command")
builder.Connect(wsg_source.get_output_port(), station.GetInputPort("wsg_position"))
meshcat.Delete()
visualizer = MeshcatVisualizerCpp.AddToBuilder(
builder, station.GetOutputPort("query_object"), meshcat)
diagram = builder.Build()
diagram.set_name("pick_and_place")
simulator = Simulator(diagram)
station_context = station.GetMyContextFromRoot(simulator.get_mutable_context())
# TODO(russt): Add this missing python binding
#integrator.set_integral_value(
# integrator.GetMyContextFromRoot(simulator.get_mutable_context()),
# station.GetIiwaPosition(station_context))
integrator.GetMyContextFromRoot(simulator.get_mutable_context()).get_mutable_continuous_state_vector().SetFromVector(station.GetIiwaPosition(station_context))
if running_as_notebook:
simulator.set_target_realtime_rate(1.0)
#meshcat.start_recording()
simulator.AdvanceTo(traj_p_G.end_time() if running_as_notebook else 0.1)
#meshcat.stop_recording()
#meshcat.publish_recording()
Sanity check: The entire maneuver will take 27.819595226260763 seconds to execute.
SVG(pydot.graph_from_dot_data(diagram.GetGraphvizString(max_depth=1))[0].create_svg())