MSRHackaPenPincher/roboexample.py at main · theoHC/MSRHackaPenPincher · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
#!/usr/bin/env python3
from interbotix_xs_modules.xs_robot.arm import InterbotixManipulatorXS
from interbotix_common_modules.common_robot.robot import robot_shutdown, robot_startup
import numpy as np
# The robot object is what you use to control the robot
robot = InterbotixManipulatorXS("px100", "arm", "gripper")

robot_startup()
mode = 'h'
# Let the user select the position
while mode != 'q':
    mode=input("[h]ome, [s]leep, [q]uit, [g]rasp, [r]elease ")

    if mode == "q":
        break

    robot.arm.set_ee_pose_components(x=0.2, y=0.0, z=0.2, roll=0.0, pitch=0.0, yaw=0.0)

    robot.arm.set_ee_pose_components(x=0.1, y=-0.1, z=0.2, roll=0.0, pitch=0.0, yaw=0.0)

    robot.arm.set_ee_pose_components(x=0.1, y=0.1, z=0.2, roll=0.0, pitch=0.0, yaw=0.0)

    robot.arm.set_ee_pose_components(x=0.1, y=0.2, z=0.1, roll=0.0, pitch=0.0, yaw=0.0)

    robot.arm.set_ee_pose_components(x=0.25, y=0.0, z=0.1, roll=0.0, pitch=0.0, yaw=0.0)

    # if mode == "h":
    #     robot.arm.go_to_home_pose()
    # elif mode == "s":
    #     robot.arm.go_to_sleep_pose()
    # elif mode == "g":
    #     robot.gripper.grasp()
    # elif mode == "r":
    #     robot.gripper.release()
    # elif mode == "+x":
    #     robot.arm.set_ee_cartesian_trajectory(x=0.01)
    # elif mode == "-x":
    #     robot.arm.set_ee_cartesian_trajectory(x=-0.01)
    # elif mode == "rotate left":
    #     robot.arm.set_single_joint_position("waist", np.pi/2.0)
    # elif mode == "rotate right":
    #     robot.arm.set_single_joint_position("waist", -np.pi/2.0)
    # elif mode == "rotate center":
    #     robot.arm.set_single_joint_position("waist", 0.0)
    # elif mode == "mypos":
    #     robot.arm.set_ee_pose_components(x=0.2, y=0.0, z=0.2, roll=0.0, pitch=0.0, yaw=0.0)
    #     pass

robot.arm.go_to_sleep_pose()
robot_shutdown()