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 main():
station = ManipulationStation()
station.SetupManipulationClassStation()
station.Finalize()
context = station.CreateDefaultContext()
print(context)
def load_station(time_step=0.0):
# https://github.com/RobotLocomotion/drake/blob/master/bindings/pydrake/examples/manipulation_station_py.cc
object_file_path = FindResourceOrThrow(
"drake/external/models_robotlocomotion/ycb_objects/061_foam_brick.sdf")
#object_file_path = FOAM_BRICK_PATH
station = ManipulationStation(time_step)
station.AddCupboard()
mbp = station.get_mutable_multibody_plant()
scene_graph = station.get_mutable_scene_graph()
object = AddModelFromSdfFile(
file_name=object_file_path,
model_name="object",
plant=mbp,
scene_graph=scene_graph)
station.Finalize()
robot = mbp.GetModelInstanceByName('iiwa')
gripper = mbp.GetModelInstanceByName('gripper')
initial_conf = [0, 0.6 - np.pi / 6, 0, -1.75, 0, 1.0, 0]
#initial_conf[1] += np.pi / 6
initial_positions = dict(zip(get_movable_joints(mbp, robot), initial_conf))
initial_poses = {
object: create_transform(translation=[.6, 0, 0]),
}
task = Task(mbp, scene_graph, robot, gripper, movable=[object], surfaces=[],
initial_positions=initial_positions, initial_poses=initial_poses,
goal_on=[])
return mbp, scene_graph, task
def test_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_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 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_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 test_planar_iiwa_setup(self):
station = ManipulationStation(time_step=0.001)
station.SetupPlanarIiwaStation()
station.Finalize()
self.assertEqual(station.num_iiwa_joints(), 3)
context = station.CreateDefaultContext()
q = np.linspace(0.04, 0.6, num=3)
v = np.linspace(-2.3, 0.5, num=3)
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))
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 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()
class ManipulationStationSimulator:
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 RunSimulation(self,
plans_list,
gripper_setpoint_list,
extra_time=0,
real_time_rate=1.0,
q0_kuka=np.zeros(7)):
'''
Constructs a Diagram that sends commands to ManipulationStation.
:param plans_list:
:param gripper_setpoint_list:
:param extra_time:
:param real_time_rate:
:param q0_kuka:
:return:
'''
builder = DiagramBuilder()
builder.AddSystem(self.station)
# Add plan runner.
plan_runner = ManipStationPlanRunner(
station=self.station,
kuka_plans=plans_list,
gripper_setpoint_list=gripper_setpoint_list)
builder.AddSystem(plan_runner)
builder.Connect(plan_runner.hand_setpoint_output_port,
self.station.GetInputPort("wsg_position"))
builder.Connect(plan_runner.gripper_force_limit_output_port,
self.station.GetInputPort("wsg_force_limit"))
demux = builder.AddSystem(Demultiplexer(14, 7))
builder.Connect(
plan_runner.GetOutputPort("iiwa_position_and_torque_command"),
demux.get_input_port(0))
builder.Connect(demux.get_output_port(0),
self.station.GetInputPort("iiwa_position"))
builder.Connect(demux.get_output_port(1),
self.station.GetInputPort("iiwa_feedforward_torque"))
builder.Connect(self.station.GetOutputPort("iiwa_position_measured"),
plan_runner.iiwa_position_input_port)
builder.Connect(self.station.GetOutputPort("iiwa_velocity_estimated"),
plan_runner.iiwa_velocity_input_port)
# Add meshcat visualizer
scene_graph = self.station.get_mutable_scene_graph()
viz = MeshcatVisualizer(scene_graph,
is_drawing_contact_force=True,
plant=self.plant)
self.viz = viz
builder.AddSystem(viz)
builder.Connect(self.station.GetOutputPort("pose_bundle"),
viz.GetInputPort("lcm_visualization"))
builder.Connect(self.station.GetOutputPort("contact_results"),
viz.GetInputPort("contact_results"))
# Add logger
iiwa_position_command_log = LogOutput(demux.get_output_port(0),
builder)
iiwa_position_command_log._DeclarePeriodicPublish(0.005)
iiwa_external_torque_log = LogOutput(
self.station.GetOutputPort("iiwa_torque_external"), builder)
iiwa_external_torque_log._DeclarePeriodicPublish(0.005)
iiwa_position_measured_log = LogOutput(
self.station.GetOutputPort("iiwa_position_measured"), builder)
iiwa_position_measured_log._DeclarePeriodicPublish(0.005)
wsg_state_log = LogOutput(
self.station.GetOutputPort("wsg_state_measured"), builder)
wsg_state_log._DeclarePeriodicPublish(0.1)
wsg_command_log = LogOutput(plan_runner.hand_setpoint_output_port,
builder)
wsg_command_log._DeclarePeriodicPublish(0.1)
# build diagram
diagram = builder.Build()
viz.load()
time.sleep(2.0)
RenderSystemWithGraphviz(diagram)
# construct simulator
simulator = Simulator(diagram)
context = diagram.GetMutableSubsystemContext(
self.station, simulator.get_mutable_context())
# set initial state of the robot
self.station.SetIiwaPosition(q0_kuka, context)
self.station.SetIiwaVelocity(np.zeros(7), context)
self.station.SetWsgPosition(0.05, context)
self.station.SetWsgVelocity(0, context)
# set initial hinge angles of the cupboard.
# setting hinge angle to exactly 0 or 90 degrees will result in intermittent contact
# with small contact forces between the door and the cupboard body.
left_hinge_joint = self.plant.GetJointByName("left_door_hinge")
left_hinge_joint.set_angle(
context=self.station.GetMutableSubsystemContext(
self.plant, context),
angle=-0.001)
right_hinge_joint = self.plant.GetJointByName("right_door_hinge")
right_hinge_joint.set_angle(
context=self.station.GetMutableSubsystemContext(
self.plant, context),
angle=0.001)
# set initial pose of the object
self.plant.tree().SetFreeBodyPoseOrThrow(
self.plant.GetBodyByName(self.object_base_link_name, self.object),
self.X_WObject,
self.station.GetMutableSubsystemContext(self.plant, context))
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(real_time_rate)
simulator.Initialize()
sim_duration = 0.
for plan in plans_list:
sim_duration += plan.get_duration() * 1.1
sim_duration += extra_time
print "simulation duration", sim_duration
simulator.StepTo(sim_duration)
return iiwa_position_command_log, \
iiwa_position_measured_log, \
iiwa_external_torque_log, \
wsg_state_log, \
wsg_command_log
def RunRealRobot(self, plans_list, gripper_setpoint_list):
'''
Constructs a Diagram that sends commands to ManipulationStationHardwareInterface.
:param plans_list:
:param gripper_setpoint_list:
:param extra_time:
:param real_time_rate:
:param q0_kuka:
:return:
'''
builder = DiagramBuilder()
camera_ids = ["805212060544"]
station_hardware = ManipulationStationHardwareInterface(camera_ids)
station_hardware.Connect(wait_for_cameras=False)
builder.AddSystem(station_hardware)
# Add plan runner
# Add plan runner.
plan_runner = ManipStationPlanRunner(
station=self.station,
kuka_plans=plans_list,
gripper_setpoint_list=gripper_setpoint_list)
builder.AddSystem(plan_runner)
builder.Connect(plan_runner.hand_setpoint_output_port,
station_hardware.GetInputPort("wsg_position"))
builder.Connect(plan_runner.gripper_force_limit_output_port,
station_hardware.GetInputPort("wsg_force_limit"))
demux = builder.AddSystem(Demultiplexer(14, 7))
builder.Connect(
plan_runner.GetOutputPort("iiwa_position_and_torque_command"),
demux.get_input_port(0))
builder.Connect(demux.get_output_port(0),
station_hardware.GetInputPort("iiwa_position"))
builder.Connect(
demux.get_output_port(1),
station_hardware.GetInputPort("iiwa_feedforward_torque"))
builder.Connect(
station_hardware.GetOutputPort("iiwa_position_measured"),
plan_runner.iiwa_position_input_port)
builder.Connect(
station_hardware.GetOutputPort("iiwa_velocity_estimated"),
plan_runner.iiwa_velocity_input_port)
# Add logger
iiwa_position_measured_log = LogOutput(
station_hardware.GetOutputPort("iiwa_position_measured"), builder)
iiwa_position_measured_log._DeclarePeriodicPublish(0.005)
iiwa_external_torque_log = LogOutput(
station_hardware.GetOutputPort("iiwa_torque_external"), builder)
iiwa_external_torque_log._DeclarePeriodicPublish(0.005)
wsg_state_log = LogOutput(
station_hardware.GetOutputPort("wsg_state_measured"), builder)
wsg_state_log._DecalrePeriodicPublish(0.1)
wsg_command_log = LogOutput(plan_runner.hand_setpoint_output_port,
builder)
wsg_command_log._DeclarePeriodicPublish(0.1)
# build diagram
diagram = builder.Build()
RenderSystemWithGraphviz(diagram)
# construct simulator
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
sim_duration = 0.
for plan in plans_list:
sim_duration += plan.get_duration() * 1.1
print "sending trajectories in 2 seconds..."
time.sleep(1.0)
print "sending trajectories in 1 second..."
time.sleep(1.0)
print "sending trajectories now!"
simulator.StepTo(sim_duration)
return iiwa_position_measured_log, iiwa_external_torque_log, wsg_state_log, wsg_command_log
def Replay(self):
self.viz.replay()
class ManipulationStationSimulator:
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 RunSimulation(self,
plan_list,
gripper_setpoint_list,
extra_time=0,
real_time_rate=1.0,
q0_kuka=np.zeros(7),
is_visualizing=True,
sim_duration=None,
is_plan_runner_diagram=False):
"""
Constructs a Diagram that sends commands to ManipulationStation.
@param plan_list: A list of Plans to be executed.
@param gripper_setpoint_list: A list of gripper setpoints. Each setpoint corresponds to a Plan.
@param extra_time: the amount of time for which commands are sent,
in addition to the sum of the durations of all plans.
@param real_time_rate: 1.0 means realtime; 0 means as fast as possible.
@param q0_kuka: initial configuration of the robot.
@param is_visualizing: if true, adds MeshcatVisualizer to the Diagram. It should be set to False
when running tests.
@param sim_duration: the duration of simulation in seconds. If unset, it is set to the sum of the durations of all
plans in plan_list plus extra_time.
@param is_plan_runner_diagram: True: use the diagram version of PlanRunner; False: use the leaf version
of PlanRunner.
@return: logs of robot configuration and MultibodyPlant, generated by simulation.
Logs are SignalLogger systems, whose data can be accessed by SignalLogger.data().
"""
builder = DiagramBuilder()
builder.AddSystem(self.station)
# Add plan runner.
if is_plan_runner_diagram:
plan_runner, duration_multiplier = CreateManipStationPlanRunnerDiagram(
kuka_plans=plan_list,
gripper_setpoint_list=gripper_setpoint_list)
else:
plan_runner = ManipStationPlanRunner(
kuka_plans=plan_list,
gripper_setpoint_list=gripper_setpoint_list)
duration_multiplier = plan_runner.kPlanDurationMultiplier
self.plan_runner = plan_runner
builder.AddSystem(plan_runner)
builder.Connect(plan_runner.GetOutputPort("gripper_setpoint"),
self.station.GetInputPort("wsg_position"))
builder.Connect(plan_runner.GetOutputPort("force_limit"),
self.station.GetInputPort("wsg_force_limit"))
builder.Connect(plan_runner.GetOutputPort("iiwa_position_command"),
self.station.GetInputPort("iiwa_position"))
builder.Connect(plan_runner.GetOutputPort("iiwa_torque_command"),
self.station.GetInputPort("iiwa_feedforward_torque"))
builder.Connect(self.station.GetOutputPort("iiwa_position_measured"),
plan_runner.GetInputPort("iiwa_position"))
builder.Connect(self.station.GetOutputPort("iiwa_velocity_estimated"),
plan_runner.GetInputPort("iiwa_velocity"))
builder.Connect(self.station.GetOutputPort("iiwa_torque_external"),
plan_runner.GetInputPort("iiwa_torque_external"))
# Add meshcat visualizer
if is_visualizing:
scene_graph = self.station.get_mutable_scene_graph()
viz = MeshcatVisualizer(scene_graph,
is_drawing_contact_force=True,
plant=self.plant)
builder.AddSystem(viz)
builder.Connect(self.station.GetOutputPort("pose_bundle"),
viz.GetInputPort("lcm_visualization"))
builder.Connect(self.station.GetOutputPort("contact_results"),
viz.GetInputPort("contact_results"))
# Add logger
iiwa_position_command_log = LogOutput(
plan_runner.GetOutputPort("iiwa_position_command"), builder)
iiwa_position_command_log._DeclarePeriodicPublish(0.005)
iiwa_external_torque_log = LogOutput(
self.station.GetOutputPort("iiwa_torque_external"), builder)
iiwa_external_torque_log._DeclarePeriodicPublish(0.005)
iiwa_position_measured_log = LogOutput(
self.station.GetOutputPort("iiwa_position_measured"), builder)
iiwa_position_measured_log._DeclarePeriodicPublish(0.005)
plant_state_log = LogOutput(
self.station.GetOutputPort("plant_continuous_state"), builder)
plant_state_log._DeclarePeriodicPublish(0.005)
# build diagram
diagram = builder.Build()
if is_visualizing:
viz.load()
time.sleep(2.0)
RenderSystemWithGraphviz(diagram)
# construct simulator
simulator = Simulator(diagram)
self.simulator = simulator
context = diagram.GetMutableSubsystemContext(
self.station, simulator.get_mutable_context())
# set initial state of the robot
self.station.SetIiwaPosition(q0_kuka, context)
self.station.SetIiwaVelocity(np.zeros(7), context)
self.station.SetWsgPosition(0.05, context)
self.station.SetWsgVelocity(0, context)
# set initial hinge angles of the cupboard.
# setting hinge angle to exactly 0 or 90 degrees will result in intermittent contact
# with small contact forces between the door and the cupboard body.
left_hinge_joint = self.plant.GetJointByName("left_door_hinge")
left_hinge_joint.set_angle(
context=self.station.GetMutableSubsystemContext(
self.plant, context),
angle=-0.001)
right_hinge_joint = self.plant.GetJointByName("right_door_hinge")
right_hinge_joint.set_angle(
context=self.station.GetMutableSubsystemContext(
self.plant, context),
angle=0.001)
# set initial pose of the object
if self.object_base_link_name is not None:
self.plant.tree().SetFreeBodyPoseOrThrow(
self.plant.GetBodyByName(self.object_base_link_name,
self.object), self.X_WObject,
self.station.GetMutableSubsystemContext(self.plant, context))
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(real_time_rate)
# calculate starting time for all plans.
t_plan = GetPlanStartingTimes(plan_list, duration_multiplier)
if sim_duration is None:
sim_duration = t_plan[-1] + extra_time
print "simulation duration", sim_duration
print "plan starting times\n", t_plan
simulator.Initialize()
self.SetInitialPlanRunnerState(plan_runner, simulator, diagram)
simulator.StepTo(sim_duration)
return iiwa_position_command_log, iiwa_position_measured_log, \
iiwa_external_torque_log, plant_state_log, t_plan
def RunRealRobot(
self,
plan_list,
gripper_setpoint_list,
sim_duration=None,
extra_time=2.0,
is_plan_runner_diagram=False,
):
"""
Constructs a Diagram that sends commands to ManipulationStationHardwareInterface.
@param plan_list: A list of Plans to be executed.
@param gripper_setpoint_list: A list of gripper setpoints. Each setpoint corresponds to a Plan.
@param sim_duration: the duration of simulation in seconds. If unset, it is set to the sum of the durations of
all plans in plan_list plus extra_time.
@param extra_time: the amount of time for which commands are sent, in addition to the duration of all plans.
@param is_plan_runner_diagram: True: use the diagram version of PlanRunner; False: use the leaf version
of PlanRunner.
@return: logs of robot configuration and torque, decoded from LCM messges sent by the robot's driver.
Logs are SignalLogger systems, whose data can be accessed by SignalLogger.data().
"""
builder = DiagramBuilder()
camera_ids = ["805212060544"]
station_hardware = ManipulationStationHardwareInterface(camera_ids)
station_hardware.Connect(wait_for_cameras=False)
builder.AddSystem(station_hardware)
# Add plan runner.
if is_plan_runner_diagram:
plan_runner, duration_multiplier = CreateManipStationPlanRunnerDiagram(
kuka_plans=plan_list,
gripper_setpoint_list=gripper_setpoint_list,
print_period=0,
)
else:
plan_runner = ManipStationPlanRunner(
kuka_plans=plan_list,
gripper_setpoint_list=gripper_setpoint_list,
print_period=0,
)
duration_multiplier = plan_runner.kPlanDurationMultiplier
builder.AddSystem(plan_runner)
builder.Connect(plan_runner.GetOutputPort("gripper_setpoint"),
station_hardware.GetInputPort("wsg_position"))
builder.Connect(plan_runner.GetOutputPort("force_limit"),
station_hardware.GetInputPort("wsg_force_limit"))
builder.Connect(plan_runner.GetOutputPort("iiwa_position_command"),
station_hardware.GetInputPort("iiwa_position"))
builder.Connect(
plan_runner.GetOutputPort("iiwa_torque_command"),
station_hardware.GetInputPort("iiwa_feedforward_torque"))
builder.Connect(
station_hardware.GetOutputPort("iiwa_position_measured"),
plan_runner.GetInputPort("iiwa_position"))
builder.Connect(
station_hardware.GetOutputPort("iiwa_velocity_estimated"),
plan_runner.GetInputPort("iiwa_velocity"))
builder.Connect(station_hardware.GetOutputPort("iiwa_torque_external"),
plan_runner.GetInputPort("iiwa_torque_external"))
# Add logger
iiwa_position_command_log = LogOutput(
plan_runner.GetOutputPort("iiwa_position_command"), builder)
iiwa_position_command_log._DeclarePeriodicPublish(0.005)
iiwa_position_measured_log = LogOutput(
station_hardware.GetOutputPort("iiwa_position_measured"), builder)
iiwa_position_measured_log._DeclarePeriodicPublish(0.005)
iiwa_velocity_estimated_log = LogOutput(
station_hardware.GetOutputPort("iiwa_velocity_estimated"), builder)
iiwa_velocity_estimated_log._DeclarePeriodicPublish(0.005)
iiwa_external_torque_log = LogOutput(
station_hardware.GetOutputPort("iiwa_torque_external"), builder)
iiwa_external_torque_log._DeclarePeriodicPublish(0.005)
# build diagram
diagram = builder.Build()
# RenderSystemWithGraphviz(diagram)
# construct simulator
simulator = Simulator(diagram)
self.simulator = simulator
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
t_plan = GetPlanStartingTimes(plan_list, duration_multiplier)
if sim_duration is None:
sim_duration = t_plan[-1] + extra_time
print "simulation duration", sim_duration
print "plan starting times\n", t_plan
print "sending trajectories in 2 seconds..."
time.sleep(1.0)
print "sending trajectories in 1 second..."
time.sleep(1.0)
print "sending trajectories now!"
simulator.Initialize()
self.SetInitialPlanRunnerState(plan_runner, simulator, diagram)
simulator.StepTo(sim_duration)
return iiwa_position_command_log, iiwa_position_measured_log, \
iiwa_velocity_estimated_log, iiwa_external_torque_log, t_plan
def SetInitialPlanRunnerState(self, plan_runner, simulator, diagram):
"""
Sets iiwa_position_command, part of the discrete state of plan_runner,
to the initial state of the robot at t=0.
Otherwise the position command at t=0 is 0, driving the robot to its upright position, usually
resulting in huge velocity.
Calling this function after simulator.Initialize() puts a column of zeros at the beginning of
iiwa_position_command_log, but that zero command doesn't seem to be sent to the robot.
TODO: turning off publishing at initialization should remove those zeros, but the findings for
that function doesn't exist yet.
"""
plan_runner_context = \
diagram.GetMutableSubsystemContext(plan_runner, simulator.get_mutable_context())
iiwa_position_input_port = plan_runner.GetInputPort("iiwa_position")
q0_iiwa = plan_runner.EvalVectorInput(
plan_runner_context,
iiwa_position_input_port.get_index()).get_value()
for i in range(7):
plan_runner_context.get_mutable_discrete_state(0).SetAtIndex(
i, q0_iiwa[i])
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([[
0.,
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 load_station(time_step=0.0, plan=None):
# TODO: map names to SDF paths
# 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 = FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf")
# RuntimeError: Error control wants to select step smaller than minimum allowed (1e-14)
station = ManipulationStation(time_step, IiwaCollisionModel.kBoxCollision)
plant = station.get_mutable_multibody_plant()
scene_graph = station.get_mutable_scene_graph()
robot = plant.GetModelInstanceByName('iiwa')
gripper = plant.GetModelInstanceByName('gripper')
station.AddCupboard()
brick = AddModelFromSdfFile(file_name=object_file_path,
model_name="brick",
plant=plant,
scene_graph=scene_graph)
station.Finalize()
door_names = ['left_door', 'right_door']
doors = [plant.GetBodyByName(name).index() for name in door_names]
initial_conf = [0, 0.6 - np.pi / 6, 0, -1.75, 0, 1.0, 0]
#initial_conf[1] += np.pi / 6
initial_positions = {
plant.GetJointByName('left_door_hinge'):
-DOOR_CLOSED,
#plant.GetJointByName('left_door_hinge'): -np.pi / 2,
plant.GetJointByName('right_door_hinge'):
np.pi / 2,
#plant.GetJointByName('right_door_hinge'): np.pi,
}
initial_positions.update(
zip(get_movable_joints(plant, robot), initial_conf))
initial_poses = {
brick:
create_transform(translation=[0.3, 0, 0], rotation=[0, 0, np.pi / 2]),
}
goal_poses = {
brick:
create_transform(translation=[0.8, 0.2, 0.2927],
rotation=[0, 0, 5 * np.pi / 4]),
}
diagram, state_machine = build_manipulation_station(station, plan)
task = Task(diagram,
plant,
scene_graph,
robot,
gripper,
movable=[brick],
doors=doors,
initial_positions=initial_positions,
initial_poses=initial_poses,
goal_poses=goal_poses,
reset_robot=True,
reset_doors=False)
task.set_initial()
return task, diagram, state_machine
class MotionPlanning:
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 lazy_sp(self, max_iters=1000, goal_sample_prob=0.05, position_tolerance=0.09, duration=5):
""" Motion Planning by LazySP
:param max_iters: max number of iterations
:param goal_sample_prob: probability to sample goal in each iteration
:param position_tolerance: tolerance in goal position
:param duration: duration of motion
:return: path found or raise error
"""
util = self.util
cspace = self.cspace
rrt = RRT(TreeNode(self.q_initial), cspace)
q_goals = []
for i in range(max_iters):
print("iter: ", i)
sample = self.sample_config(util, goal_sample_prob)
neighbor = rrt.nearest(sample)
# Find best node for reaching q_new
min_cost = rrt.cost(neighbor) + cspace.distance(neighbor.value, sample)
q_min_node = neighbor
# Stores q near nodes, and dist to q_new
Q_near = [[q_node, None] for q_node in rrt.near(sample, max_cost=10)]
print("num near nodes: {}".format(len(Q_near)))
for i, v in enumerate(Q_near):
q_near_node, _ = v
dist_to_new = cspace.distance(q_near_node.value, sample)
Q_near[i][1] = dist_to_new
cost = rrt.cost(q_near_node) + dist_to_new
if cost < min_cost:
min_cost = cost
q_min_node = q_near_node
q_new_node = rrt.add_configuration(q_min_node, sample)
# Rewiring existing nodes
new_node_cost = rrt.cost(q_new_node)
rewires = 0
for q_near_node, dist_to_new in Q_near:
if (q_near_node != q_min_node and
rrt.cost(q_near_node) > new_node_cost + dist_to_new):
rewires += 1
rrt.change_parent(q_new_node, q_near_node)
print("Num rewires: {}".format(rewires))
self.rewires += rewires
# check for goal
translation = util.FK(sample).translation()
if self.within_tol(translation, self.p_goal, position_tolerance):
q_goals.append(q_new_node)
def filter(goals):
""" Filter out Nones in goals
"""
return [g for g in goals if g is not None]
def in_free_edges(path, edges):
"""
:param path: list of nodes
:param edges: all free edges
:return: True if all edges between nodes are in free edges
"""
for s, e in zip(path[:-1], path[1:]):
if (s, e) not in edges:
return False
return True
def edge_selector(path, collision_free_edges):
"""
:param path: list of nodes
:param collision_free_edges: list of edges
:return: first edge in path that is in free edges
"""
for s, e in zip(path[:-1], path[1:]):
if (s, e) not in collision_free_edges:
return s, e
raise Exception("Unexpected that all edges are collision free")
collision_free_edges = []
while len(q_goals) > 0:
best_path, min_cost = rrt.shortest_path(q_goals)
if best_path is None:
break
# Get shortest path and check for collision
q_goals = filter(q_goals)
if in_free_edges(best_path, collision_free_edges):
best_path = np.array([node.value for node in best_path])
num_knot_points = best_path.shape[0]
t_knots = np.linspace(0, duration, num_knot_points)
qtraj = PiecewisePolynomial.Cubic(t_knots, best_path.T, np.zeros(7), np.zeros(7))
print("Path cost: {}".format(min_cost))
print("Total number of rewires: {}".format(self.rewires))
return [JointSpacePlan(qtraj)], [0.1]
# If no collision, add to free edges; otherwise, remove from tree
start, end = edge_selector(best_path, collision_free_edges)
if self.obstacle_free(cspace, start.value, end.value, util):
collision_free_edges.append((start, end))
else:
rrt.remove_edge(start, end)
raise Exception("Did not find path to goal")
def rrt(self, star=False, max_iters=1000, goal_sample_prob=0.05, position_tolerance=0.09, duration=5):
""" Motion Planning by RRT and RRT*
:param star: True if RRT*
:param max_iters: max number of iterations
:param goal_sample_prob: probability to sample goal position
:param position_tolerance: tolerance to goal position
:param duration: duration of motion
:return: path found or raise error
"""
util = self.util
cspace = self.cspace
rrt = RRT(TreeNode(self.q_initial), cspace)
q_goals = []
for i in range(max_iters):
print("iter: ", i)
sample = self.sample_config(util, goal_sample_prob)
neighbor = rrt.nearest(sample)
path = self.safe_path(cspace, neighbor.value, sample, util)
if len(path) > 1:
q_end = path[-1]
# If pure RRT, include intermediate points as nodes
q_new_node = neighbor
add_middle_nodes = not star
if add_middle_nodes:
middle_path = path[1:-1:10]
else:
middle_path = []
for q in middle_path + [q_end]:
if not star:
q_new_node = self.rrt_extend(rrt, q_new_node, q)
else:
q_new_node = self.rrt_star_extend(rrt, q_new_node, q)
# check for goal
translation = util.FK(q_end).translation()
if self.within_tol(translation, self.p_goal, position_tolerance):
q_goals.append(q_new_node)
if ((not star and len(q_goals) > 0) or
(star and len(q_goals) > 0 and i == max_iters-1)):
min_cost = None
best_path = None
for q_goal_node in q_goals:
path = np.array(rrt.bfs(q_goal_node.value))
cost = rrt.cost(q_goal_node)
if min_cost is None or cost < min_cost:
min_cost = cost
best_path = path
num_knot_points = best_path.shape[0]
t_knots = np.linspace(0, duration, num_knot_points)
qtraj = PiecewisePolynomial.Cubic(t_knots, best_path.T, np.zeros(7), np.zeros(7))
print("Path cost: {}".format(min_cost))
print("Total number of rewires: {}".format(self.rewires))
return [JointSpacePlan(qtraj)], [0.1]
raise Exception("Did not find path to goal")
def rrt_extend(self, rrt, neighbor, q_new):
""" Extend the tree by q_new from neighbor (RRT)
"""
return rrt.add_configuration(neighbor, q_new)
def rrt_star_extend(self, rrt, neighbor, q_new):
""" Extend the tree by q_new from neighbor while doing rewiring (RRT*)
"""
cspace = rrt.cspace
# Find best node for reaching q_new
min_cost = rrt.cost(neighbor) + cspace.distance(neighbor.value, q_new)
q_min_node = neighbor
# Stores q near nodes, whether obstacle free, and dist to q_new
Q_near = [[q_node, None, None] for q_node in rrt.near(q_new, max_cost=10)]
print("num near nodes: {}".format(len(Q_near)))
# Find best node to reach
for i, v in enumerate(Q_near):
q_near_node, _, _ = v
dist_to_new = cspace.distance(q_near_node.value, q_new)
Q_near[i][2] = dist_to_new
cost = rrt.cost(q_near_node) + dist_to_new
if cost < min_cost:
if self.obstacle_free(cspace, q_near_node.value, q_new, self.util):
Q_near[i][1] = True
min_cost = cost
q_min_node = q_near_node
q_new_node = rrt.add_configuration(q_min_node, q_new)
# Rewiring existing nodes
new_node_cost = rrt.cost(q_new_node)
rewires = 0
for q_near_node, collision_free, dist_to_new in Q_near:
if (q_near_node != q_min_node and
rrt.cost(q_near_node) > new_node_cost + dist_to_new):
if collision_free or (collision_free is None and
self.obstacle_free(cspace, q_near_node.value, q_new, self.util)):
rewires += 1
rrt.change_parent(q_new_node, q_near_node)
print("Num rewires: {}".format(rewires))
self.rewires += rewires
return q_new_node
def sample_config(self, util, goal_sample_prob):
""" Sample a random configuration with goal_sample_prob of sampling a goal
"""
prob = random()
if prob < goal_sample_prob:
q_goal = None
it = 0
while q_goal is None:
# print("goal sampling iter: ", it)
it += 1
q_goal_sample = util.IK(self.p_goal, is_goal=False)
if not util.collision(q_goal_sample):
q_goal = q_goal_sample
return q_goal
else:
q = None
it = 0
while q is None:
# print("non goal sampling iter: ", it)
it += 1
q_sample = util.random_q()
if not util.collision(q_sample):
q = q_sample
return q
class ManipulationStationCollisionChecker:
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 SetStationConfiguration(self, q_iiwa, gripper_setpoint,
left_door_angle, right_door_angle):
"""
:param q_iiwa: (7,) numpy array, joint angle of robots in radian.
:param gripper_setpoint: float, gripper opening distance in meters.
:param left_door_angle: float, left door hinge angle, \in [0, pi/2].
:param right_door_angle: float, right door hinge angle, \in [0, pi/2].
:return:
"""
self.station.SetIiwaPosition(self.context_station, q_iiwa)
self.station.SetWsgPosition(self.context_station, gripper_setpoint)
# cabinet doors
if left_door_angle > 0:
left_door_angle *= -1
left_hinge_joint = self.plant.GetJointByName("left_door_hinge")
left_hinge_joint.set_angle(context=self.context_plant,
angle=left_door_angle)
right_hinge_joint = self.plant.GetJointByName("right_door_hinge")
right_hinge_joint.set_angle(context=self.context_plant,
angle=right_door_angle)
def DrawStation(self, q_iiwa, gripper_setpoint, q_door_left, q_door_right):
if not self.is_visualizing:
print("collision checker is not initialized with visualization.")
return
self.SetStationConfiguration(q_iiwa, gripper_setpoint, q_door_left,
q_door_right)
self.viz.DoPublish(self.context_meshcat, [])
def ExistsCollision(self, q_iiwa, gripper_setpoint, q_door_left,
q_door_right):
self.SetStationConfiguration(q_iiwa, gripper_setpoint, q_door_left,
q_door_right)
query_object = self.query_output_port.Eval(self.context_scene_graph)
collision_paris = query_object.ComputePointPairPenetration()
return len(collision_paris) > 0
def GenerateIiwaTrajectoriesAndGripperSetPoints(X_WO, is_printing=True):
"""
:param X_WO: the pose of the foam brick in world frame.
:param is_printing: set to True to print IK solution results.
:return: 1. a list of joint space trajectories.
2. a list of gripper set points.
The two lists must have the same length.
"""
station = ManipulationStation()
station.SetupManipulationClassStation()
station.Finalize()
plant = station.get_controller_plant()
# Go to p_WQ_home
ik_iiwa = inverse_kinematics.InverseKinematics(plant)
world_frame = plant.world_frame()
l7_frame = plant.GetFrameByName("iiwa_link_7")
theta_bound = 0.005 * np.pi
R_WL7 = RollPitchYaw(0, np.pi * 5 / 6, 0).ToRotationMatrix()
ik_iiwa.AddOrientationConstraint(
frameAbar=world_frame, R_AbarA=R_WL7, # rotate end effector to R_WL7 angle
frameBbar=l7_frame, R_BbarB=RotationMatrix(),
theta_bound=theta_bound)
ik_iiwa.AddPositionConstraint(
frameB=l7_frame, p_BQ=p_L7Q,
frameA=world_frame,
p_AQ_lower=p_WQ_home - 0.01, #p_WQ_home corresponds to q_val_0's pos
p_AQ_upper=p_WQ_home + 0.01)
prog = ik_iiwa.prog()
prog.SetInitialGuess(ik_iiwa.q(), np.zeros(7))
result = mp.Solve(prog)
if is_printing:
print(result.get_solution_result())
q_val_0 = result.GetSolution(ik_iiwa.q())
qtraj_leave_home = ConnectPointsWithCubicPolynomial(q_home, q_val_0, 2.0)
# close fingers
q_knots = np.zeros((2, 7))
q_knots[0] = qtraj_leave_home.value(qtraj_leave_home.end_time()).squeeze()
q_knots[1] = q_knots[0]
qtraj_open_gripper = PiecewisePolynomial.ZeroOrderHold([0, 1], q_knots.T)
# Complete your pick and place trajectories.
def MoveEndEffectorAlongStraightLine(
p_WQ_start, p_WQ_end, duration, get_orientation, q_initial_guess, num_knots):
"""
p_WQ_start is the starting position of point Q (the point Q we defined earlier!)
p_WQ_end is the end position of point Q.
duration is the duration of the trajectory measured in seconds.
get_orientation(i, n) is a function that returns the interpolated orientation R_WL7 at the i-th knot point out of n knot points. It can simply return a constant orientation, but changing the orientation of link 7 should be helpful for placing the object on the shelf.
num_knots is the number of knot points in the trajectory. Generally speaking,the larger num_knots is, the smoother the trajectory, but solving for more knot points also takes longer. You can start by setting num_knots to 10, which should be sufficient for the task.
"""
t_knots = np.linspace(0, duration, num_knots+1)
q_knots = np.zeros((num_knots, plant.num_positions()))
q_knots[0] = q_initial_guess
n = len(q_initial_guess)
for i in range(num_knots):
ik = inverse_kinematics.InverseKinematics(plant)
prog = ik.prog()
p_AQ = p_WQ_start + (p_WQ_end - p_WQ_start) * i / (num_knots - 1)
ik.AddPositionConstraint(frameB=l7_frame, p_BQ=p_L7Q, frameA=world_frame, p_AQ_lower=p_AQ - 0.01, p_AQ_upper=p_AQ + 0.01) # interpolate between p_WQ_start and p_WQ_end
ik.AddOrientationConstraint(frameAbar=world_frame, R_AbarA=get_orientation(i, num_knots), frameBbar=l7_frame, R_BbarB=RotationMatrix(), theta_bound=theta_bound) # call get_orientation(i, num_knots).
if i == 0:
prog.SetInitialGuess(ik.q(), q_initial_guess)
else:
# This is very important for the smoothness of the whole trajectory.
prog.SetInitialGuess(ik.q(), q_knots[i - 1])
result = mp.Solve(ik.prog())
assert result.get_solution_result() == mp.SolutionResult.kSolutionFound
q_knots[i] = result.GetSolution(ik.q())
return PiecewisePolynomial.Cubic(t_knots[1:], q_knots.T, np.zeros(n), np.zeros(n)) # return a cubic spline that connects all q_knots.
q_curr = q_knots[1]
p_mid = np.array([0.53, 0.02, 0.38])
a_box = 0.14
tilt_link_7 = lambda i, n: RollPitchYaw(a_box*i/(n-1), 5/6*np.pi, 0).ToRotationMatrix()
RPY = RollPitchYaw(a_box, np.pi * 5 / 6, 0).ToRotationMatrix()
qtraj_down = MoveEndEffectorAlongStraightLine(p_WQ_home, p_mid, 3, tilt_link_7, q_curr, 10)
q_curr = qtraj_down.value(qtraj_down.end_time()).squeeze()
p_box = np.array([0.52, 0.033, 0.18])
qtraj_down2 = MoveEndEffectorAlongStraightLine(p_mid, p_box, 4, lambda i, n: RPY, q_curr, 10)
q_curr = qtraj_down2.value(qtraj_down2.end_time()).squeeze()
p_up = np.array([0.53, 0.033, 0.38])
qtraj_up = MoveEndEffectorAlongStraightLine(p_box, p_up, 4, lambda i, n: RPY, q_curr, 10)
q_curr = qtraj_up.value(qtraj_up.end_time()).squeeze()
p_down = np.array([0.52, 0.033, 0.21])
qtraj_backdown = MoveEndEffectorAlongStraightLine(p_up, p_down, 4, lambda i, n: RPY, q_curr, 10)
q_curr = qtraj_backdown.value(qtraj_backdown.end_time()).squeeze()
qtraj_return = ConnectPointsWithCubicPolynomial(q_curr, q_home, 3.0)
q_traj_list = [qtraj_leave_home,
qtraj_open_gripper,
qtraj_down,
qtraj_down2,
qtraj_up,
qtraj_backdown,
qtraj_return
]
gripper_setpoint_list = [0.01, 0.1, 0.1, 0.1, 0.029, 0.029, 0.1]
return q_traj_list, gripper_setpoint_list
class ManipulationStationSimulator:
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 RunSimulation(self,
plans_list,
gripper_setpoint_list,
extra_time=0,
real_time_rate=1.0,
q0_kuka=np.zeros(7)):
'''
Constructs a Diagram that sends commands to ManipulationStation.
:param plans_list:
:param gripper_setpoint_list:
:param extra_time:
:param real_time_rate:
:param q0_kuka:
:return:
'''
builder = DiagramBuilder()
builder.AddSystem(self.station)
# Add plan runner
plan_runner = KukaPlanRunner(self.plant)
builder.AddSystem(plan_runner)
builder.Connect(plan_runner.get_output_port(0),
self.station.GetInputPort("iiwa_position"))
# Add state machine.
state_machine = ManipStateMachine(
plant=self.plant,
kuka_plans=plans_list,
gripper_setpoint_list=gripper_setpoint_list)
builder.AddSystem(state_machine)
builder.Connect(state_machine.kuka_plan_output_port,
plan_runner.plan_input_port)
builder.Connect(state_machine.hand_setpoint_output_port,
self.station.GetInputPort("wsg_position"))
builder.Connect(state_machine.gripper_force_limit_output_port,
self.station.GetInputPort("wsg_force_limit"))
builder.Connect(self.station.GetOutputPort("iiwa_position_measured"),
state_machine.iiwa_position_input_port)
# Add meshcat visualizer
from underactuated.meshcat_visualizer import MeshcatVisualizer
scene_graph = self.station.get_mutable_scene_graph()
viz = MeshcatVisualizer(scene_graph)
builder.AddSystem(viz)
builder.Connect(self.station.GetOutputPort("pose_bundle"),
viz.get_input_port(0))
# Add logger
iiwa_position_command_log = builder.AddSystem(
SignalLogger(self.station.GetInputPort("iiwa_position").size()))
iiwa_position_command_log._DeclarePeriodicPublish(0.005)
builder.Connect(plan_runner.get_output_port(0),
iiwa_position_command_log.get_input_port(0))
# build diagram
diagram = builder.Build()
viz.load()
time.sleep(2.0)
RenderSystemWithGraphviz(diagram)
# construct simulator
simulator = Simulator(diagram)
context = diagram.GetMutableSubsystemContext(
self.station, simulator.get_mutable_context())
# set initial state of the robot
self.station.SetIiwaPosition(q0_kuka, context)
self.station.SetIiwaVelocity(np.zeros(7), context)
self.station.SetWsgState(0.05, 0, context)
# set initial hinge angles of the cupboard.
left_hinge_joint = self.plant.GetJointByName("left_door_hinge")
left_hinge_joint.set_angle(
context=self.station.GetMutableSubsystemContext(
self.plant, context),
angle=-np.pi / 2)
right_hinge_joint = self.plant.GetJointByName("right_door_hinge")
right_hinge_joint.set_angle(
context=self.station.GetMutableSubsystemContext(
self.plant, context),
angle=np.pi / 2)
# set initial pose of the object
X_WObject = Isometry3.Identity()
X_WObject.set_translation([.6, 0, 0])
self.plant.tree().SetFreeBodyPoseOrThrow(
self.plant.GetBodyByName(self.object_base_link_name, self.object),
X_WObject,
self.station.GetMutableSubsystemContext(self.plant, context))
# fix feedforward torque input to 0.
context.FixInputPort(
self.station.GetInputPort("iiwa_feedforward_torque").get_index(),
np.zeros(7))
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(real_time_rate)
simulator.Initialize()
sim_duration = 0.
for plan in plans_list:
sim_duration += plan.get_duration() * 1.1
sim_duration += extra_time
print "simulation duration", sim_duration
simulator.StepTo(sim_duration)
return iiwa_position_command_log
def RunRealRobot(self, plans_list, gripper_setpoint_list):
'''
Constructs a Diagram that sends commands to ManipulationStationHardwareInterface.
:param plans_list:
:param gripper_setpoint_list:
:param extra_time:
:param real_time_rate:
:param q0_kuka:
:return:
'''
from pydrake.examples.manipulation_station import ManipulationStationHardwareInterface
builder = DiagramBuilder()
self.station_hardware = ManipulationStationHardwareInterface()
builder.AddSystem(self.station_hardware)
# Add plan runner
plan_runner = KukaPlanRunner(self.plant)
builder.AddSystem(plan_runner)
builder.Connect(plan_runner.get_output_port(0),
self.station_hardware.GetInputPort("iiwa_position"))
# Add state machine.
state_machine = ManipStateMachine(
plant=self.plant,
kuka_plans=plans_list,
gripper_setpoint_list=gripper_setpoint_list)
builder.AddSystem(state_machine)
builder.Connect(state_machine.kuka_plan_output_port,
plan_runner.plan_input_port)
builder.Connect(state_machine.hand_setpoint_output_port,
self.station_hardware.GetInputPort("wsg_position"))
builder.Connect(state_machine.gripper_force_limit_output_port,
self.station_hardware.GetInputPort("wsg_force_limit"))
builder.Connect(
self.station_hardware.GetOutputPort("iiwa_position_measured"),
state_machine.iiwa_position_input_port)
# Add logger
iiwa_position_command_log = builder.AddSystem(
SignalLogger(
self.station_hardware.GetInputPort("iiwa_position").size()))
builder.Connect(plan_runner.get_output_port(0),
iiwa_position_command_log.get_input_port(0))
# build diagram
diagram = builder.Build()
RenderSystemWithGraphviz(diagram)
# construct simulator
simulator = Simulator(diagram)
context = diagram.GetMutableSubsystemContext(
self.station_hardware, simulator.get_mutable_context())
context.FixInputPort(
self.station.GetInputPort("iiwa_feedforward_torque").get_index(),
np.zeros(7))
simulator.set_target_realtime_rate(1.0)
simulator.Initialize()
# simulator.set_publish_at_initialization(False)
sim_duration = 0.
for plan in plans_list:
sim_duration += plan.get_duration() * 1.1
print "sending trajectories in 2 seconds..."
time.sleep(1.0)
print "sending trajectories in 1 second..."
time.sleep(1.0)
print "sending trajectories now!"
simulator.StepTo(sim_duration)
return iiwa_position_command_log
def load_dope(time_step=0.0,
dope_path=DOPE_PATH,
goal_name='soup',
is_visualizing=True,
**kwargs):
station = ManipulationStation(time_step, IiwaCollisionModel.kBoxCollision)
plant = station.get_mutable_multibody_plant()
scene_graph = station.get_mutable_scene_graph()
station.AddCupboard()
robot = plant.GetModelInstanceByName('iiwa')
gripper = plant.GetModelInstanceByName('gripper')
cupboard = plant.GetModelInstanceByName('cupboard')
ceiling = AddModelFromSdfFile(file_name=PLANE_FILE_PATH,
model_name="ceiling",
plant=plant,
scene_graph=scene_graph)
weld_to_world(plant, ceiling,
create_transform(translation=[0.3257, 0, 1.1]))
pose_from_name = read_poses_from_file(dope_path)
model_from_name = {}
for name in pose_from_name:
model_from_name[name] = AddModelFromSdfFile(
file_name=get_sdf_path(name),
model_name=name,
plant=plant,
scene_graph=scene_graph)
station.Finalize()
door_names = [
'left_door',
]
doors = [plant.GetBodyByName(name).index() for name in door_names]
initial_positions = {
plant.GetJointByName('left_door_hinge'): -DOOR_CLOSED,
plant.GetJointByName('right_door_hinge'): DOOR_CLOSED,
}
initial_conf = [0, 0, 0, -1.75, 0, 1.0, 0]
initial_positions.update(
zip(get_movable_joints(plant, robot), initial_conf))
initial_poses = {
model_from_name[name]: pose_from_name[name]
for name in pose_from_name
}
goal_shelf = 'shelf_lower'
print('Goal shelf:', goal_shelf)
goal_surface = Surface(plant, cupboard, 'top_and_bottom',
CUPBOARD_SHELVES.index(goal_shelf))
surfaces = [
goal_surface,
]
item = model_from_name[goal_name]
diagram, state_machine = build_manipulation_station(
station, visualize=is_visualizing, **kwargs)
task = Task(
diagram,
plant,
scene_graph,
robot,
gripper,
movable=[item],
surfaces=surfaces,
doors=doors,
initial_positions=initial_positions,
initial_poses=initial_poses,
#goal_holding=[item],
goal_on=[(item, goal_surface)],
reset_robot=True,
reset_doors=False)
task.set_initial()
task.station = station
return task, diagram, state_machine
def load_station(time_step=0.0, **kwargs):
station = ManipulationStation(time_step, IiwaCollisionModel.kBoxCollision)
plant = station.get_mutable_multibody_plant()
scene_graph = station.get_mutable_scene_graph()
station.AddCupboard()
robot = plant.GetModelInstanceByName('iiwa')
gripper = plant.GetModelInstanceByName('gripper')
table = plant.GetModelInstanceByName('table')
cupboard = plant.GetModelInstanceByName('cupboard')
model_name = 'soup'
item = AddModelFromSdfFile(file_name=get_sdf_path(model_name),
model_name=model_name,
plant=plant,
scene_graph=scene_graph)
ceiling = AddModelFromSdfFile(file_name=PLANE_FILE_PATH,
model_name="ceiling",
plant=plant,
scene_graph=scene_graph)
weld_to_world(plant, ceiling,
create_transform(translation=[0.3257, 0, 1.0]))
station.Finalize()
diagram, state_machine = build_manipulation_station(station, **kwargs)
box_from_geom = get_box_from_geom(scene_graph)
table_body = plant.GetBodyByName('amazon_table', table)
table_index = 0
#table_surface = Surface(plant, table, table_body.name(), table_index),
start_z = get_z_placement(plant, box_from_geom, item, table_body,
table_index)
shelf_body = plant.GetBodyByName('top_and_bottom', cupboard)
shelf_index = CUPBOARD_SHELVES.index('shelf_lower')
goal_surface = Surface(plant, cupboard, shelf_body.name(), shelf_index)
door_names = [
'left_door',
#'right_door',
]
doors = [plant.GetBodyByName(name).index() for name in door_names]
initial_positions = {
plant.GetJointByName('left_door_hinge'): -DOOR_CLOSED,
plant.GetJointByName('right_door_hinge'): DOOR_CLOSED,
}
initial_conf = [0, 0, 0, -1.75, 0, 1.0, 0]
initial_positions.update(
zip(get_movable_joints(plant, robot), initial_conf))
start_x, start_y, start_theta = 0.4, -0.2, np.pi / 2
initial_poses = {
item:
create_transform(translation=[start_x, start_y, start_z],
rotation=[0, 0, start_theta]),
}
surfaces = [
#table_surface,
goal_surface,
]
task = Task(
diagram,
plant,
scene_graph,
robot,
gripper,
movable=[item],
surfaces=surfaces,
doors=doors,
initial_positions=initial_positions,
initial_poses=initial_poses,
#goal_holding=[item],
goal_on=[(item, goal_surface)],
#goal_poses=goal_poses,
reset_robot=True,
reset_doors=False)
task.set_initial()
task.station = station
return task, diagram, state_machine
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")
ConnectDrakeVisualizer(builder, scene_graph,
station.GetOutputPort("pose_bundle"))
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]
q_wsg = [-0.03, 0.03]
plant.SetPositions(context, iiwa, q_iiwa)
plant.SetPositions(context, wsg, q_wsg)
# Build and visualize.
control_builder = DiagramBuilder()
control_plant, control_scene_graph = AddMultibodyPlantSceneGraph(
control_builder, time_step=0.)
if VISUALIZE:
ConnectDrakeVisualizer(control_builder, control_scene_graph)
# N.B. This has the same scene, but with all joints outside of the
# IIWA frozen.
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,
scene_graph_dest=control_scene_graph)
self.assertIsInstance(to_control, mut.MultibodyPlantElementsMap)
control_plant.Finalize()
self.assertEqual(control_plant.num_positions(),
plant.num_positions(iiwa))
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_frames(plant, context, control_plant, control_context,
"iiwa_link_0", "iiwa_link_7")
util.compare_frames(plant, context, control_plant, control_context,
"body", "left_finger")
# Visualize.
if VISUALIZE:
print(" Visualize original plant")
Simulator(diagram, d_context.Clone()).Initialize()
input(" Press enter...")
print(" Visualize control plant")
Simulator(control_diagram, control_d_context.Clone()).Initialize()
input(" Press enter...")
# For grins, ensure that we can copy everything, including world weld.
control_plant_copy = MultibodyPlant(time_step=0.)
subgraph = mut.MultibodyPlantSubgraph(
mut.get_elements_from_plant(control_plant))
subgraph.add_to(control_plant_copy)
control_plant_copy.Finalize()
self.assertEqual(control_plant_copy.num_positions(),
control_plant.num_positions())
util.assert_plant_equals(control_plant, None, control_plant_copy, None)
class ManipulationStationSimulator:
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()
# Initial pose of the object
self.X_WObject = X_WObject
def RunSimulation(self,
plans_list,
gripper_setpoint_list,
extra_time=0,
real_time_rate=1.0,
q0_kuka=np.zeros(7),
is_visualizing=True):
"""
Constructs a Diagram that sends commands to ManipulationStation.
@param plans_list: A list of Plans to be executed.
@param gripper_setpoint_list: A list of gripper setpoints. Each setpoint corresponds to a Plan.
@param extra_time: Time in seconds that the simulation is run,
in addition to the sum of the durations of all plans.
@param real_time_rate: 1.0 means realtime; 0 means as fast as possible.
@param q0_kuka: initial configuration of the robot.
@param is_visualizing: if true, adds MeshcatVisualizer to the Diagram. It should be set to False
when running tests.
@return: logs of robot configuration and MultibodyPlant, generated by simulation.
Logs are SignalLogger systems, whose data can be accessed by SignalLogger.data().
"""
builder = DiagramBuilder()
builder.AddSystem(self.station)
# Add plan runner.
plan_runner = ManipStationPlanRunner(
station=self.station,
kuka_plans=plans_list,
gripper_setpoint_list=gripper_setpoint_list)
builder.AddSystem(plan_runner)
builder.Connect(plan_runner.hand_setpoint_output_port,
self.station.GetInputPort("wsg_position"))
builder.Connect(plan_runner.gripper_force_limit_output_port,
self.station.GetInputPort("wsg_force_limit"))
demux = builder.AddSystem(Demultiplexer(14, 7))
builder.Connect(
plan_runner.GetOutputPort("iiwa_position_and_torque_command"),
demux.get_input_port(0))
builder.Connect(demux.get_output_port(0),
self.station.GetInputPort("iiwa_position"))
builder.Connect(demux.get_output_port(1),
self.station.GetInputPort("iiwa_feedforward_torque"))
builder.Connect(self.station.GetOutputPort("iiwa_position_measured"),
plan_runner.iiwa_position_input_port)
builder.Connect(self.station.GetOutputPort("iiwa_velocity_estimated"),
plan_runner.iiwa_velocity_input_port)
# Add meshcat visualizer
if is_visualizing:
scene_graph = self.station.get_mutable_scene_graph()
viz = MeshcatVisualizer(scene_graph,
is_drawing_contact_force=True,
plant=self.plant)
builder.AddSystem(viz)
builder.Connect(self.station.GetOutputPort("pose_bundle"),
viz.GetInputPort("lcm_visualization"))
builder.Connect(self.station.GetOutputPort("contact_results"),
viz.GetInputPort("contact_results"))
# Add logger
iiwa_position_command_log = LogOutput(demux.get_output_port(0),
builder)
iiwa_position_command_log._DeclarePeriodicPublish(0.005)
iiwa_external_torque_log = LogOutput(
self.station.GetOutputPort("iiwa_torque_external"), builder)
iiwa_external_torque_log._DeclarePeriodicPublish(0.005)
iiwa_position_measured_log = LogOutput(
self.station.GetOutputPort("iiwa_position_measured"), builder)
iiwa_position_measured_log._DeclarePeriodicPublish(0.005)
plant_state_log = LogOutput(
self.station.GetOutputPort("plant_continuous_state"), builder)
plant_state_log._DeclarePeriodicPublish(0.005)
# build diagram
diagram = builder.Build()
if is_visualizing:
viz.load()
time.sleep(2.0)
# RenderSystemWithGraphviz(diagram)
# construct simulator
simulator = Simulator(diagram)
context = diagram.GetMutableSubsystemContext(
self.station, simulator.get_mutable_context())
# set initial state of the robot
self.station.SetIiwaPosition(q0_kuka, context)
self.station.SetIiwaVelocity(np.zeros(7), context)
self.station.SetWsgPosition(0.05, context)
self.station.SetWsgVelocity(0, context)
# set initial hinge angles of the cupboard.
# setting hinge angle to exactly 0 or 90 degrees will result in intermittent contact
# with small contact forces between the door and the cupboard body.
left_hinge_joint = self.plant.GetJointByName("left_door_hinge")
left_hinge_joint.set_angle(
context=self.station.GetMutableSubsystemContext(
self.plant, context),
angle=-0.001)
right_hinge_joint = self.plant.GetJointByName("right_door_hinge")
right_hinge_joint.set_angle(
context=self.station.GetMutableSubsystemContext(
self.plant, context),
angle=0.001)
# set initial pose of the object
if self.object_base_link_name is not None:
self.plant.tree().SetFreeBodyPoseOrThrow(
self.plant.GetBodyByName(self.object_base_link_name,
self.object), self.X_WObject,
self.station.GetMutableSubsystemContext(self.plant, context))
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(real_time_rate)
simulator.Initialize()
sim_duration = 0.
for plan in plans_list:
sim_duration += plan.get_duration() * 1.1
sim_duration += extra_time
print "simulation duration", sim_duration
simulator.StepTo(sim_duration)
return iiwa_position_command_log, \
iiwa_position_measured_log, \
iiwa_external_torque_log, \
plant_state_log
def RunRealRobot(self, plans_list, gripper_setpoint_list):
"""
Constructs a Diagram that sends commands to ManipulationStationHardwareInterface.
@param plans_list: A list of Plans to be executed.
@param gripper_setpoint_list: A list of gripper setpoints. Each setpoint corresponds to a Plan.
@return: logs of robot configuration and torque, decoded from LCM messges sent by the robot's driver.
Logs are SignalLogger systems, whose data can be accessed by SignalLogger.data().
"""
builder = DiagramBuilder()
camera_ids = ["805212060544"]
station_hardware = ManipulationStationHardwareInterface(camera_ids)
station_hardware.Connect(wait_for_cameras=False)
builder.AddSystem(station_hardware)
# Add plan runner
# Add plan runner.
plan_runner = ManipStationPlanRunner(
station=self.station,
kuka_plans=plans_list,
gripper_setpoint_list=gripper_setpoint_list)
builder.AddSystem(plan_runner)
builder.Connect(plan_runner.hand_setpoint_output_port,
station_hardware.GetInputPort("wsg_position"))
builder.Connect(plan_runner.gripper_force_limit_output_port,
station_hardware.GetInputPort("wsg_force_limit"))
demux = builder.AddSystem(Demultiplexer(14, 7))
builder.Connect(
plan_runner.GetOutputPort("iiwa_position_and_torque_command"),
demux.get_input_port(0))
builder.Connect(demux.get_output_port(0),
station_hardware.GetInputPort("iiwa_position"))
builder.Connect(
demux.get_output_port(1),
station_hardware.GetInputPort("iiwa_feedforward_torque"))
builder.Connect(
station_hardware.GetOutputPort("iiwa_position_measured"),
plan_runner.iiwa_position_input_port)
builder.Connect(
station_hardware.GetOutputPort("iiwa_velocity_estimated"),
plan_runner.iiwa_velocity_input_port)
# Add logger
iiwa_position_command_log = LogOutput(demux.get_output_port(0),
builder)
iiwa_position_command_log._DeclarePeriodicPublish(0.005)
iiwa_position_measured_log = LogOutput(
station_hardware.GetOutputPort("iiwa_position_measured"), builder)
iiwa_position_measured_log._DeclarePeriodicPublish(0.005)
iiwa_external_torque_log = LogOutput(
station_hardware.GetOutputPort("iiwa_torque_external"), builder)
iiwa_external_torque_log._DeclarePeriodicPublish(0.005)
# build diagram
diagram = builder.Build()
# RenderSystemWithGraphviz(diagram)
# construct simulator
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
sim_duration = 0.
for plan in plans_list:
sim_duration += plan.get_duration() * 1.1
print "sending trajectories in 2 seconds..."
time.sleep(1.0)
print "sending trajectories in 1 second..."
time.sleep(1.0)
print "sending trajectories now!"
simulator.StepTo(sim_duration)
return iiwa_position_command_log, \
iiwa_position_measured_log, iiwa_external_torque_log
