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 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
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
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
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
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 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()