robotcontroller.py

#!/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

class robocontroller:
# The robot object is what you use to control the robot
    def __init__(self):
        self.robot = InterbotixManipulatorXS("px100", "arm", "gripper")

    def __enter__(self):
        robot_startup()
        return self.robot
    
    def __exit__(self, exc_type, exc_value, traceback):
        self.robot.arm.go_to_sleep_pose()
        self.robot.gripper.release()
        robot_shutdown()
    
    def getEECartesianPose(self):

        poseMatrix = self.robot.arm.get_ee_pose()

        #get the fourth column of poseMatrix - done by copilot
        position = poseMatrix[0:3, 3].tolist()

        return position
    
    def goTo(self, position):
        return self.robot.arm.set_ee_pose_components(x=position[0], y=position[1], z=position[2], roll=0.0, pitch=0.0, yaw=0.0)