def add_meshcat_visualizer(scene_graph, builder):
# https://github.com/rdeits/meshcat-python
# https://github.com/RussTedrake/underactuated/blob/master/src/underactuated/meshcat_visualizer.py
from underactuated.meshcat_visualizer import MeshcatVisualizer
viz = MeshcatVisualizer(scene_graph, draw_timestep=0.033333)
builder.AddSystem(viz)
builder.Connect(scene_graph.get_pose_bundle_output_port(),
viz.get_input_port(0))
viz.load()
return viz
def build_manipulation_station(station, plan=None, visualize=False):
from underactuated.meshcat_visualizer import MeshcatVisualizer
builder = DiagramBuilder()
builder.AddSystem(station)
plan_runner = None
if plan is not None:
plan_runner = connect_plan_runner(builder, station, plan)
# Add meshcat visualizer
plant = station.get_mutable_multibody_plant()
scene_graph = station.get_mutable_scene_graph()
if visualize:
viz = MeshcatVisualizer(scene_graph,
is_drawing_contact_force=True,
plant=plant)
builder.AddSystem(viz)
builder.Connect(station.GetOutputPort("pose_bundle"),
viz.GetInputPort("lcm_visualization"))
builder.Connect(station.GetOutputPort("contact_results"),
viz.GetInputPort("contact_results"))
# build diagram
diagram = builder.Build()
if visualize:
viz.load()
#RenderSystemWithGraphviz(diagram)
return diagram, plan_runner
def build_manipulation_station(station):
from underactuated.meshcat_visualizer import MeshcatVisualizer
from .manipulation_station.manipulation_station_plan_runner import ManipStationPlanRunner
builder = DiagramBuilder()
builder.AddSystem(station)
# Add plan runner.
plan_runner = ManipStationPlanRunner(station=station)
builder.AddSystem(plan_runner)
builder.Connect(plan_runner.hand_setpoint_output_port,
station.GetInputPort("wsg_position"))
builder.Connect(plan_runner.gripper_force_limit_output_port,
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),
station.GetInputPort("iiwa_position"))
builder.Connect(demux.get_output_port(1),
station.GetInputPort("iiwa_feedforward_torque"))
builder.Connect(station.GetOutputPort("iiwa_position_measured"),
plan_runner.iiwa_position_input_port)
builder.Connect(station.GetOutputPort("iiwa_velocity_estimated"),
plan_runner.iiwa_velocity_input_port)
# Add meshcat visualizer
plant = station.get_mutable_multibody_plant()
scene_graph = station.get_mutable_scene_graph()
viz = MeshcatVisualizer(scene_graph,
is_drawing_contact_force=True,
plant=plant)
builder.AddSystem(viz)
builder.Connect(station.GetOutputPort("pose_bundle"),
viz.GetInputPort("lcm_visualization"))
builder.Connect(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(
station.GetOutputPort("iiwa_torque_external"), builder)
iiwa_external_torque_log._DeclarePeriodicPublish(0.005)
iiwa_position_measured_log = LogOutput(
station.GetOutputPort("iiwa_position_measured"), builder)
iiwa_position_measured_log._DeclarePeriodicPublish(0.005)
wsg_state_log = LogOutput(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)
return diagram, plan_runner
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 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 build(self, real_time_rate=0, is_visualizing=False):
# Create manipulation station simulator
self.manip_station_sim = ManipulationStationSimulator(
time_step=5e-3,
object_file_path=object_file_path,
object_base_link_name="base_link",
)
# Create plan runner
plan_runner, self.plan_scheduler = CreateManipStationPlanRunnerDiagram(
station=self.manip_station_sim.station,
kuka_plans=[],
gripper_setpoint_list=[],
rl_environment=True)
self.manip_station_sim.plan_runner = plan_runner
# Create builder and add systems
builder = DiagramBuilder()
builder.AddSystem(self.manip_station_sim.station)
builder.AddSystem(plan_runner)
# Connect systems
builder.Connect(
plan_runner.GetOutputPort("gripper_setpoint"),
self.manip_station_sim.station.GetInputPort("wsg_position"))
builder.Connect(
plan_runner.GetOutputPort("force_limit"),
self.manip_station_sim.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.manip_station_sim.station.GetInputPort("iiwa_position"))
builder.Connect(
demux.get_output_port(1),
self.manip_station_sim.station.GetInputPort(
"iiwa_feedforward_torque"))
builder.Connect(
self.manip_station_sim.station.GetOutputPort(
"iiwa_position_measured"),
plan_runner.GetInputPort("iiwa_position"))
builder.Connect(
self.manip_station_sim.station.GetOutputPort(
"iiwa_velocity_estimated"),
plan_runner.GetInputPort("iiwa_velocity"))
# Add meshcat visualizer
if is_visualizing:
scene_graph = self.manip_station_sim.station.get_mutable_scene_graph(
)
viz = MeshcatVisualizer(scene_graph,
is_drawing_contact_force=False,
plant=self.manip_station_sim.plant)
builder.AddSystem(viz)
builder.Connect(
self.manip_station_sim.station.GetOutputPort("pose_bundle"),
viz.GetInputPort("lcm_visualization"))
# Build diagram
self.diagram = builder.Build()
if is_visualizing:
print("Setting up visualizer...")
viz.load()
time.sleep(2.0)
# Construct Simulator
self.simulator = Simulator(self.diagram)
self.manip_station_sim.simulator = self.simulator
self.simulator.set_publish_every_time_step(False)
self.simulator.set_target_realtime_rate(real_time_rate)
self.context = self.diagram.GetMutableSubsystemContext(
self.manip_station_sim.station,
self.simulator.get_mutable_context())
self.left_hinge_joint = self.manip_station_sim.plant.GetJointByName(
"left_door_hinge")
self.right_hinge_joint = self.manip_station_sim.plant.GetJointByName(
"right_door_hinge")
# Goal for training
self.goal_position = np.array([0.85, 0, 0.31])
# Object body
self.obj = self.manip_station_sim.plant.GetBodyByName(
self.manip_station_sim.object_base_link_name,
self.manip_station_sim.object)
# Properties for RL
max_action = np.ones(8) * 0.1
max_action[-1] = 0.03
low_action = -1 * max_action
low_action[-1] = 0
self.action_space = ActionSpace(low=low_action, high=max_action)
self.state_dim = self._getObservation().shape[0]
self._episode_steps = 0
self._max_episode_steps = 75
# Set initial state of the robot
self.reset_sim = False
self.reset()
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