grip = DXL_XL(port_num, 5)
rexarm = Rexarm((base,shld,elbw,wrst,wrst2), grip)
rexarm.set_speeds_normalized_global(0.15)
rexarm.initialize()
rexarm.open_gripper()
tp = TrajectoryPlanner(rexarm)
time.sleep(1)
waypoints = [[1.0,0.8,1.0,1.0,1.0],
[-1.0,-0.8,-1.0,-1.0,-1.0],
[-1.0,0.8,1.0,1.0,1.0],
[1.0,-0.8,-1.0,-1.0,-1.0],
[0.0,0.0,0.0,0.0,0.0]]
for wp in waypoints:
goal_wp = wp
tp.set_initial_wp()
tp.set_final_wp(goal_wp)
tp.go()
rexarm.toggle_gripper()
time.sleep(1)
#rexarm.disable_torque()
dxlbus.close()
tmp_waypoints = [[0.0, 0.0, 0.0, 0.0], [np.pi * 0.1, 0.0, np.pi / 2, 0.0],
[np.pi * 0.25, np.pi / 2, -np.pi / 2, np.pi / 2],
[np.pi * 0.4, np.pi / 2, -np.pi / 2, 0.0],
[np.pi * 0.55, 0, 0, 0], [np.pi * 0.7, 0.0, np.pi / 2, 0.0],
[np.pi * 0.85, np.pi / 2, -np.pi / 2, np.pi / 2],
[np.pi, np.pi / 2, -np.pi / 2, 0.0],
[0.0, np.pi / 2, np.pi / 2, 0.0],
[np.pi / 2, -np.pi / 2, np.pi / 2, 0.0]]
waypoints = []
for wp in tmp_waypoints:
full_wp = [0.0] * rexarm.num_joints
full_wp[0:len(wp)] = wp
waypoints.append(full_wp)
if args.trajectory_planner:
tp = TrajectoryPlanner(rexarm)
for wp in waypoints:
tp.set_initial_wp()
tp.set_final_wp(wp)
tp.go(max_speed=2.5)
time.sleep(1.0)
else:
for wp in waypoints:
rexarm.set_positions(wp)
time.sleep(1)
rexarm.disable_torque()
time.sleep(0.1)