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


Example #2
0
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)