def main():
time_step = 2e-3
parser = argparse.ArgumentParser()
parser.add_argument('-c',
'--cfree',
action='store_true',
help='Disables collisions when planning')
parser.add_argument(
'-d',
'--deterministic',
action='store_true',
help='Manually sets the random seeds used by the stream generators')
parser.add_argument('-s',
'--simulate',
action='store_true',
help='Simulates the system')
parser.add_argument('-e',
'--execute',
action='store_true',
help='Executes the system')
parser.add_argument('-l',
'--load',
action='store_true',
help='Loads the last plan')
parser.add_argument('-f',
'--force_control',
action='store_true',
help='Use impedance control to open the door')
parser.add_argument('-p',
'--poses',
default=DOPE_PATH,
help='The path to the dope poses file')
args = parser.parse_args()
if args.deterministic:
random.seed(0)
np.random.seed(0)
goal_name = 'soup'
if args.poses == 'none':
task, diagram, state_machine = load_station(time_step=time_step)
else:
task, diagram, state_machine = load_dope(time_step=time_step,
dope_path=args.poses,
goal_name=goal_name)
print(task)
plant = task.mbp
RenderSystemWithGraphviz(diagram) # Useful for getting port names
task.publish()
context = diagram.GetMutableSubsystemContext(plant, task.diagram_context)
world_frame = plant.world_frame()
tree = plant.tree()
X_WSoup = tree.CalcRelativeTransform(
context,
frame_A=world_frame,
frame_B=plant.GetFrameByName("base_link_soup"))
if not args.load:
replan(task,
context,
visualize=True,
collisions=not args.cfree,
use_impedance=args.force_control)
##################################################
if not args.simulate and not args.execute:
return
manip_station_sim = ManipulationStationSimulator(
time_step=time_step,
object_file_path=get_sdf_path(goal_name),
object_base_link_name="base_link_soup",
X_WObject=X_WSoup)
plan_list = []
gripper_setpoints = []
splines = np.load("splines.npy")
setpoints = np.load("gripper_setpoints.npy")
for control, setpoint in zip(splines, setpoints):
if isinstance(control, ForceControl):
new_plans, new_setpoints = \
GenerateOpenLeftDoorPlansByImpedanceOrPosition("Impedance", is_open_fully=True)
plan_list.extend(new_plans)
gripper_setpoints.extend(new_setpoints)
else:
plan_list.append(control.plan())
gripper_setpoints.append(setpoint)
dump_plans(plan_list, gripper_setpoints)
sim_duration = compute_duration(plan_list)
print('Splines: {}\nDuration: {:.3f} seconds'.format(
len(plan_list), sim_duration))
if args.execute:
raw_input('Execute on hardware?')
iiwa_position_command_log, iiwa_position_measured_log, iiwa_external_torque_log = \
manip_station_sim.RunRealRobot(plan_list, gripper_setpoints)
#PlotExternalTorqueLog(iiwa_external_torque_log)
#PlotIiwaPositionLog(iiwa_position_command_log, iiwa_position_measured_log)
else:
raw_input('Execute in simulation?')
q0 = [0, 0, 0, -1.75, 0, 1.0, 0]
iiwa_position_command_log, iiwa_position_measured_log, iiwa_external_torque_log, \
plant_state_log = \
manip_station_sim.RunSimulation(plan_list, gripper_setpoints,
extra_time=2.0, real_time_rate=1.0, q0_kuka=q0)
time_step=2e-3,
object_file_path=object_file_path,
object_base_link_name="base_link",)
# Generate plans.
plan_list = None
gripper_setpoint_list = None
if args.controller == "Trajectory":
plan_list, gripper_setpoint_list = GenerateOpenLeftDoorPlansByTrajectory()
elif args.controller == "Impedance" or args.controller == "Position":
plan_list, gripper_setpoint_list = GenerateOpenLeftDoorPlansByImpedanceOrPosition(
open_door_method=args.controller, is_open_fully=args.open_fully, handle_angle_start=theta0_hinge_new, handle_position=p_WC_handle_new, is_hardware=args.hardware)
print type(gripper_setpoint_list), gripper_setpoint_list
# Run simulator (simulation or hardware).
if is_hardware:
iiwa_position_command_log, iiwa_position_measured_log, iiwa_external_torque_log = \
manip_station_sim.RunRealRobot(plan_list, gripper_setpoint_list, is_plan_runner_diagram=args.diagram_plan_runner)
PlotExternalTorqueLog(iiwa_external_torque_log)
PlotIiwaPositionLog(iiwa_position_command_log, iiwa_position_measured_log)
else:
q0 = [0, 0, 0, -1.75, 0, 1.0, 0]
iiwa_position_command_log, iiwa_position_measured_log, iiwa_external_torque_log, \
state_log = manip_station_sim.RunSimulation(
plan_list, gripper_setpoint_list, extra_time=2.0, real_time_rate=1.0, q0_kuka=q0,
is_visualizing=not args.no_visualization, left_door_angle=args.left_door_angle, right_door_angle=args.right_door_angle,
is_plan_runner_diagram=args.diagram_plan_runner)
PlotExternalTorqueLog(iiwa_external_torque_log)
PlotIiwaPositionLog(iiwa_position_command_log, iiwa_position_measured_log)
X_WObject_list=[X_WObject, X_WObstacle])
if algo == 'RRT':
# Generate with RRT
plan_list, gripper_setpoint_list = motion_planning.rrt(
star=False, max_iters=max_iter)
elif algo == 'RRTStar':
# Generate with RRT Star
plan_list, gripper_setpoint_list = motion_planning.rrt(
star=True, max_iters=max_iter)
elif algo == 'LazySP':
# Generate with lazySP
plan_list, gripper_setpoint_list = motion_planning.lazy_sp(
max_iters=max_iter)
else:
# Generate with Trajectory
plan_list, gripper_setpoint_list = GeneratePickupBrickPlansByTrajectory(
p_WC_box)
# Simulation
ans = raw_input("Run simulation? ")
iiwa_position_command_log, iiwa_position_measured_log, iiwa_external_torque_log, \
state_log = manip_station_sim.RunSimulation(
plan_list, gripper_setpoint_list, extra_time=0.0, real_time_rate=1.0, q0_kuka=q0)
PlotExternalTorqueLog(iiwa_external_torque_log)
PlotIiwaPositionLog(iiwa_position_command_log, iiwa_position_measured_log)
ans = raw_input("You sure you want to run for real?: y/n ")
if ans == "y":
print("Let's go")
manip_station_sim.RunRealRobot(plan_list, gripper_setpoint_list)