draw_circle.py

import sys
import time
import numpy as np
import stretch_body.robot
import stretch_body.trajectories
stretch_body.trajectories.WAYPOINT_ISCLOSE_ATOL = 0.1


r = stretch_body.robot.Robot()
r.arm.motor.disable_guarded_mode()
r.lift.motor.disable_guarded_mode()
r.startup()
if not r.is_calibrated():
    print('Home the robot')
    sys.exit(1)
arm_init = 0.1
lift_init = 1.0


def setup_robot():
    r.arm.move_to(arm_init)
    r.lift.move_to(lift_init)
    r.push_command()
    r.end_of_arm.move_to('wrist_yaw', 1.5707)
    r.end_of_arm.move_to('stretch_gripper', 100)
    time.sleep(5)
    input('Press enter to close the gripper')
    r.end_of_arm.move_to('stretch_gripper', -100)
    r.base.left_wheel.gains['enable_vel_watchdog'] = 0
    r.base.left_wheel._dirty_gains = 1
    r.base.right_wheel.gains['enable_vel_watchdog'] = 0
    r.base.right_wheel._dirty_gains = 1
    r.push_command()
    time.sleep(3)
    input('Press enter to command base towards whiteboard at 1cm/sec. NOTE: base is blind, you should be ready to press enter again to stop the base')
    r.base.set_velocity(0.01, 0.0)
    r.push_command()
    input('Press enter to stop the base')
    r.base.enable_freewheel_mode()
    r.push_command()


def draw_circle_position_mode(n, time_dt=1.5, diameter_m=0.2):
    t = np.linspace(0, 2*np.pi, n, endpoint=True)
    x = (diameter_m / 2) * np.cos(t) + arm_init
    y = (diameter_m / 2) * np.sin(t) + lift_init
    circle_mat = np.c_[x, y]
    for pt in circle_mat:
        r.arm.move_to(pt[0])
        r.lift.move_to(pt[1])
        r.push_command()
        time.sleep(time_dt)


def draw_circle_trajectory_mode(n, diameter_m=0.2, time_dt=1.5, globalv_m=None, globala_m=None):
    t = np.linspace(0, 2*np.pi, n, endpoint=True)
    x = (diameter_m / 2) * np.cos(t) + arm_init
    y = (diameter_m / 2) * np.sin(t) + lift_init
    circle_mat = np.c_[x, y]
    for i in range(n):
        pt = circle_mat[i]
        pt_t = i * time_dt
        r.arm.trajectory.add(t_s=pt_t, x_m=pt[0], v_m=globalv_m, a_m=globala_m)
        r.lift.trajectory.add(t_s=pt_t, x_m=pt[1], v_m=globalv_m, a_m=globala_m)
    r.follow_trajectory()
    time.sleep(n * time_dt + 0.5)


if __name__ == "__main__":
    yn = input('setup robot? (y/[n]): ')
    if yn == 'y':
        setup_robot()
    # draw_circle_position_mode(10, 1.5)
    draw_circle_trajectory_mode(50, time_dt=0.3)