def test_clutter_clearing_setup(self):
station = ManipulationStation(time_step=0.001)
station.SetupClutterClearingStation()
num_station_bodies = (
station.get_multibody_plant().num_model_instances())
ycb_objects = CreateDefaultYcbObjectList()
for model_file, X_WObject in ycb_objects:
station.AddManipulandFromFile(model_file, X_WObject)
station.Finalize()
context = station.CreateDefaultContext()
q = np.linspace(0.04, 0.6, num=7)
v = np.linspace(-2.3, 0.5, num=7)
station.SetIiwaPosition(context, q)
np.testing.assert_array_equal(q, station.GetIiwaPosition(context))
station.SetIiwaVelocity(context, v)
np.testing.assert_array_equal(v, station.GetIiwaVelocity(context))
q = 0.0423
v = 0.0851
station.SetWsgPosition(context, q)
self.assertEqual(q, station.GetWsgPosition(context))
station.SetWsgVelocity(context, v)
self.assertEqual(v, station.GetWsgVelocity(context))
self.assertEqual(len(station.get_camera_names()), 1)
self.assertEqual(station.get_multibody_plant().num_model_instances(),
num_station_bodies + len(ycb_objects))
def main():
station = ManipulationStation()
station.SetupManipulationClassStation()
station.Finalize()
context = station.CreateDefaultContext()
print(context)
def __init__(self, time_step,
object_file_path=None,
object_base_link_name=None,
X_WObject=X_WObject_default,):
self.object_base_link_name = object_base_link_name
self.time_step = time_step
# Finalize manipulation station by adding manipuland.
self.station = ManipulationStation(self.time_step)
self.station.AddCupboard()
self.plant = self.station.get_mutable_multibody_plant()
if object_file_path is not None:
self.object = AddModelFromSdfFile(
file_name=object_file_path,
model_name="object",
plant=self.station.get_mutable_multibody_plant(),
scene_graph=self.station.get_mutable_scene_graph() )
self.station.Finalize()
self.tree = self.plant.tree()
self.simulator = None
self.plan_runner = None
# Initial pose of the object
self.X_WObject = X_WObject
# Frames
self.gripper_model = self.plant.GetModelInstanceByName("gripper")
self.world_frame = self.plant.world_frame()
self.gripper_frame = self.plant.GetFrameByName("body", self.gripper_model)
def __init__(self, time_step,
object_file_paths=None,
object_base_link_names=None,
X_WObject_list=(X_WObject_default,)):
self.time_step = time_step
# Finalize manipulation station by adding manipuland.
self.station = ManipulationStation(self.time_step)
self.station.AddCupboard()
self.plant = self.station.get_mutable_multibody_plant()
self.objects = []
if object_file_paths is not None:
for i, file_path in enumerate(object_file_paths):
self.objects.append(AddModelFromSdfFile(
file_name=file_path,
model_name="object_{}".format(i),
plant=self.plant,
scene_graph=self.station.get_mutable_scene_graph()
))
self.station.Finalize()
# Object base link names for reference
self.object_base_link_names = object_base_link_names
# Initial pose of the object
self.X_WObject_list = X_WObject_list
def __init__(
self,
time_step,
object_file_path=None,
object_base_link_name=None,
X_WObject=X_WObject_default,
):
self.object_base_link_name = object_base_link_name
self.time_step = time_step
# Finalize manipulation station by adding manipuland.
self.station = ManipulationStation(self.time_step)
self.station.SetupDefaultStation()
self.plant = self.station.get_mutable_multibody_plant()
if object_file_path is not None:
self.object = AddModelFromSdfFile(
file_name=object_file_path,
model_name="object",
plant=self.station.get_mutable_multibody_plant(),
scene_graph=self.station.get_mutable_scene_graph())
self.station.Finalize()
self.simulator = None
self.plan_runner = None
# Initial pose of the object
self.X_WObject = X_WObject
def __init__(self, q_initial, p_goal,
object_file_paths=None,
object_base_link_names=None,
X_WObject_list=None):
"""
:param q_initial: initial config of the robot
:param p_goal: goal point in World space
:param object_file_paths: a list of objects in the world, each a sdf file path
:param object_base_link_names: a list of object base link names
:param X_WObject_list: a list of initial pose of the objects
"""
self.q_initial=q_initial
self.p_goal=p_goal
# Set up station
self.station = ManipulationStation(collision_model=IiwaCollisionModel.kBoxCollision)
self.station.AddCupboard()
self.plant = self.station.get_mutable_multibody_plant()
# Add objects into world
self.objects = []
if object_file_paths is not None:
for i, file_path in enumerate(object_file_paths):
self.objects.append(AddModelFromSdfFile(
file_name=file_path,
model_name="object_{}".format(i),
plant=self.plant,
scene_graph=self.station.get_mutable_scene_graph()
))
self.station.Finalize()
# Object base link names for reference
self.object_base_link_names = object_base_link_names
# Initial pose of the object
self.X_WObject_list = X_WObject_list
# Set up util module
self.util = Util(self.station)
# Set up configuration space
joint_ranges = self.util.get_joint_ranges()
self.cspace = ConfigurationSpace(joint_ranges, self.util)
# Set initial pose of the objects
if self.object_base_link_names is not None:
for link_name, object, X_WObject in zip(self.object_base_link_names,
self.objects,
self.X_WObject_list):
self.util.set_object_pose(link_name, object, X_WObject)
# Set robot initial pose
self.util.set_iiwa_position(self.q_initial)
self.util.set_wsg_position(0.1)
# book keeping
self.rewires = 0
self.edge_evaluation = 0
def build_station_simulation(builder):
station = builder.AddSystem(ManipulationStation())
station.AddCupboard()
object_file_path = "drake/examples/manipulation_station/models/061_foam_brick.sdf"
brick = AddModelFromSdfFile(FindResourceOrThrow(object_file_path),
station.get_mutable_multibody_plant(),
station.get_mutable_scene_graph())
station.Finalize()
return station, brick
def __init__(self,
time_step,
object_file_path,
object_base_link_name,
is_hardware=False):
self.object_base_link_name = object_base_link_name
self.time_step = time_step
self.is_hardware = is_hardware
# Finalize manipulation station by adding manipuland.
self.station = ManipulationStation(self.time_step)
self.station.AddCupboard()
self.plant = self.station.get_mutable_multibody_plant()
self.object = AddModelFromSdfFile(
file_name=object_file_path,
model_name="object",
plant=self.station.get_mutable_multibody_plant(),
scene_graph=self.station.get_mutable_scene_graph())
self.station.Finalize()
def test_manipulation_station_add_iiwa_and_wsg_explicitly(self):
station = ManipulationStation()
parser = Parser(station.get_mutable_multibody_plant(),
station.get_mutable_scene_graph())
plant = station.get_mutable_multibody_plant()
# Add models for iiwa and wsg
iiwa_model_file = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/iiwa7/"
"iiwa7_no_collision.sdf")
iiwa = parser.AddModelFromFile(iiwa_model_file, "iiwa")
X_WI = RigidTransform.Identity()
plant.WeldFrames(plant.world_frame(),
plant.GetFrameByName("iiwa_link_0", iiwa),
X_WI.GetAsIsometry3())
wsg_model_file = FindResourceOrThrow(
"drake/manipulation/models/wsg_50_description/sdf/"
"schunk_wsg_50.sdf")
wsg = parser.AddModelFromFile(wsg_model_file, "gripper")
X_7G = RigidTransform.Identity()
plant.WeldFrames(
plant.GetFrameByName("iiwa_link_7", iiwa),
plant.GetFrameByName("body", wsg),
X_7G.GetAsIsometry3())
# Register models for the controller.
station.RegisterIiwaControllerModel(
iiwa_model_file, iiwa, plant.world_frame(),
plant.GetFrameByName("iiwa_link_0", iiwa), X_WI)
station.RegisterWsgControllerModel(
wsg_model_file, wsg,
plant.GetFrameByName("iiwa_link_7", iiwa),
plant.GetFrameByName("body", wsg), X_7G)
# Finalize
station.Finalize()
# This WSG gripper model has 2 independent dof, and the IIWA model
# has 7.
self.assertEqual(plant.num_positions(), 9)
self.assertEqual(plant.num_velocities(), 9)
def get_basic_manip_station():
"""Used for IK"""
builder = DiagramBuilder()
station = builder.AddSystem(ManipulationStation())
station.SetupClutterClearingStation()
station.Finalize()
diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant = station.get_multibody_plant()
return plant, context
def setup_manipulation_station(T_world_objectInitial, zmq_url, T_world_targetBin, manipuland="foam", include_bin=True, include_hoop=False):
builder = DiagramBuilder()
station = builder.AddSystem(ManipulationStation(time_step=1e-3))
station.SetupClutterClearingStation()
if manipuland is "foam":
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/061_foam_brick.sdf",
T_world_objectInitial)
elif manipuland is "ball":
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/sphere.sdf",
T_world_objectInitial)
elif manipuland is "bball":
station.AddManipulandFromFile(
"drake/../../../../../../manipulation/sdfs/bball.sdf", # this is some path hackery
T_world_objectInitial)
elif manipuland is "rod":
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/rod.sdf",
T_world_objectInitial)
station_plant = station.get_multibody_plant()
parser = Parser(station_plant)
if include_bin:
parser.AddModelFromFile("extra_bin.sdf")
station_plant.WeldFrames(station_plant.world_frame(), station_plant.GetFrameByName("extra_bin_base"), T_world_targetBin)
if include_hoop:
parser.AddModelFromFile("sdfs/hoop.sdf")
station_plant.WeldFrames(station_plant.world_frame(), station_plant.GetFrameByName("base_link_hoop"), T_world_targetBin)
station.Finalize()
frames_to_draw = {"gripper": {"body"}}
meshcat = None
if zmq_url is not None:
meshcat = ConnectMeshcatVisualizer(builder,
station.get_scene_graph(),
output_port=station.GetOutputPort("pose_bundle"),
delete_prefix_on_load=False,
frames_to_draw=frames_to_draw,
zmq_url=zmq_url)
diagram = builder.Build()
plant = station.get_multibody_plant()
context = plant.CreateDefaultContext()
gripper = plant.GetBodyByName("body")
initial_pose = plant.EvalBodyPoseInWorld(context, gripper)
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(0.01)
return initial_pose, meshcat
def load_station(time_step=0.0):
# https://github.com/RobotLocomotion/drake/blob/master/bindings/pydrake/examples/manipulation_station_py.cc
object_file_path = FindResourceOrThrow(
"drake/external/models_robotlocomotion/ycb_objects/061_foam_brick.sdf")
#object_file_path = FOAM_BRICK_PATH
station = ManipulationStation(time_step)
station.AddCupboard()
mbp = station.get_mutable_multibody_plant()
scene_graph = station.get_mutable_scene_graph()
object = AddModelFromSdfFile(
file_name=object_file_path,
model_name="object",
plant=mbp,
scene_graph=scene_graph)
station.Finalize()
robot = mbp.GetModelInstanceByName('iiwa')
gripper = mbp.GetModelInstanceByName('gripper')
initial_conf = [0, 0.6 - np.pi / 6, 0, -1.75, 0, 1.0, 0]
#initial_conf[1] += np.pi / 6
initial_positions = dict(zip(get_movable_joints(mbp, robot), initial_conf))
initial_poses = {
object: create_transform(translation=[.6, 0, 0]),
}
task = Task(mbp, scene_graph, robot, gripper, movable=[object], surfaces=[],
initial_positions=initial_positions, initial_poses=initial_poses,
goal_on=[])
return mbp, scene_graph, task
def PlotEeOrientationError(iiwa_position_measured_log, Q_WL7_ref, t_plan):
""" Plots the absolute value of rotation angle between frame L7 and its reference.
Q_WL7_ref is a quaternion of frame L7's reference orientation relative to world frame.
t_plan is the starting time of every plan. They are plotted as vertical dashed black lines.
"""
station = ManipulationStation()
station.SetupManipulationClassStation()
station.Finalize()
plant_iiwa = station.get_controller_plant()
tree_iiwa = plant_iiwa.tree()
context_iiwa = plant_iiwa.CreateDefaultContext()
l7_frame = plant_iiwa.GetFrameByName('iiwa_link_7')
t_sample = iiwa_position_measured_log.sample_times()
n = len(t_sample)
angle_error_abs = np.zeros(n - 1)
for i in range(1, n):
q_iiwa = iiwa_position_measured_log.data()[:, i]
x_iiwa_mutable = \
tree_iiwa.GetMutablePositionsAndVelocities(context_iiwa)
x_iiwa_mutable[:7] = q_iiwa
X_WL7 = tree_iiwa.CalcRelativeTransform(
context_iiwa, frame_A=plant_iiwa.world_frame(), frame_B=l7_frame)
Q_L7L7ref = X_WL7.quaternion().inverse().multiply(Q_WL7_ref)
angle_error_abs[i - 1] = np.arccos(Q_L7L7ref.w()) * 2
fig = plt.figure(dpi=150)
ax = fig.add_subplot(111)
ax.axhline(0, linestyle='--', color='r')
for t in t_plan:
ax.axvline(t, linestyle='--', color='k')
ax.plot(t_sample[1:], angle_error_abs / np.pi * 180)
ax.set_xlabel("t(s)")
ax.set_ylabel("abs angle error, degrees")
plt.tight_layout()
plt.show()
def test_manipulation_station_add_iiwa_and_wsg_explicitly(self):
station = ManipulationStation()
parser = Parser(station.get_mutable_multibody_plant(),
station.get_mutable_scene_graph())
plant = station.get_mutable_multibody_plant()
# Add models for iiwa and wsg
iiwa_model_file = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/iiwa7/"
"iiwa7_no_collision.sdf")
iiwa = parser.AddModelFromFile(iiwa_model_file, "iiwa")
X_WI = RigidTransform.Identity()
plant.WeldFrames(plant.world_frame(),
plant.GetFrameByName("iiwa_link_0", iiwa),
X_WI)
wsg_model_file = FindResourceOrThrow(
"drake/manipulation/models/wsg_50_description/sdf/"
"schunk_wsg_50.sdf")
wsg = parser.AddModelFromFile(wsg_model_file, "gripper")
X_7G = RigidTransform.Identity()
plant.WeldFrames(
plant.GetFrameByName("iiwa_link_7", iiwa),
plant.GetFrameByName("body", wsg),
X_7G)
# Register models for the controller.
station.RegisterIiwaControllerModel(
iiwa_model_file, iiwa, plant.world_frame(),
plant.GetFrameByName("iiwa_link_0", iiwa), X_WI)
station.RegisterWsgControllerModel(
wsg_model_file, wsg,
plant.GetFrameByName("iiwa_link_7", iiwa),
plant.GetFrameByName("body", wsg), X_7G)
# Finalize
station.Finalize()
self.assertEqual(station.num_iiwa_joints(), 7)
# This WSG gripper model has 2 independent dof, and the IIWA model
# has 7.
self.assertEqual(plant.num_positions(), 9)
self.assertEqual(plant.num_velocities(), 9)
def __init__(self, is_visualizing=False):
self.station = ManipulationStation()
self.station.SetupManipulationClassStation(
IiwaCollisionModel.kBoxCollision)
self.station.Finalize()
self.plant = self.station.get_mutable_multibody_plant()
self.scene_graph = self.station.get_mutable_scene_graph()
self.is_visualizing = is_visualizing
# scene graph query output port.
self.query_output_port = self.scene_graph.GetOutputPort("query")
builder = DiagramBuilder()
builder.AddSystem(self.station)
# meshcat visualizer
if is_visualizing:
self.viz = MeshcatVisualizer(self.scene_graph,
zmq_url="tcp://127.0.0.1:6000")
# connect meshcat to manipulation station
builder.AddSystem(self.viz)
builder.Connect(self.station.GetOutputPort("pose_bundle"),
self.viz.GetInputPort("lcm_visualization"))
self.diagram = builder.Build()
# contexts
self.context_diagram = self.diagram.CreateDefaultContext()
self.context_station = self.diagram.GetSubsystemContext(
self.station, self.context_diagram)
self.context_scene_graph = self.station.GetSubsystemContext(
self.scene_graph, self.context_station)
self.context_plant = self.station.GetMutableSubsystemContext(
self.plant, self.context_station)
if is_visualizing:
self.viz.load()
self.context_meshcat = self.diagram.GetSubsystemContext(
self.viz, self.context_diagram)
def __init__(self,
time_step,
object_file_path,
object_base_link_name,
is_hardware=False):
self.object_base_link_name = object_base_link_name
self.time_step = time_step
self.is_hardware = is_hardware
# Finalize manipulation station by adding manipuland.
self.station = ManipulationStation(self.time_step)
self.station.AddCupboard()
self.plant = self.station.get_mutable_multibody_plant()
self.object = AddModelFromSdfFile(
file_name=object_file_path,
model_name="object",
plant=self.station.get_mutable_multibody_plant(),
scene_graph=self.station.get_mutable_scene_graph())
self.station.Finalize()
# Initial pose of the object
X_WObject = Isometry3.Identity()
X_WObject.set_translation([.6, 0, 0])
self.X_WObject = X_WObject
def test_rgbd_sensor_registration_deprecated(self):
X_PC = RigidTransform(p=[1, 2, 3])
station = ManipulationStation(time_step=0.001)
station.SetupManipulationClassStation()
plant = station.get_multibody_plant()
with catch_drake_warnings(expected_count=2):
properties = DepthCameraProperties(10, 10, np.pi / 4, "renderer",
0.01, 5.0)
station.RegisterRgbdSensor("deprecated", plant.world_frame(), X_PC,
properties)
station.Finalize()
camera_poses = station.GetStaticCameraPosesInWorld()
# The three default cameras plus the one just added.
self.assertEqual(len(camera_poses), 4)
self.assertTrue("deprecated" in camera_poses)
def test_manipulation_station(self):
# Just check the spelling.
station = ManipulationStation(time_step=0.001)
station.Finalize()
station.get_mutable_multibody_plant()
station.get_mutable_scene_graph()
station.get_controller_plant()
# Check the setters/getters.
context = station.CreateDefaultContext()
q = np.linspace(0.04, 0.6, num=7)
v = np.linspace(-2.3, 0.5, num=7)
station.SetIiwaPosition(q, context)
np.testing.assert_array_equal(q, station.GetIiwaPosition(context))
station.SetIiwaVelocity(v, context)
np.testing.assert_array_equal(v, station.GetIiwaVelocity(context))
def run_manipulation_example(args):
builder = DiagramBuilder()
station = builder.AddSystem(ManipulationStation(time_step=0.005))
station.SetupManipulationClassStation()
station.Finalize()
plant = station.get_multibody_plant()
scene_graph = station.get_scene_graph()
pose_bundle_output_port = station.GetOutputPort("pose_bundle")
# Side-on view of the station.
T_VW = np.array([[1., 0., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]])
visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(
scene_graph, T_VW=T_VW, xlim=[-0.5, 1.0], ylim=[-1.2, 1.2],
draw_period=0.1))
builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0))
if args.playback:
visualizer.start_recording()
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
# Fix the control inputs to zero.
station_context = diagram.GetMutableSubsystemContext(
station, simulator.get_mutable_context())
station.GetInputPort("iiwa_position").FixValue(
station_context, station.GetIiwaPosition(station_context))
station.GetInputPort("iiwa_feedforward_torque").FixValue(
station_context, np.zeros(7))
station.GetInputPort("wsg_position").FixValue(
station_context, np.zeros(1))
station.GetInputPort("wsg_force_limit").FixValue(
station_context, [40.0])
simulator.AdvanceTo(args.duration)
if args.playback:
visualizer.stop_recording()
ani = visualizer.get_recording_as_animation()
# Playback the recording and save the output.
ani.save("{}/manip_playback.mp4".format(temp_directory()), fps=30)
def test_rgbd_sensor_registration(self):
X_PC = RigidTransform(p=[1, 2, 3])
station = ManipulationStation(time_step=0.001)
station.SetupManipulationClassStation()
plant = station.get_multibody_plant()
color_camera = ColorRenderCamera(
RenderCameraCore("renderer", CameraInfo(10, 10, np.pi / 4),
ClippingRange(0.1, 10.0), RigidTransform()),
False)
depth_camera = DepthRenderCamera(color_camera.core(),
DepthRange(0.1, 9.5))
station.RegisterRgbdSensor("single_sensor", plant.world_frame(), X_PC,
depth_camera)
station.RegisterRgbdSensor("dual_sensor", plant.world_frame(), X_PC,
color_camera, depth_camera)
station.Finalize()
camera_poses = station.GetStaticCameraPosesInWorld()
# The three default cameras plus the two just added.
self.assertEqual(len(camera_poses), 5)
self.assertTrue("single_sensor" in camera_poses)
self.assertTrue("dual_sensor" in camera_poses)
def gripper_forward_kinematics_example():
builder = DiagramBuilder()
station = builder.AddSystem(ManipulationStation())
station.SetupClutterClearingStation()
station.Finalize()
frames_to_draw = {
"iiwa": {
"iiwa_link_1", "iiwa_link_2", "iiwa_link_3", "iiwa_link_4",
"iiwa_link_5", "iiwa_link_6", "iiwa_link_7"
},
"gripper": {"body"}
}
meshcat = ConnectMeshcatVisualizer(
builder,
station.get_scene_graph(),
output_port=station.GetOutputPort("pose_bundle"),
frames_to_draw=frames_to_draw,
axis_length=0.3,
axis_radius=0.01)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
# xyz = Text(value="", description="gripper position (m): ", layout=Layout(width='500px'), style={'description_width':'initial'})
# rpy = Text(value="", description="gripper roll-pitch-yaw (rad): ", layout=Layout(width='500px'), style={'description_width':'initial'})
plant = station.get_multibody_plant()
gripper = plant.GetBodyByName("body")
def pose_callback(context):
pose = plant.EvalBodyPoseInWorld(context, gripper) # Important
# xyz.value = np.array2string(pose.translation(), formatter={'float': lambda x: "{:3.2f}".format(x)})
# rpy.value = np.array2string(RollPitchYaw(pose.rotation()).vector(), formatter={'float': lambda x: "{:3.2f}".format(x)})
meshcat.load()
MakeJointSlidersThatPublishOnCallback(station.get_multibody_plant(),
meshcat,
context,
my_callback=pose_callback)
def runManipulationExample(args):
builder = DiagramBuilder()
station = builder.AddSystem(ManipulationStation(time_step=0.005))
station.SetupDefaultStation()
station.Finalize()
plant = station.get_multibody_plant()
scene_graph = station.get_scene_graph()
pose_bundle_output_port = station.GetOutputPort("pose_bundle")
# Side-on view of the station.
Tview = np.array([[1., 0., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]],
dtype=np.float64)
visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(
scene_graph, Tview=Tview, xlim=[-0.5, 1.0], ylim=[-1.2, 1.2],
draw_period=0.1))
builder.Connect(pose_bundle_output_port,
visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
# Fix the control inputs to zero.
station_context = diagram.GetMutableSubsystemContext(
station, simulator.get_mutable_context())
station.GetInputPort("iiwa_position").FixValue(
station_context, station.GetIiwaPosition(station_context))
station.GetInputPort("iiwa_feedforward_torque").FixValue(
station_context, np.zeros(7))
station.GetInputPort("wsg_position").FixValue(
station_context, np.zeros(1))
station.GetInputPort("wsg_force_limit").FixValue(
station_context, [40.0])
simulator.StepTo(args.duration)
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 = ConnectMeshcatVisualizer(
builder,
station.get_scene_graph(),
output_port=station.GetOutputPort("pose_bundle"))
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.GetMyContextFromRoot(simulator.get_mutable_context(
)).get_mutable_continuous_state_vector().SetFromVector(
station.GetIiwaPosition(station_context))
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(0.01)
return simulator
def test_decomposition_controller_like_workflow(self):
"""Tests subgraphs (post-finalize) for decomposition, with a
scene-graph. Also shows a workflow of replacing joints / welding
joints."""
builder = DiagramBuilder()
# N.B. I (Eric) am using ManipulationStation because it's currently
# the simplest way to get a complex scene in pydrake.
station = ManipulationStation(time_step=0.001)
station.SetupManipulationClassStation()
station.Finalize()
builder.AddSystem(station)
plant = station.get_multibody_plant()
scene_graph = station.get_scene_graph()
iiwa = plant.GetModelInstanceByName("iiwa")
wsg = plant.GetModelInstanceByName("gripper")
if VISUALIZE:
print("test_decomposition_controller_like_workflow")
DrakeVisualizer.AddToBuilder(builder,
station.GetOutputPort("query_object"))
diagram = builder.Build()
# Set the context with which things should be computed.
d_context = diagram.CreateDefaultContext()
context = plant.GetMyContextFromRoot(d_context)
q_iiwa = [0.3, 0.7, 0.3, 0.6, 0.5, 0.6, 0.7]
ndof = 7
q_wsg = [-0.03, 0.03]
plant.SetPositions(context, iiwa, q_iiwa)
plant.SetPositions(context, wsg, q_wsg)
# Add copy of only the IIWA to a control diagram.
control_builder = DiagramBuilder()
control_plant = control_builder.AddSystem(MultibodyPlant(time_step=0))
# N.B. This has the same scene, but with all joints outside of the
# IIWA frozen
# TODO(eric.cousineau): Re-investigate weird "build_with_no_control"
# behavior (with scene graph) with default conditions and time_step=0
# - see Anzu commit 2cf08cfc3.
to_control = mut.add_plant_with_articulated_subset_to(
plant_src=plant,
scene_graph_src=scene_graph,
articulated_models_src=[iiwa],
context_src=context,
plant_dest=control_plant)
self.assertIsInstance(to_control, mut.MultibodyPlantElementsMap)
control_iiwa = to_control.model_instances[iiwa]
control_plant.Finalize()
self.assertEqual(control_plant.num_positions(),
plant.num_positions(iiwa))
kp = np.array([2000., 1500, 1500, 1500, 1500, 500, 500])
kd = np.sqrt(2 * kp)
ki = np.zeros(7)
controller = control_builder.AddSystem(
InverseDynamicsController(robot=control_plant,
kp=kp,
ki=ki,
kd=kd,
has_reference_acceleration=False))
# N.B. Rather than use model instances for direct correspence, we could
# use the mappings themselves within a custom system.
control_builder.Connect(
control_plant.get_state_output_port(control_iiwa),
controller.get_input_port_estimated_state())
control_builder.Connect(
controller.get_output_port_control(),
control_plant.get_actuation_input_port(control_iiwa))
# Control to having the elbow slightly bent.
q_iiwa_final = np.zeros(7)
q_iiwa_final[3] = -np.pi / 2
t_start = 0.
t_end = 1.
nx = 2 * ndof
def q_desired_interpolator(t: ContextTimeArg) -> VectorArg(nx):
s = (t - t_start) / (t_end - t_start)
ds = 1 / (t_end - t_start)
q = q_iiwa + s * (q_iiwa_final - q_iiwa)
v = ds * (q_iiwa_final - q_iiwa)
x = np.hstack((q, v))
return x
interpolator = control_builder.AddSystem(
FunctionSystem(q_desired_interpolator))
control_builder.Connect(interpolator.get_output_port(0),
controller.get_input_port_desired_state())
control_diagram = control_builder.Build()
control_d_context = control_diagram.CreateDefaultContext()
control_context = control_plant.GetMyContextFromRoot(control_d_context)
to_control.copy_state(context, control_context)
util.compare_frame_poses(plant, context, control_plant,
control_context, "iiwa_link_0", "iiwa_link_7")
util.compare_frame_poses(plant, context, control_plant,
control_context, "body", "left_finger")
from_control = to_control.inverse()
def viz_monitor(control_d_context):
# Simulate control, visualizing in original diagram.
assert (control_context is
control_plant.GetMyContextFromRoot(control_d_context))
from_control.copy_state(control_context, context)
d_context.SetTime(control_d_context.get_time())
diagram.Publish(d_context)
return EventStatus.DidNothing()
simulator = Simulator(control_diagram, control_d_context)
simulator.Initialize()
if VISUALIZE:
simulator.set_monitor(viz_monitor)
simulator.set_target_realtime_rate(1.)
simulator.AdvanceTo(t_end)
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--target_realtime_rate", type=float, default=1.0,
help="Desired rate relative to real time. See documentation for "
"Simulator::set_target_realtime_rate() for details.")
parser.add_argument(
"--duration", type=float, default=np.inf,
help="Desired duration of the simulation in seconds.")
parser.add_argument(
"--hardware", action='store_true',
help="Use the ManipulationStationHardwareInterface instead of an "
"in-process simulation.")
parser.add_argument(
"--test", action='store_true',
help="Disable opening the gui window for testing.")
parser.add_argument(
"--filter_time_const", type=float, default=0.1,
help="Time constant for the first order low pass filter applied to"
"the teleop commands")
parser.add_argument(
"--velocity_limit_factor", type=float, default=1.0,
help="This value, typically between 0 and 1, further limits the "
"iiwa14 joint velocities. It multiplies each of the seven "
"pre-defined joint velocity limits. "
"Note: The pre-defined velocity limits are specified by "
"iiwa14_velocity_limits, found in this python file.")
parser.add_argument(
'--setup', type=str, default='manipulation_class',
help="The manipulation station setup to simulate. ",
choices=['manipulation_class', 'clutter_clearing', 'planar'])
parser.add_argument(
'--schunk_collision_model', type=str, default='box',
help="The Schunk collision model to use for simulation. ",
choices=['box', 'box_plus_fingertip_spheres'])
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
builder = DiagramBuilder()
if args.hardware:
station = builder.AddSystem(ManipulationStationHardwareInterface())
station.Connect(wait_for_cameras=False)
else:
station = builder.AddSystem(ManipulationStation())
if args.schunk_collision_model == "box":
schunk_model = SchunkCollisionModel.kBox
elif args.schunk_collision_model == "box_plus_fingertip_spheres":
schunk_model = SchunkCollisionModel.kBoxPlusFingertipSpheres
# Initializes the chosen station type.
if args.setup == 'manipulation_class':
station.SetupManipulationClassStation(
schunk_model=schunk_model)
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/"
+ "061_foam_brick.sdf",
RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))
elif args.setup == 'clutter_clearing':
station.SetupClutterClearingStation(
schunk_model=schunk_model)
ycb_objects = CreateClutterClearingYcbObjectList()
for model_file, X_WObject in ycb_objects:
station.AddManipulandFromFile(model_file, X_WObject)
elif args.setup == 'planar':
station.SetupPlanarIiwaStation(
schunk_model=schunk_model)
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/"
+ "061_foam_brick.sdf",
RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))
station.Finalize()
# If using meshcat, don't render the cameras, since RgbdCamera
# rendering only works with drake-visualizer. Without this check,
# running this code in a docker container produces libGL errors.
if args.meshcat:
meshcat = ConnectMeshcatVisualizer(
builder, output_port=station.GetOutputPort("geometry_query"),
zmq_url=args.meshcat, open_browser=args.open_browser)
if args.setup == 'planar':
meshcat.set_planar_viewpoint()
elif args.setup == 'planar':
pyplot_visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(
station.get_scene_graph()))
builder.Connect(station.GetOutputPort("pose_bundle"),
pyplot_visualizer.get_input_port(0))
else:
DrakeVisualizer.AddToBuilder(builder,
station.GetOutputPort("query_object"))
image_to_lcm_image_array = builder.AddSystem(
ImageToLcmImageArrayT())
image_to_lcm_image_array.set_name("converter")
for name in station.get_camera_names():
cam_port = (
image_to_lcm_image_array
.DeclareImageInputPort[PixelType.kRgba8U](
"camera_" + name))
builder.Connect(
station.GetOutputPort("camera_" + name + "_rgb_image"),
cam_port)
image_array_lcm_publisher = builder.AddSystem(
LcmPublisherSystem.Make(
channel="DRAKE_RGBD_CAMERA_IMAGES",
lcm_type=image_array_t,
lcm=None,
publish_period=0.1,
use_cpp_serializer=True))
image_array_lcm_publisher.set_name("rgbd_publisher")
builder.Connect(
image_to_lcm_image_array.image_array_t_msg_output_port(),
image_array_lcm_publisher.get_input_port(0))
robot = station.get_controller_plant()
params = DifferentialInverseKinematicsParameters(robot.num_positions(),
robot.num_velocities())
time_step = 0.005
params.set_timestep(time_step)
# True velocity limits for the IIWA14 (in rad, rounded down to the first
# decimal)
iiwa14_velocity_limits = np.array([1.4, 1.4, 1.7, 1.3, 2.2, 2.3, 2.3])
if args.setup == 'planar':
# Extract the 3 joints that are not welded in the planar version.
iiwa14_velocity_limits = iiwa14_velocity_limits[1:6:2]
# The below constant is in body frame.
params.set_end_effector_velocity_gain([1, 0, 0, 0, 1, 1])
# Stay within a small fraction of those limits for this teleop demo.
factor = args.velocity_limit_factor
params.set_joint_velocity_limits((-factor*iiwa14_velocity_limits,
factor*iiwa14_velocity_limits))
differential_ik = builder.AddSystem(DifferentialIK(
robot, robot.GetFrameByName("iiwa_link_7"), params, time_step))
builder.Connect(differential_ik.GetOutputPort("joint_position_desired"),
station.GetInputPort("iiwa_position"))
teleop = builder.AddSystem(EndEffectorTeleop(args.setup == 'planar'))
if args.test:
teleop.window.withdraw() # Don't display the window when testing.
filter = builder.AddSystem(
FirstOrderLowPassFilter(time_constant=args.filter_time_const, size=6))
builder.Connect(teleop.get_output_port(0), filter.get_input_port(0))
builder.Connect(filter.get_output_port(0),
differential_ik.GetInputPort("rpy_xyz_desired"))
wsg_buttons = builder.AddSystem(SchunkWsgButtons(teleop.window))
builder.Connect(wsg_buttons.GetOutputPort("position"),
station.GetInputPort("wsg_position"))
builder.Connect(wsg_buttons.GetOutputPort("force_limit"),
station.GetInputPort("wsg_force_limit"))
# When in regression test mode, log our joint velocities to later check
# that they were sufficiently quiet.
num_iiwa_joints = station.num_iiwa_joints()
if args.test:
iiwa_velocities = builder.AddSystem(SignalLogger(num_iiwa_joints))
builder.Connect(station.GetOutputPort("iiwa_velocity_estimated"),
iiwa_velocities.get_input_port(0))
else:
iiwa_velocities = None
diagram = builder.Build()
simulator = Simulator(diagram)
# This is important to avoid duplicate publishes to the hardware interface:
simulator.set_publish_every_time_step(False)
station_context = diagram.GetMutableSubsystemContext(
station, simulator.get_mutable_context())
station.GetInputPort("iiwa_feedforward_torque").FixValue(
station_context, np.zeros(num_iiwa_joints))
# If the diagram is only the hardware interface, then we must advance it a
# little bit so that first LCM messages get processed. A simulated plant is
# already publishing correct positions even without advancing, and indeed
# we must not advance a simulated plant until the sliders and filters have
# been initialized to match the plant.
if args.hardware:
simulator.AdvanceTo(1e-6)
q0 = station.GetOutputPort("iiwa_position_measured").Eval(
station_context)
differential_ik.parameters.set_nominal_joint_position(q0)
teleop.SetPose(differential_ik.ForwardKinematics(q0))
filter.set_initial_output_value(
diagram.GetMutableSubsystemContext(
filter, simulator.get_mutable_context()),
teleop.get_output_port(0).Eval(diagram.GetMutableSubsystemContext(
teleop, simulator.get_mutable_context())))
differential_ik.SetPositions(diagram.GetMutableSubsystemContext(
differential_ik, simulator.get_mutable_context()), q0)
simulator.set_target_realtime_rate(args.target_realtime_rate)
simulator.AdvanceTo(args.duration)
# Ensure that our initialization logic was correct, by inspecting our
# logged joint velocities.
if args.test:
for time, qdot in zip(iiwa_velocities.sample_times(),
iiwa_velocities.data().transpose()):
# TODO(jwnimmer-tri) We should be able to do better than a 40
# rad/sec limit, but that's the best we can enforce for now.
if qdot.max() > 0.1:
print(f"ERROR: large qdot {qdot} at time {time}")
sys.exit(1)
def main():
goal_position = np.array([0.5, 0., 0.025])
blue_box_clean_position = [0.4, 0., 0.05]
red_box_clean_position = [0.6, 0., 0.05]
goal_delta = 0.05
parser = argparse.ArgumentParser(description=__doc__)
MeshcatVisualizer.add_argparse_argument(parser)
parser.add_argument('--use_meshcat',
action='store_true',
help="Must be set for meshcat to be used.")
parser.add_argument('--disable_planar_viz',
action='store_true',
help="Don't create a planar visualizer. Probably"
" breaks something that assumes the planar"
" vis exists.")
parser.add_argument('--teleop',
action='store_true',
help="Enable teleop, so *don't* use the state machine"
" and motion primitives.")
args = parser.parse_args()
builder = DiagramBuilder()
# Set up the ManipulationStation
station = builder.AddSystem(ManipulationStation(0.001))
mbp = station.get_multibody_plant()
station.SetupManipulationClassStation()
add_goal_region_visual_geometry(mbp, goal_position, goal_delta)
add_box_at_location(mbp,
name="blue_box",
color=[0.25, 0.25, 1., 1.],
pose=RigidTransform(p=[0.4, 0.0, 0.025]))
add_box_at_location(mbp,
name="red_box",
color=[1., 0.25, 0.25, 1.],
pose=RigidTransform(p=[0.6, 0.0, 0.025]))
station.Finalize()
iiwa_q0 = np.array([0.0, 0.6, 0.0, -1.75, 0., 1., np.pi / 2.])
# Attach a visualizer.
if args.use_meshcat:
meshcat = builder.AddSystem(
MeshcatVisualizer(station.get_scene_graph(), zmq_url=args.meshcat))
builder.Connect(station.GetOutputPort("pose_bundle"),
meshcat.get_input_port(0))
if not args.disable_planar_viz:
plt.gca().clear()
viz = builder.AddSystem(
PlanarSceneGraphVisualizer(station.get_scene_graph(),
xlim=[0.25, 0.8],
ylim=[-0.1, 0.5],
ax=plt.gca()))
builder.Connect(station.GetOutputPort("pose_bundle"),
viz.get_input_port(0))
plt.draw()
# Hook up DifferentialIK, since both control modes use it.
robot = station.get_controller_plant()
params = DifferentialInverseKinematicsParameters(robot.num_positions(),
robot.num_velocities())
time_step = 0.005
params.set_timestep(time_step)
# True velocity limits for the IIWA14 (in rad, rounded down to the first
# decimal)
iiwa14_velocity_limits = np.array([1.4, 1.4, 1.7, 1.3, 2.2, 2.3, 2.3])
# Stay within a small fraction of those limits for this teleop demo.
factor = 1.0
params.set_joint_velocity_limits(
(-factor * iiwa14_velocity_limits, factor * iiwa14_velocity_limits))
differential_ik = builder.AddSystem(
DifferentialIK(robot, robot.GetFrameByName("iiwa_link_7"), params,
time_step))
differential_ik.set_name("Differential IK")
differential_ik.parameters.set_nominal_joint_position(iiwa_q0)
builder.Connect(differential_ik.GetOutputPort("joint_position_desired"),
station.GetInputPort("iiwa_position"))
if not args.teleop:
symbol_list = [
SymbolL2Close("blue_box_in_goal", "blue_box", goal_position,
goal_delta),
SymbolL2Close("red_box_in_goal", "red_box", goal_position,
goal_delta),
SymbolRelativePositionL2("blue_box_on_red_box",
"blue_box",
"red_box",
l2_thresh=0.01,
offset=np.array([0., 0., 0.05])),
SymbolRelativePositionL2("red_box_on_blue_box",
"red_box",
"blue_box",
l2_thresh=0.01,
offset=np.array([0., 0., 0.05])),
]
primitive_list = [
MoveBoxPrimitive("put_blue_box_in_goal", mbp, "blue_box",
goal_position),
MoveBoxPrimitive("put_red_box_in_goal", mbp, "red_box",
goal_position),
MoveBoxPrimitive("put_blue_box_away", mbp, "blue_box",
blue_box_clean_position),
MoveBoxPrimitive("put_red_box_away", mbp, "red_box",
red_box_clean_position),
MoveBoxPrimitive("put_red_box_on_blue_box", mbp, "red_box",
np.array([0., 0., 0.05]), "blue_box"),
MoveBoxPrimitive("put_blue_box_on_red_box", mbp, "blue_box",
np.array([0., 0., 0.05]), "red_box"),
]
task_execution_system = builder.AddSystem(
TaskExectionSystem(
mbp,
symbol_list=symbol_list,
primitive_list=primitive_list,
dfa_json_file="specifications/red_and_blue_boxes_stacking.json"
))
builder.Connect(station.GetOutputPort("plant_continuous_state"),
task_execution_system.GetInputPort("mbp_state_vector"))
builder.Connect(task_execution_system.get_output_port(0),
differential_ik.GetInputPort("rpy_xyz_desired"))
builder.Connect(task_execution_system.get_output_port(1),
station.GetInputPort("wsg_position"))
#movebox = MoveBoxPrimitive("test_move_box", mbp, "red_box", goal_position)
#rpy_xyz_trajectory, gripper_traj = movebox.generate_rpyxyz_and_gripper_trajectory(mbp.CreateDefaultContext())
#rpy_xyz_trajectory_source = builder.AddSystem(TrajectorySource(rpy_xyz_trajectory))
#builder.Connect(rpy_xyz_trajectory_source.get_output_port(0),
# differential_ik.GetInputPort("rpy_xyz_desired"))
#wsg_position_source = builder.AddSystem(TrajectorySource(gripper_traj))
#builder.Connect(wsg_position_source.get_output_port(0),
# station.GetInputPort("wsg_position"))
# Target zero feedforward residual torque at all times.
fft = builder.AddSystem(ConstantVectorSource(np.zeros(7)))
builder.Connect(fft.get_output_port(0),
station.GetInputPort("iiwa_feedforward_torque"))
input_force_fix = builder.AddSystem(ConstantVectorSource([40.0]))
builder.Connect(input_force_fix.get_output_port(0),
station.GetInputPort("wsg_force_limit"))
end_time = 10000
else: # Set up teleoperation.
# Hook up a pygame-based keyboard+mouse interface for
# teleoperation, and low pass its output to drive the EE target
# for the differential IK.
print_instructions()
teleop = builder.AddSystem(MouseKeyboardTeleop(grab_focus=True))
filter_ = builder.AddSystem(
FirstOrderLowPassFilter(time_constant=0.005, size=6))
builder.Connect(teleop.get_output_port(0), filter_.get_input_port(0))
builder.Connect(filter_.get_output_port(0),
differential_ik.GetInputPort("rpy_xyz_desired"))
builder.Connect(teleop.GetOutputPort("position"),
station.GetInputPort("wsg_position"))
builder.Connect(teleop.GetOutputPort("force_limit"),
station.GetInputPort("wsg_force_limit"))
# Target zero feedforward residual torque at all times.
fft = builder.AddSystem(ConstantVectorSource(np.zeros(7)))
builder.Connect(fft.get_output_port(0),
station.GetInputPort("iiwa_feedforward_torque"))
# Simulate functionally forever.
end_time = 10000
# Create symbol log
#symbol_log = SymbolFromTransformLog(
# [SymbolL2Close('at_goal', 'red_box', goal_position, .025),
# SymbolL2Close('at_goal', 'blue_box', goal_position, .025)])
#
#symbol_logger_system = builder.AddSystem(
# SymbolLoggerSystem(
# station.get_multibody_plant(), symbol_logger=symbol_log))
#builder.Connect(
# station.GetOutputPort("plant_continuous_state"),
# symbol_logger_system.GetInputPort("mbp_state_vector"))
# Remaining input ports need to be tied up.
diagram = builder.Build()
g = pydot.graph_from_dot_data(diagram.GetGraphvizString())[0]
g.write_png("system_diagram.png")
diagram_context = diagram.CreateDefaultContext()
station_context = diagram.GetMutableSubsystemContext(
station, diagram_context)
station.SetIiwaPosition(station_context, iiwa_q0)
differential_ik.SetPositions(
diagram.GetMutableSubsystemContext(differential_ik, diagram_context),
iiwa_q0)
if args.teleop:
teleop.SetPose(differential_ik.ForwardKinematics(iiwa_q0))
filter_.set_initial_output_value(
diagram.GetMutableSubsystemContext(filter_, diagram_context),
teleop.get_output_port(0).Eval(
diagram.GetMutableSubsystemContext(teleop, diagram_context)))
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(end_time)
def ConnectPointsWithCubicPolynomial(x_start, x_end, duration):
t_knots = [0, duration / 2, duration]
n = len(x_start)
assert n == len(x_end)
x_knots = np.zeros((3, n))
x_knots[0] = x_start
x_knots[2] = x_end
x_knots[1] = (x_knots[0] + x_knots[2]) / 2
return PiecewisePolynomial.Cubic(t_knots, x_knots.T, np.zeros(n),
np.zeros(n))
'''
Create an instance of ManipulationStation for kinematic and dynamic calculations.
'''
station = ManipulationStation()
station.Finalize()
'''
Ea, or End_Effector_world_aligned is a frame fixed w.r.t the gripper.
Ea has the same origin as the end effector's body frame, but
its axes are aligned with those of the world frame when the system
has zero state, i.e. the robot is upright with all joint angles
equal to zero.
This frame is defined so that it is convenient to define end effector orientation
relative to the world frame using RollPitchYaw.
'''
def GetEndEffectorWorldAlignedFrame():
X_EEa = Isometry3.Identity()
X_EEa.set_rotation(np.array([[
def test_manipulation_station(self):
# Just check the spelling.
station = ManipulationStation(time_step=0.001)
station.SetupManipulationClassStation()
station.SetWsgGains(0.1, 0.1)
station.SetIiwaPositionGains(np.ones(7))
station.SetIiwaVelocityGains(np.ones(7))
station.SetIiwaIntegralGains(np.ones(7))
station.Finalize()
station.get_multibody_plant()
station.get_mutable_multibody_plant()
station.get_scene_graph()
station.get_mutable_scene_graph()
station.get_controller_plant()
# Check the setters/getters.
context = station.CreateDefaultContext()
q = np.linspace(0.04, 0.6, num=7)
v = np.linspace(-2.3, 0.5, num=7)
station.SetIiwaPosition(context, q)
np.testing.assert_array_equal(q, station.GetIiwaPosition(context))
station.SetIiwaVelocity(context, v)
np.testing.assert_array_equal(v, station.GetIiwaVelocity(context))
q = 0.0423
v = 0.0851
station.SetWsgPosition(context, q)
self.assertEqual(q, station.GetWsgPosition(context))
station.SetWsgVelocity(context, v)
self.assertEqual(v, station.GetWsgVelocity(context))
station.GetStaticCameraPosesInWorld()["0"]
self.assertEqual(len(station.get_camera_names()), 3)
def test_manipulation_station(self):
# Just check the spelling.
station = ManipulationStation(time_step=0.001)
station.SetupDefaultStation()
station.SetWsgGains(0.1, 0.1)
station.SetIiwaPositionGains(np.ones(7))
station.SetIiwaVelocityGains(np.ones(7))
station.SetIiwaIntegralGains(np.ones(7))
station.Finalize()
station.get_multibody_plant()
station.get_mutable_multibody_plant()
station.get_scene_graph()
station.get_mutable_scene_graph()
station.get_controller_plant()
# Check the setters/getters.
context = station.CreateDefaultContext()
q = np.linspace(0.04, 0.6, num=7)
v = np.linspace(-2.3, 0.5, num=7)
station.SetIiwaPosition(context, q)
np.testing.assert_array_equal(q, station.GetIiwaPosition(context))
station.SetIiwaVelocity(context, v)
np.testing.assert_array_equal(v, station.GetIiwaVelocity(context))
q = 4.23
v = 8.51
station.SetWsgPosition(context, q)
self.assertEqual(q, station.GetWsgPosition(context))
station.SetWsgVelocity(context, v)
self.assertEqual(v, station.GetWsgVelocity(context))
station.GetStaticCameraPosesInWorld()["0"]
self.assertEqual(len(station.get_camera_names()), 3)
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--target_realtime_rate",
type=float,
default=1.0,
help="Desired rate relative to real time. See documentation for "
"Simulator::set_target_realtime_rate() for details.")
parser.add_argument("--duration",
type=float,
default=np.inf,
help="Desired duration of the simulation in seconds.")
parser.add_argument(
"--hardware",
action='store_true',
help="Use the ManipulationStationHardwareInterface instead of an "
"in-process simulation.")
parser.add_argument("--test",
action='store_true',
help="Disable opening the gui window for testing.")
parser.add_argument(
'--setup',
type=str,
default='manipulation_class',
help="The manipulation station setup to simulate. ",
choices=['manipulation_class', 'clutter_clearing', 'planar'])
parser.add_argument(
"-w",
"--open-window",
dest="browser_new",
action="store_const",
const=1,
default=None,
help="Open the MeshCat display in a new browser window.")
args = parser.parse_args()
builder = DiagramBuilder()
# NOTE: the meshcat instance is always created in order to create the
# teleop controls (joint sliders and open/close gripper button). When
# args.hardware is True, the meshcat server will *not* display robot
# geometry, but it will contain the joint sliders and open/close gripper
# button in the "Open Controls" tab in the top-right of the viewing server.
meshcat = Meshcat()
if args.hardware:
# TODO(russt): Replace this hard-coded camera serial number with a
# config file.
camera_ids = ["805212060544"]
station = builder.AddSystem(
ManipulationStationHardwareInterface(camera_ids))
station.Connect(wait_for_cameras=False)
else:
station = builder.AddSystem(ManipulationStation())
# Initializes the chosen station type.
if args.setup == 'manipulation_class':
station.SetupManipulationClassStation()
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/" +
"061_foam_brick.sdf",
RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))
elif args.setup == 'clutter_clearing':
station.SetupClutterClearingStation()
ycb_objects = CreateClutterClearingYcbObjectList()
for model_file, X_WObject in ycb_objects:
station.AddManipulandFromFile(model_file, X_WObject)
elif args.setup == 'planar':
station.SetupPlanarIiwaStation()
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/" +
"061_foam_brick.sdf",
RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))
station.Finalize()
geometry_query_port = station.GetOutputPort("geometry_query")
DrakeVisualizer.AddToBuilder(builder, geometry_query_port)
meshcat_visualizer = MeshcatVisualizerCpp.AddToBuilder(
builder=builder,
query_object_port=geometry_query_port,
meshcat=meshcat)
if args.setup == 'planar':
meshcat.Set2dRenderMode()
pyplot_visualizer = ConnectPlanarSceneGraphVisualizer(
builder, station.get_scene_graph(), geometry_query_port)
if args.browser_new is not None:
url = meshcat.web_url()
webbrowser.open(url=url, new=args.browser_new)
teleop = builder.AddSystem(
JointSliders(meshcat=meshcat, plant=station.get_controller_plant()))
num_iiwa_joints = station.num_iiwa_joints()
filter = builder.AddSystem(
FirstOrderLowPassFilter(time_constant=2.0, size=num_iiwa_joints))
builder.Connect(teleop.get_output_port(0), filter.get_input_port(0))
builder.Connect(filter.get_output_port(0),
station.GetInputPort("iiwa_position"))
wsg_buttons = builder.AddSystem(SchunkWsgButtons(meshcat=meshcat))
builder.Connect(wsg_buttons.GetOutputPort("position"),
station.GetInputPort("wsg_position"))
builder.Connect(wsg_buttons.GetOutputPort("force_limit"),
station.GetInputPort("wsg_force_limit"))
# When in regression test mode, log our joint velocities to later check
# that they were sufficiently quiet.
if args.test:
iiwa_velocities = builder.AddSystem(VectorLogSink(num_iiwa_joints))
builder.Connect(station.GetOutputPort("iiwa_velocity_estimated"),
iiwa_velocities.get_input_port(0))
else:
iiwa_velocities = None
diagram = builder.Build()
simulator = Simulator(diagram)
# This is important to avoid duplicate publishes to the hardware interface:
simulator.set_publish_every_time_step(False)
station_context = diagram.GetMutableSubsystemContext(
station, simulator.get_mutable_context())
station.GetInputPort("iiwa_feedforward_torque").FixValue(
station_context, np.zeros(num_iiwa_joints))
# If the diagram is only the hardware interface, then we must advance it a
# little bit so that first LCM messages get processed. A simulated plant is
# already publishing correct positions even without advancing, and indeed
# we must not advance a simulated plant until the sliders and filters have
# been initialized to match the plant.
if args.hardware:
simulator.AdvanceTo(1e-6)
# Eval the output port once to read the initial positions of the IIWA.
q0 = station.GetOutputPort("iiwa_position_measured").Eval(station_context)
teleop.SetPositions(q0)
filter.set_initial_output_value(
diagram.GetMutableSubsystemContext(filter,
simulator.get_mutable_context()),
q0)
simulator.set_target_realtime_rate(args.target_realtime_rate)
simulator.AdvanceTo(args.duration)
# Ensure that our initialization logic was correct, by inspecting our
# logged joint velocities.
if args.test:
iiwa_velocities_log = iiwa_velocities.FindLog(simulator.get_context())
for time, qdot in zip(iiwa_velocities_log.sample_times(),
iiwa_velocities_log.data().transpose()):
# TODO(jwnimmer-tri) We should be able to do better than a 40
# rad/sec limit, but that's the best we can enforce for now.
if qdot.max() > 0.1:
print(f"ERROR: large qdot {qdot} at time {time}")
sys.exit(1)
parser.add_argument("--test",
action='store_true',
help="Disable opening the gui window for testing.")
args = parser.parse_args()
builder = DiagramBuilder()
if args.hardware:
# TODO(russt): Replace this hard-coded camera serial number with a config
# file.
camera_ids = ["805212060544"]
station = builder.AddSystem(
ManipulationStationHardwareInterface(camera_ids))
station.Connect(wait_for_cameras=False)
else:
station = builder.AddSystem(ManipulationStation())
station.AddCupboard()
object = AddModelFromSdfFile(
FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf"),
"object", station.get_mutable_multibody_plant(),
station.get_mutable_scene_graph())
station.Finalize()
ConnectDrakeVisualizer(builder, station.get_scene_graph(),
station.GetOutputPort("pose_bundle"))
teleop = builder.AddSystem(
JointSliders(station.get_controller_plant(), length=800))
if args.test:
teleop.window.withdraw() # Don't display the window when testing.
def test_manipulation_station(self):
# Just check the spelling.
station = ManipulationStation(
time_step=0.001, collision_model=IiwaCollisionModel.kNoCollision)
station.Finalize()
station.get_multibody_plant()
station.get_mutable_multibody_plant()
station.get_scene_graph()
station.get_mutable_scene_graph()
station.get_controller_plant()
# Check the setters/getters.
context = station.CreateDefaultContext()
q = np.linspace(0.04, 0.6, num=7)
v = np.linspace(-2.3, 0.5, num=7)
station.SetIiwaPosition(q, context)
np.testing.assert_array_equal(q, station.GetIiwaPosition(context))
station.SetIiwaVelocity(v, context)
np.testing.assert_array_equal(v, station.GetIiwaVelocity(context))
q = 4.23
v = 8.51
station.SetWsgPosition(q, context)
self.assertEqual(q, station.GetWsgPosition(context))
station.SetWsgVelocity(v, context)
self.assertEqual(v, station.GetWsgVelocity(context))
station.get_camera_pose(0)
def test_clutter_clearing_setup(self):
station = ManipulationStation(time_step=0.001)
station.SetupClutterClearingStation()
num_station_bodies = (
station.get_multibody_plant().num_model_instances())
ycb_objects = CreateClutterClearingYcbObjectList()
for model_file, X_WObject in ycb_objects:
station.AddManipulandFromFile(model_file, X_WObject)
station.Finalize()
context = station.CreateDefaultContext()
q = np.linspace(0.04, 0.6, num=7)
v = np.linspace(-2.3, 0.5, num=7)
station.SetIiwaPosition(context, q)
np.testing.assert_array_equal(q, station.GetIiwaPosition(context))
station.SetIiwaVelocity(context, v)
np.testing.assert_array_equal(v, station.GetIiwaVelocity(context))
q = 0.0423
v = 0.0851
station.SetWsgPosition(context, q)
self.assertEqual(q, station.GetWsgPosition(context))
station.SetWsgVelocity(context, v)
self.assertEqual(v, station.GetWsgVelocity(context))
self.assertEqual(len(station.get_camera_names()), 1)
self.assertEqual(station.get_multibody_plant().num_model_instances(),
num_station_bodies + len(ycb_objects))
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--target_realtime_rate", type=float, default=1.0,
help="Desired rate relative to real time. See documentation for "
"Simulator::set_target_realtime_rate() for details.")
parser.add_argument(
"--duration", type=float, default=np.inf,
help="Desired duration of the simulation in seconds.")
parser.add_argument(
"--hardware", action='store_true',
help="Use the ManipulationStationHardwareInterface instead of an "
"in-process simulation.")
parser.add_argument(
"--test", action='store_true',
help="Disable opening the gui window for testing.")
parser.add_argument(
"--time_step", type=float, default=0.005,
help="Time constant for the differential IK solver and first order"
"low pass filter applied to the teleop commands")
parser.add_argument(
"--velocity_limit_factor", type=float, default=1.0,
help="This value, typically between 0 and 1, further limits the "
"iiwa14 joint velocities. It multiplies each of the seven "
"pre-defined joint velocity limits. "
"Note: The pre-defined velocity limits are specified by "
"iiwa14_velocity_limits, found in this python file.")
parser.add_argument(
'--setup', type=str, default='manipulation_class',
help="The manipulation station setup to simulate. ",
choices=['manipulation_class', 'clutter_clearing'])
parser.add_argument(
'--schunk_collision_model', type=str, default='box',
help="The Schunk collision model to use for simulation. ",
choices=['box', 'box_plus_fingertip_spheres'])
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
if args.test:
# Don't grab mouse focus during testing.
# See: https://stackoverflow.com/a/52528832/7829525
os.environ["SDL_VIDEODRIVER"] = "dummy"
builder = DiagramBuilder()
if args.hardware:
station = builder.AddSystem(ManipulationStationHardwareInterface())
station.Connect(wait_for_cameras=False)
else:
station = builder.AddSystem(ManipulationStation())
if args.schunk_collision_model == "box":
schunk_model = SchunkCollisionModel.kBox
elif args.schunk_collision_model == "box_plus_fingertip_spheres":
schunk_model = SchunkCollisionModel.kBoxPlusFingertipSpheres
# Initializes the chosen station type.
if args.setup == 'manipulation_class':
station.SetupManipulationClassStation(
schunk_model=schunk_model)
station.AddManipulandFromFile(
("drake/examples/manipulation_station/models/"
"061_foam_brick.sdf"),
RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))
elif args.setup == 'clutter_clearing':
station.SetupClutterClearingStation(
schunk_model=schunk_model)
ycb_objects = CreateClutterClearingYcbObjectList()
for model_file, X_WObject in ycb_objects:
station.AddManipulandFromFile(model_file, X_WObject)
station.Finalize()
DrakeVisualizer.AddToBuilder(builder,
station.GetOutputPort("query_object"))
if args.meshcat:
meshcat = ConnectMeshcatVisualizer(
builder, output_port=station.GetOutputPort("geometry_query"),
zmq_url=args.meshcat, open_browser=args.open_browser)
if args.setup == 'planar':
meshcat.set_planar_viewpoint()
robot = station.get_controller_plant()
params = DifferentialInverseKinematicsParameters(robot.num_positions(),
robot.num_velocities())
params.set_timestep(args.time_step)
# True velocity limits for the IIWA14 (in rad/s, rounded down to the first
# decimal)
iiwa14_velocity_limits = np.array([1.4, 1.4, 1.7, 1.3, 2.2, 2.3, 2.3])
# Stay within a small fraction of those limits for this teleop demo.
factor = args.velocity_limit_factor
params.set_joint_velocity_limits((-factor*iiwa14_velocity_limits,
factor*iiwa14_velocity_limits))
differential_ik = builder.AddSystem(DifferentialIK(
robot, robot.GetFrameByName("iiwa_link_7"), params, args.time_step))
builder.Connect(differential_ik.GetOutputPort("joint_position_desired"),
station.GetInputPort("iiwa_position"))
teleop = builder.AddSystem(DualShock4Teleop(initialize_joystick()))
filter_ = builder.AddSystem(
FirstOrderLowPassFilter(time_constant=args.time_step, size=6))
builder.Connect(teleop.get_output_port(0), filter_.get_input_port(0))
builder.Connect(filter_.get_output_port(0),
differential_ik.GetInputPort("rpy_xyz_desired"))
builder.Connect(teleop.GetOutputPort("position"), station.GetInputPort(
"wsg_position"))
builder.Connect(teleop.GetOutputPort("force_limit"),
station.GetInputPort("wsg_force_limit"))
diagram = builder.Build()
simulator = Simulator(diagram)
# This is important to avoid duplicate publishes to the hardware interface:
simulator.set_publish_every_time_step(False)
station_context = diagram.GetMutableSubsystemContext(
station, simulator.get_mutable_context())
station.GetInputPort("iiwa_feedforward_torque").FixValue(
station_context, np.zeros(7))
# If the diagram is only the hardware interface, then we must advance it a
# little bit so that first LCM messages get processed. A simulated plant is
# already publishing correct positions even without advancing, and indeed
# we must not advance a simulated plant until the sliders and filters have
# been initialized to match the plant.
if args.hardware:
simulator.AdvanceTo(1e-6)
q0 = station.GetOutputPort("iiwa_position_measured").Eval(station_context)
differential_ik.parameters.set_nominal_joint_position(q0)
teleop.SetPose(differential_ik.ForwardKinematics(q0))
filter_.set_initial_output_value(
diagram.GetMutableSubsystemContext(
filter_, simulator.get_mutable_context()),
teleop.get_output_port(0).Eval(diagram.GetMutableSubsystemContext(
teleop, simulator.get_mutable_context())))
differential_ik.SetPositions(diagram.GetMutableSubsystemContext(
differential_ik, simulator.get_mutable_context()), q0)
simulator.set_target_realtime_rate(args.target_realtime_rate)
print_instructions()
simulator.AdvanceTo(args.duration)
