From 1c2c2f63a1a76063937a10aaf14226c8986c4e11 Mon Sep 17 00:00:00 2001 From: yding25 Date: Tue, 17 Dec 2024 22:47:27 +0800 Subject: [PATCH 01/24] update --- RoboticsToolBox/Bestman_real_xarm6 copy.py | 574 +++++++++++ RoboticsToolBox/Bestman_real_xarm6.py | 407 ++++---- Robotics_API/Bestman_Real_Flexiv.py | 898 ++++++++++++++++++ Robotics_API/Pose.py | 101 ++ Robotics_API/Utils.py | 174 ++++ Robotics_API/__init__.py | 2 + .../Bestman_Real_Flexiv.cpython-312.pyc | Bin 0 -> 43835 bytes .../Bestman_Real_Flexiv.cpython-38.pyc | Bin 0 -> 28893 bytes .../__pycache__/Bestman_flexiv.cpython-38.pyc | Bin 0 -> 18731 bytes .../__pycache__/Bestman_flexiv.cpython-39.pyc | Bin 0 -> 18749 bytes .../Bestman_sim_flexiv.cpython-38.pyc | Bin 0 -> 20037 bytes Robotics_API/__pycache__/Pose.cpython-312.pyc | Bin 0 -> 5528 bytes Robotics_API/__pycache__/Pose.cpython-38.pyc | Bin 0 -> 3494 bytes Robotics_API/__pycache__/Utils.cpython-38.pyc | Bin 0 -> 4765 bytes .../__pycache__/__init__.cpython-312.pyc | Bin 0 -> 219 bytes .../__pycache__/__init__.cpython-38.pyc | Bin 0 -> 212 bytes .../__pycache__/utility.cpython-38.pyc | Bin 0 -> 3971 bytes Robotics_API/__pycache__/utils.cpython-38.pyc | Bin 0 -> 3969 bytes Robotics_API/__pycache__/utils.cpython-39.pyc | Bin 0 -> 3965 bytes Sensor/Camera.py | 240 +++++ Sensor/__init__.py | 3 + Sensor/__pycache__/Camera.cpython-38.pyc | Bin 0 -> 6543 bytes Sensor/__pycache__/__init__.cpython-38.pyc | Bin 0 -> 185 bytes Sensor/__pycache__/camera.cpython-38.pyc | Bin 0 -> 4309 bytes 24 files changed, 2226 insertions(+), 173 deletions(-) create mode 100644 RoboticsToolBox/Bestman_real_xarm6 copy.py create mode 100644 Robotics_API/Bestman_Real_Flexiv.py create mode 100644 Robotics_API/Pose.py create mode 100644 Robotics_API/Utils.py create mode 100644 Robotics_API/__init__.py create mode 100644 Robotics_API/__pycache__/Bestman_Real_Flexiv.cpython-312.pyc create mode 100644 Robotics_API/__pycache__/Bestman_Real_Flexiv.cpython-38.pyc create mode 100644 Robotics_API/__pycache__/Bestman_flexiv.cpython-38.pyc create mode 100644 Robotics_API/__pycache__/Bestman_flexiv.cpython-39.pyc create mode 100644 Robotics_API/__pycache__/Bestman_sim_flexiv.cpython-38.pyc create mode 100644 Robotics_API/__pycache__/Pose.cpython-312.pyc create mode 100644 Robotics_API/__pycache__/Pose.cpython-38.pyc create mode 100644 Robotics_API/__pycache__/Utils.cpython-38.pyc create mode 100644 Robotics_API/__pycache__/__init__.cpython-312.pyc create mode 100644 Robotics_API/__pycache__/__init__.cpython-38.pyc create mode 100644 Robotics_API/__pycache__/utility.cpython-38.pyc create mode 100644 Robotics_API/__pycache__/utils.cpython-38.pyc create mode 100644 Robotics_API/__pycache__/utils.cpython-39.pyc create mode 100755 Sensor/Camera.py create mode 100644 Sensor/__init__.py create mode 100644 Sensor/__pycache__/Camera.cpython-38.pyc create mode 100644 Sensor/__pycache__/__init__.cpython-38.pyc create mode 100644 Sensor/__pycache__/camera.cpython-38.pyc diff --git a/RoboticsToolBox/Bestman_real_xarm6 copy.py b/RoboticsToolBox/Bestman_real_xarm6 copy.py new file mode 100644 index 0000000..a7cd07b --- /dev/null +++ b/RoboticsToolBox/Bestman_real_xarm6 copy.py @@ -0,0 +1,574 @@ +# !/usr/bin/env python +# -*- encoding: utf-8 -*- +""" +# @FileName : Bestman_real_xarm6.py +# @Time : 2024-12-17 22:22:23 +# @Author : Zhaxi & Yan +# @Email : zhaxizhuoma.ayang@gmail.com & yding25@binghamton.edu +# @Description : XXX +# @Usage : XXX +""" + + +import numpy as np +import argparse +import time +import datetime +import ikpy +import serial +import serial.tools.list_ports +from ikpy.chain import Chain +from ikpy.inverse_kinematics import inverse_kinematic_optimization +from scipy.spatial.transform import Rotation as R +import sys +import os +import minimalmodbus as mm +import pyRobotiqGripper + +from xarm.wrapper import XArmAPI +current_dir = os.path.dirname(os.path.abspath(__file__)) + + +class Bestman_Real_Xarm6: + ''' + Functions for initalization + ''' + + def __init__(self, robot_ip, local_ip, frequency): + # Initialize the robot and gripper with the provided IPs and frequency + self.robot = XArmAPI(robot_ip) + local_ip = None + self.mode = self.robot.set_mode(0) # 0: default + self.robot_states = self.robot.set_state(0) + self.first_init_flag = True + self.gripper = True # have gripper by default + self.frequency = frequency + + # # URDF + # urdf_file = os.path.join(current_dir, "../Asset/xarm6_robot.urdf") + # self.robot_chain = Chain.from_urdf_file(urdf_file) + # self.active_joints = [ + # joint for joint in self.robot_chain.links + # if isinstance(joint, ikpy.link.URDFLink) and (joint.joint_type == 'revolute' or joint.joint_type == 'prismatic') + # ] + + ''' + Functions for xarm itself + ''' + def clear_fault(self): + '''Clear fault, and updates the current robot states.''' + self.robot.set_state(0) + + def update_robot_states(self): + '''Updates the current robot states.''' + self.robot.getRobotStates(self.robot_states) + + def set_mode(self, _mode): + ''' + Parameters: + _mode= + 0: position controlmode + 1: servo motionmode + 2: joint teachingmodemode (invalid)3: cartesian teaching + 4: joint velocity control mode + 5: cartesian velocity control mode + 6: joint online trajectory planningmode + 7: cartesian online trajectory planning + + Notes: + https://github.com/xArm-Developer/xArm-Python-SDK/blob/master/doc/api/xarm_api.md#mode + ''' + print("set mode") + self.robot.set_mode(_mode) + + def go_home(self,dist=0): + '''Move arm to initial pose.''' + + self.robot.set_position(x=396.4+dist, y=-5.5,z=360,roll=-90,pitch=-90,yaw=-90,wait=False) + time.sleep(3) + + def pose_to_euler(self, pose): + ''' + Convert robot pose from a list [x, y, z, qw, qx, qy, qz] to [x, y, z] and Euler angles. + + Parameters: + pose: list of 7 floats - [x, y, z, qw, qx, qy, qz] + + Returns: + tuple: (x, y, z, roll, pitch, yaw) where (x, y, z) is the position and (roll, pitch, yaw) are the Euler angles in radians. + ''' + x, y, z, qw, qx, qy, qz = pose + r = R.from_quat([qx, qy, qz, qw]) # Reordering to match scipy's [qx, qy, qz, qw] + roll, pitch, yaw = r.as_euler('xyz', degrees=False) + return [x, y, z, roll, pitch, yaw] + + def euler_to_pose(self, position_euler): + ''' + Convert robot pose from [x, y, z, roll, pitch, yaw] to [x, y, z, qw, qx, qy, qz]. + + Parameters: + position_euler: list of 6 floats - [x, y, z, roll, pitch, yaw] + + Returns: + list: [x, y, z, qw, qx, qy, qz] + ''' + x, y, z, roll, pitch, yaw = position_euler + r = R.from_euler('xyz', [roll, pitch, yaw], degrees=False) + qx, qy, qz, qw = r.as_quat() # Getting [qx, qy, qz, qw] from scipy + return [x, y, z, qw, qx, qy, qz] # Reordering to match [qw, qx, qy, qz] + + def set_pay_load(self,payload_weight, payload_cog): + + self.robot.set_tcp_load(payload_weight, payload_cog) + + + # ---------------------------------------------------------------- + # Functions for others + # ---------------------------------------------------------------- + + def get_joint_bounds(self): + ''' + Retrieves the joint bounds of the robot arm. + + Returns: + list: A list of tuples representing the joint bounds, where each tuple contains the minimum and maximum values for a joint. + ''' + maxbounds = self.robot.info().qMax + minbounds = self.robot.info().qMin + jointbounds = list(zip(maxbounds,minbounds)) + return jointbounds + + + def get_DOF(self): + ''' + Retrieves the degree of freedom (DOF) of the robot arm. + + Returns: + int: The degree of freedom of the robot arm. + ''' + return 6 + + # ---------------------------------------------------------------- + # Functions for joint control + # ---------------------------------------------------------------- + + def print_joint_link_info(self, name): + ''' + Prints the joint and link information of a robot. + + Args: + name (str): 'base' or 'arm' + ''' + if name == 'base': + print("Base joint and link information:") + for i, link in enumerate(self.robot_chain.links[:1]): # Assuming the base is the first link + print(f"Link {i}: {link.name}") + elif name == 'arm': + print("Arm joint and link information:") + for i, link in enumerate(self.robot_chain.links[1:]): # Assuming the arm starts from the second link + print(f"Link {i + 1}: {link.name}") + + def get_joint_idx(self): + ''' + Retrieves the indices of the joints in the robot arm. + + Returns: + list: A list of indices for the joints in the robot arm. + ''' + return list(range(len(self.active_joints))) + + def get_tcp_link(self): + ''' + Retrieves the TCP (Tool Center Point) link of the robot arm. + + Returns: + str: The TCP link of the robot arm. + ''' + return self.robot_chain.links[6].name + + def get_current_joint_angles(self): + ''' + Retrieves the current joint angles of the robot arm. + + Returns: + list: A list of the current joint angles of the robot arm. + ''' + _joint_states = self.robot.get_joint_states(is_radian=True) + _joint_angles = _joint_states[1][0][0:6] + + return _joint_angles + + def get_current_joint_velocities(self): + ''' + Retrieves the current joint velocities of the robot arm. + + Returns: + list: A list of the current joint velocities of the robot arm. + ''' + + _joint_states = self.robot.get_joint_states(is_radian=True) + _joint_velocities = _joint_states[1][1][0:6] + + return _joint_velocities + + + + def move_arm_to_joint_angles(self, joint_angles, target_vel=None, target_acc=None, MAX_VEL=None, MAX_ACC=None, wait_for_finish=None): + ''' + Move arm to a specific set of joint angles, considering physics. + + Args: + Set the servo angle, the API will modify self.last_used_angles value + Note: +     https://github.com/xArm-Developer/xArm-Python-SDK/blob/master/doc/api/xarm_api.md#def-set_servo_angleself-servo_idnone-anglenone-speednone-mvaccnone-mvtimenone-relativefalse-is_radiannone-waitfalse-timeoutnone-radiusnone-kwargs + ''' + + #! Force mode switch + self.robot.set_mode(6) # 6: online joint + self.robot.set_state(0) + + self.robot.set_servo_angle(angle=joint_angles, is_radian=True, speed=0.7, wait=wait_for_finish) # speed in rad/s + + def move_arm_follow_joint_angles(self, target_trajectory, target_vel=None, target_acc=None, MAX_VEL=None, MAX_ACC=None): + ''' + Move arm to a few set of joint angles, considering physics. + + Args: + target_trajectory: A list of desired joint angles (in radians) for each joint of the arm. + target_vel: Optional. A list of target velocities for each joint. + target_acc: Optional. A list of target accelerations for each joint. + MAX_VEL: Optional. A list of maximum velocities for each joint. + MAX_ACC: Optional. A list of maximum accelerations for each joint. + ''' + period = 1.0 / self.frequency + self.update_robot_states() + DOF = len(self.robot_states.q) + + for target_pos in target_trajectory: + # Monitor fault on robot server + if self.robot.isFault(): + raise Exception("Fault occurred on robot server, exiting ...") + + # Send command + self.robot.set_servo_angle(angle=target_pos, is_radian=True, speed=target_vel, wait=True) # speed in rad/s + print(f"Sent joint positions: {target_pos}") + + # Use sleep to control loop period + time.sleep(period) + + # ---------------------------------------------------------------- + # Functions for end effector + # ---------------------------------------------------------------- + + # TODO + def get_current_tcp_speed(self): + ''' + Retrieves the current tcp velocities of the robot arm. + + Returns: + list: A list of the current tcp velocities of the robot arm. + ''' + speed = self.robot.realtime_tcp_speed + return speed + + + def get_current_end_effector_pose(self): + ''' + Retrieves the current pose of the robot arm's end effector. + + This function obtains the position and orientation of the end effector. + + Returns: + pose: the [x, y, z, roll, pitch, yaw] value of tcp in meter and radian + ''' + + _pose = self.robot.get_position(is_radian=True) + pose = _pose[1] + pose[0] = pose[0] / 1000 + pose[1] = pose[1] / 1000 + pose[2] = pose[2] / 1000 + return pose + + + + def move_end_effector_to_tcp_velocity(self, _velocity_setpoint, _duration): + ''' + Move arm's end effector to a target tcp velocity. + + Args: + _velocity_setpoint: mm/s in transition, rad/s in orientation + _duration: function calling loop time + Notes: + https://github.com/xArm-Developer/xArm-Python-SDK/blob/master/doc/api/xarm_api.md#def-vc_set_cartesian_velocityself-speeds-is_radiannone-is_tool_coordfalse-duration-1-kwargs + ''' + + self.robot.set_mode(5) # 5 for cartesian vel + self.robot.set_state(0) + self.robot.vc_set_cartesian_velocity(speeds=[_velocity_setpoint[0], + _velocity_setpoint[1], + _velocity_setpoint[2], + _velocity_setpoint[3], + _velocity_setpoint[4], + _velocity_setpoint[5]], + duration=_duration) + + def move_end_effector_to_goal_pose(self, end_effector_goal_pose, speed=1000, mvacc=50000, wait=False): + ''' + Move arm's end effector to a target position. + + Args: + end_effector_goal_pose (Pose): The desired pose of the end effector (includes both position in mm and euler_orientation), please use radian for orientaiton. + ''' + + self.robot.set_position(x=end_effector_goal_pose[0]*1000 , y=end_effector_goal_pose[1]*1000,z=end_effector_goal_pose[2]*1000, + roll=end_effector_goal_pose[3],pitch=end_effector_goal_pose[4],yaw=end_effector_goal_pose[5], speed=speed, mvacc=mvacc, is_radian=True, wait=wait) + + + + def rotate_end_effector_joint(self, angle): + ''' + Rotate the end effector of the robot arm by a specified angle by joint. + + Args: + angle (float): The desired rotation angle in radians. + ''' + current_joint_angles = self.get_current_joint_angles() + + target_joint_angles = current_joint_angles.copy() + target_joint_angles[6] += angle + target_vel = 1 + self.robot.set_mode(6) # 0: joint control mode; 6: online joint + self.robot.set_state(0) + + self.robot.set_servo_angle(angle=target_joint_angles, is_radian=True, speed=target_vel, wait=False) # speed in rad/s + + # ---------------------------------------------------------------- + # Functions for IK + # ---------------------------------------------------------------- + + def joints_to_cartesian(self, joint_angles): + ''' + Transforms the robot arm's joint angles to its Cartesian coordinates. + + Args: + joint_angles (list): A list of joint angles for the robot arm. + + Returns: + tuple: A tuple containing the Cartesian coordinates (position and orientation) of the robot arm. + ''' + # Validate the number of joint values matches the number of active joints + if len(joint_angles) != len(self.active_joints): + raise ValueError("The number of joint values does not match the number of active joints") + + # Map joint values to the full joint chain + full_joint_angles = np.zeros(len(self.robot_chain.links)) + active_joint_indices = [self.robot_chain.links.index(joint) for joint in self.active_joints] + + for i, joint_value in enumerate(joint_angles): + full_joint_angles[active_joint_indices[i]] = joint_value + + # Calculate the end effector position and orientation + cartesian_matrix = self.robot_chain.forward_kinematics(full_joint_angles) + + # Extract position and orientation + position = cartesian_matrix[:3, 3] + orientation_matrix = cartesian_matrix[:3, :3] + + # Convert rotation matrix to quaternion + r = R.from_matrix(orientation_matrix) + quaternion = r.as_quat() + orientation = [quaternion[3], quaternion[0], quaternion[1], quaternion[2]] + + return position, orientation + + def cartesian_to_joints(self, position, orientation): + ''' + Transforms the robot arm's Cartesian coordinates to its joint angles. + + Args: + position (list): The Cartesian position of the robot arm. + orientation (list): The Cartesian orientation of the robot arm. + + Returns: + list: A list of joint angles corresponding to the given Cartesian coordinates. + ''' + rotation_matrix = R.from_euler('xyz', orientation).as_matrix() + + # Combine rotation matrix and position into a list + target_pose = np.eye(4) + target_pose[:3, :3] = rotation_matrix + target_pose[:3, 3] = position + + initial_joint_angles = [0] * len(self.robot_chain) + + # inverse kinematics calculations and return joint angles + joint_angles = ikpy.inverse_kinematics.inverse_kinematic_optimization( + chain=self.robot_chain, + target_frame=target_pose, + starting_nodes_angles=initial_joint_angles, + orientation_mode='all', + ) + + return joint_angles[1:8] + + + def calculate_IK_error(self, goal_position, goal_orientation): + ''' + Calculate the inverse kinematics (IK) error for performing pick-and-place manipulation of an object using a robot arm. + + Args: + goal_position: The desired goal position for the target object. + goal_orientation: The desired goal orientation for the target object. + ''' + pass + + # ---------------------------------------------------------------- + # Functions for gripper + # ---------------------------------------------------------------- + + def find_gripper_xarm(self): + ''' + Searches for the gripper on available serial ports and returns the port if found. + + Returns: + str: The serial port where the gripper is connected, or None if not found. + ''' + _pos = self.robot.get_gripper_position() + _ver = self.robot.get_gripper_version() + + if _ver is not None and _pos is not None: + print("Have Xarm gripper", _ver) + return True + else: + print("Not found Xarm gripper") + return None + + + def find_gripper_robotiq(self): + """ + Config the parameter via Python SDK + """ + # Baud rate + # Modify the baud rate to 115200, the default is 2000000. + self.robot.set_tgpio_modbus_baudrate(115200) + + # TCP Payload and offset + # Robotiq 2F/85 Gripper + self.robot.set_tcp_load(0.925, [0, 0, 58]) + self.robot.set_tcp_offset([0, 0, 174, 0, 0, 0]) + self.robot.save_conf() + + # Self-Collision Prevention Model + # Robotiq 2F/85 Gripper + self.robot.set_collision_tool_model(4) + + self.robot.robotiq_reset() + self.robot.robotiq_set_activate() #enable the robotiq gripper + + + def get_gripper_position_xarm(self): + ''' + Get the position of the XArm gripper. + ''' + gripper_pos = self.robot.get_gripper_position() + + return gripper_pos[1] + + + def get_gripper_position_robotiq(self, number_of_registers=3): + """ + Reading the status of robotiq gripper + + :param number_of_registers: number of registers, 1/2/3, default is 3 + number_of_registers=1: reading the content of register 0x07D0 + number_of_registers=2: reading the content of register 0x07D0/0x07D1 + number_of_registers=3: reading the content of register 0x07D0/0x07D1/0x07D2 + + Note: + register 0x07D0: Register GRIPPER STATUS + register 0x07D1: Register FAULT STATUS and register POSITION REQUEST ECHO + register 0x07D2: Register POSITION and register CURRENT + :return: tuple((code, robotiq_response)) + code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details. + robotiq_response: See the robotiq documentation + """ + status = self.robot.robotiq_get_status(number_of_registers=number_of_registers) + gripper_width = status[1][-2] + return gripper_width + + + + def gripper_goto_xarm(self, value, speed=5000, force=None): + ''' + Moves the gripper to a specified position with given speed. + + Args: + value (int): Position of the gripper. Integer between 0 and 800. + 0 represents the open position, and 255 represents the closed position. + speed (int): Speed of the gripper movement, between 0 and 8000. + force (int): Not applicable for xarm gripper + + Note: + - 0 means fully closed. + - 800 means fully open. + ''' + self.robot.set_gripper_position(pos=value, speed=speed, wait=False, timeout=1, auto_enable=True) + + + def gripper_goto_robotiq(self, pos, speed=0xFF, force=0xFF, wait=False, timeout=5, **kwargs): + """ + Go to the position with determined speed and force. + + :param pos: position of the gripper. Integer between 0 and 255. 0 being the open position and 255 being the close position. + :param speed: gripper speed between 0 and 255 + :param force: gripper force between 0 and 255 + :param wait: whether to wait for the robotion motion complete, default is True + :param timeout: maximum waiting time(unit: second), default is 5, only available if wait=True + + :return: tuple((code, robotiq_response)) + code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details. + robotiq_response: See the robotiq documentation + """ + return self.robot.robotiq_set_position(pos, speed=speed, force=force, wait=wait, timeout=timeout, **kwargs) + + + def open_gripper_xarm(self): + ''' Opens the gripper to its maximum position with maximum speed and force. ''' + self.gripper_goto(value=850, speed=5000, force=None) + + + def open_gripper_robotiq(self, speed=0xFF, force=0xFF, wait=False, timeout=5, **kwargs): + """ + Open the robotiq gripper + + :param speed: gripper speed between 0 and 255 + :param force: gripper force between 0 and 255 + :param wait: whether to wait for the robotiq motion to complete, default is True + :param timeout: maximum waiting time(unit: second), default is 5, only available if wait=True + + :return: tuple((code, robotiq_response)) + code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details. + robotiq_response: See the robotiq documentation + """ + return self.robot.robotiq_open(speed=speed, force=force, wait=wait, timeout=timeout, **kwargs) + + + def close_gripper_xarm(self): + '''Closes the gripper to its minimum position with maximum speed and force.''' + self.gripper_goto(value=0, speed=5000, force=None) + + + def close_gripper_robotiq(self, speed=0xFF, force=0xFF, wait=False, timeout=5, **kwargs): + """ + Close the robotiq gripper + + :param speed: gripper speed between 0 and 255 + :param force: gripper force between 0 and 255 + :param wait: whether to wait for the robotiq motion to complete, default is True + :param timeout: maximum waiting time(unit: second), default is 3, only available if wait=True + + :return: tuple((code, robotiq_response)) + code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details. + robotiq_response: See the robotiq documentation + """ + return self.robot.robotiq_close(speed=speed, force=force, wait=wait, timeout=timeout, **kwargs) \ No newline at end of file diff --git a/RoboticsToolBox/Bestman_real_xarm6.py b/RoboticsToolBox/Bestman_real_xarm6.py index 12c0872..ba59ed7 100644 --- a/RoboticsToolBox/Bestman_real_xarm6.py +++ b/RoboticsToolBox/Bestman_real_xarm6.py @@ -1,58 +1,77 @@ -import numpy as np -import argparse +# !/usr/bin/env python +# -*- encoding: utf-8 -*- +""" +# @FileName : Bestman_real_xarm6.py +# @Time : 2024-12-17 22:22:23 +# @Author : Zhaxi & Yan +# @Email : zhaxizhuoma.ayang@gmail.com & yding25@binghamton.edu +# @Description : XXX +# @Usage : XXX +""" + + +import os import time -import datetime +import numpy as np +from .Pose import Pose +from scipy.spatial.transform import Rotation as R import ikpy -import serial -import serial.tools.list_ports from ikpy.chain import Chain -from ikpy.inverse_kinematics import inverse_kinematic_optimization -from scipy.spatial.transform import Rotation as R -import sys -import os -import minimalmodbus as mm -import pyRobotiqGripper - +from ikpy.link import URDFLink from xarm.wrapper import XArmAPI -current_dir = os.path.dirname(os.path.abspath(__file__)) class Bestman_Real_Xarm6: - ''' + """ + Class to interface with the xArm robotic arm, optimized for consistency with Flexiv API. + """ + + """ Functions for initalization - ''' + """ def __init__(self, robot_ip, local_ip, frequency): - # Initialize the robot and gripper with the provided IPs and frequency + """ + Initializes the xArm robot and related components. + + Args: + robot_ip (str): The IP address of the xArm robot. + frequency (float): Control frequency for the robot. + """ + # Initialize robot self.robot = XArmAPI(robot_ip) - local_ip = None - self.mode = self.robot.set_mode(0) # 0: default - self.robot_states = self.robot.set_state(0) - self.first_init_flag = True - self.gripper = True # have gripper by default self.frequency = frequency + self.gripper = True # Default gripper is enabled + self.log_file = "command_log.tum" + + # Initialize robot states + self.robot_states = self.robot.set_state(0) + self.mode = self.robot.set_mode(0) # Default mode + self.update_robot_states() + # # URDF # urdf_file = os.path.join(current_dir, "../Asset/xarm6_robot.urdf") # self.robot_chain = Chain.from_urdf_file(urdf_file) # self.active_joints = [ - # joint for joint in self.robot_chain.links + # joint for joint in self.robot_chain.links # if isinstance(joint, ikpy.link.URDFLink) and (joint.joint_type == 'revolute' or joint.joint_type == 'prismatic') # ] - ''' + """ Functions for xarm itself - ''' + """ + def clear_fault(self): - '''Clear fault, and updates the current robot states.''' + """Clear fault, and updates the current robot states.""" self.robot.set_state(0) def update_robot_states(self): - '''Updates the current robot states.''' + """Updates the current robot states.""" self.robot.getRobotStates(self.robot_states) def set_mode(self, _mode): - ''' + """ Parameters: _mode= 0: position controlmode @@ -65,160 +84,180 @@ def set_mode(self, _mode): Notes: https://github.com/xArm-Developer/xArm-Python-SDK/blob/master/doc/api/xarm_api.md#mode - ''' + """ print("set mode") self.robot.set_mode(_mode) - def go_home(self,dist=0): - '''Move arm to initial pose.''' + def go_home(self, dist=0): + """Move arm to initial pose.""" - self.robot.set_position(x=396.4+dist, y=-5.5,z=360,roll=-90,pitch=-90,yaw=-90,wait=False) + self.robot.set_position( + x=396.4 + dist, y=-5.5, z=360, roll=-90, pitch=-90, yaw=-90, wait=False + ) time.sleep(3) def pose_to_euler(self, pose): - ''' + """ Convert robot pose from a list [x, y, z, qw, qx, qy, qz] to [x, y, z] and Euler angles. - + Parameters: pose: list of 7 floats - [x, y, z, qw, qx, qy, qz] - + Returns: tuple: (x, y, z, roll, pitch, yaw) where (x, y, z) is the position and (roll, pitch, yaw) are the Euler angles in radians. - ''' + """ x, y, z, qw, qx, qy, qz = pose - r = R.from_quat([qx, qy, qz, qw]) # Reordering to match scipy's [qx, qy, qz, qw] - roll, pitch, yaw = r.as_euler('xyz', degrees=False) + r = R.from_quat( + [qx, qy, qz, qw] + ) # Reordering to match scipy's [qx, qy, qz, qw] + roll, pitch, yaw = r.as_euler("xyz", degrees=False) return [x, y, z, roll, pitch, yaw] def euler_to_pose(self, position_euler): - ''' + """ Convert robot pose from [x, y, z, roll, pitch, yaw] to [x, y, z, qw, qx, qy, qz]. - + Parameters: position_euler: list of 6 floats - [x, y, z, roll, pitch, yaw] - + Returns: list: [x, y, z, qw, qx, qy, qz] - ''' + """ x, y, z, roll, pitch, yaw = position_euler - r = R.from_euler('xyz', [roll, pitch, yaw], degrees=False) + r = R.from_euler("xyz", [roll, pitch, yaw], degrees=False) qx, qy, qz, qw = r.as_quat() # Getting [qx, qy, qz, qw] from scipy return [x, y, z, qw, qx, qy, qz] # Reordering to match [qw, qx, qy, qz] - - def set_pay_load(self,payload_weight, payload_cog): + + def set_pay_load(self, payload_weight, payload_cog): self.robot.set_tcp_load(payload_weight, payload_cog) - # ---------------------------------------------------------------- # Functions for others # ---------------------------------------------------------------- def get_joint_bounds(self): - ''' + """ Retrieves the joint bounds of the robot arm. Returns: list: A list of tuples representing the joint bounds, where each tuple contains the minimum and maximum values for a joint. - ''' + """ maxbounds = self.robot.info().qMax minbounds = self.robot.info().qMin - jointbounds = list(zip(maxbounds,minbounds)) + jointbounds = list(zip(maxbounds, minbounds)) return jointbounds - def get_DOF(self): - ''' + """ Retrieves the degree of freedom (DOF) of the robot arm. Returns: int: The degree of freedom of the robot arm. - ''' + """ return 6 - + # ---------------------------------------------------------------- # Functions for joint control # ---------------------------------------------------------------- def print_joint_link_info(self, name): - ''' + """ Prints the joint and link information of a robot. Args: name (str): 'base' or 'arm' - ''' - if name == 'base': + """ + if name == "base": print("Base joint and link information:") - for i, link in enumerate(self.robot_chain.links[:1]): # Assuming the base is the first link + for i, link in enumerate( + self.robot_chain.links[:1] + ): # Assuming the base is the first link print(f"Link {i}: {link.name}") - elif name == 'arm': + elif name == "arm": print("Arm joint and link information:") - for i, link in enumerate(self.robot_chain.links[1:]): # Assuming the arm starts from the second link + for i, link in enumerate( + self.robot_chain.links[1:] + ): # Assuming the arm starts from the second link print(f"Link {i + 1}: {link.name}") def get_joint_idx(self): - ''' + """ Retrieves the indices of the joints in the robot arm. Returns: list: A list of indices for the joints in the robot arm. - ''' + """ return list(range(len(self.active_joints))) - + def get_tcp_link(self): - ''' + """ Retrieves the TCP (Tool Center Point) link of the robot arm. Returns: str: The TCP link of the robot arm. - ''' + """ return self.robot_chain.links[6].name def get_current_joint_angles(self): - ''' + """ Retrieves the current joint angles of the robot arm. Returns: list: A list of the current joint angles of the robot arm. - ''' + """ _joint_states = self.robot.get_joint_states(is_radian=True) _joint_angles = _joint_states[1][0][0:6] return _joint_angles - + def get_current_joint_velocities(self): - ''' + """ Retrieves the current joint velocities of the robot arm. Returns: list: A list of the current joint velocities of the robot arm. - ''' + """ _joint_states = self.robot.get_joint_states(is_radian=True) _joint_velocities = _joint_states[1][1][0:6] return _joint_velocities - - - def move_arm_to_joint_angles(self, joint_angles, target_vel=None, target_acc=None, MAX_VEL=None, MAX_ACC=None, wait_for_finish=None): - ''' + def move_arm_to_joint_angles( + self, + joint_angles, + target_vel=None, + target_acc=None, + MAX_VEL=None, + MAX_ACC=None, + wait_for_finish=None, + ): + """ Move arm to a specific set of joint angles, considering physics. Args: Set the servo angle, the API will modify self.last_used_angles value - Note: -     https://github.com/xArm-Developer/xArm-Python-SDK/blob/master/doc/api/xarm_api.md#def-set_servo_angleself-servo_idnone-anglenone-speednone-mvaccnone-mvtimenone-relativefalse-is_radiannone-waitfalse-timeoutnone-radiusnone-kwargs - ''' + Note: https://github.com/xArm-Developer/xArm-Python-SDK/blob/master/doc/api/xarm_api.md#def-set_servo_angleself-servo_idnone-anglenone-speednone-mvaccnone-mvtimenone-relativefalse-is_radiannone-waitfalse-timeoutnone-radiusnone-kwargs + """ #! Force mode switch - self.robot.set_mode(6) # 6: online joint + self.robot.set_mode(6) # 6: online joint self.robot.set_state(0) - self.robot.set_servo_angle(angle=joint_angles, is_radian=True, speed=0.7, wait=wait_for_finish) # speed in rad/s - - def move_arm_follow_joint_angles(self, target_trajectory, target_vel=None, target_acc=None, MAX_VEL=None, MAX_ACC=None): - ''' + self.robot.set_servo_angle( + angle=joint_angles, is_radian=True, speed=0.7, wait=wait_for_finish + ) # speed in rad/s + + def move_arm_follow_joint_angles( + self, + target_trajectory, + target_vel=None, + target_acc=None, + MAX_VEL=None, + MAX_ACC=None, + ): + """ Move arm to a few set of joint angles, considering physics. Args: @@ -227,7 +266,7 @@ def move_arm_follow_joint_angles(self, target_trajectory, target_vel=None, targe target_acc: Optional. A list of target accelerations for each joint. MAX_VEL: Optional. A list of maximum velocities for each joint. MAX_ACC: Optional. A list of maximum accelerations for each joint. - ''' + """ period = 1.0 / self.frequency self.update_robot_states() DOF = len(self.robot_states.q) @@ -238,7 +277,9 @@ def move_arm_follow_joint_angles(self, target_trajectory, target_vel=None, targe raise Exception("Fault occurred on robot server, exiting ...") # Send command - self.robot.set_servo_angle(angle=target_pos, is_radian=True, speed=target_vel, wait=True) # speed in rad/s + self.robot.set_servo_angle( + angle=target_pos, is_radian=True, speed=target_vel, wait=True + ) # speed in rad/s print(f"Sent joint positions: {target_pos}") # Use sleep to control loop period @@ -250,25 +291,24 @@ def move_arm_follow_joint_angles(self, target_trajectory, target_vel=None, targe # TODO def get_current_tcp_speed(self): - ''' + """ Retrieves the current tcp velocities of the robot arm. Returns: list: A list of the current tcp velocities of the robot arm. - ''' + """ speed = self.robot.realtime_tcp_speed return speed - - def get_current_end_effector_pose(self): - ''' + def get_current_eef_pose(self): + """ Retrieves the current pose of the robot arm's end effector. This function obtains the position and orientation of the end effector. Returns: pose: the [x, y, z, roll, pitch, yaw] value of tcp in meter and radian - ''' + """ _pose = self.robot.get_position(is_radian=True) pose = _pose[1] @@ -277,10 +317,8 @@ def get_current_end_effector_pose(self): pose[2] = pose[2] / 1000 return pose - - - def move_end_effector_to_tcp_velocity(self, _velocity_setpoint, _duration): - ''' + def move_eef_to_tcp_velocity(self, _velocity_setpoint, _duration): + """ Move arm's end effector to a target tcp velocity. Args: @@ -288,54 +326,70 @@ def move_end_effector_to_tcp_velocity(self, _velocity_setpoint, _duration): _duration: function calling loop time Notes: https://github.com/xArm-Developer/xArm-Python-SDK/blob/master/doc/api/xarm_api.md#def-vc_set_cartesian_velocityself-speeds-is_radiannone-is_tool_coordfalse-duration-1-kwargs - ''' + """ - self.robot.set_mode(5) # 5 for cartesian vel + self.robot.set_mode(5) # 5 for cartesian vel self.robot.set_state(0) - self.robot.vc_set_cartesian_velocity(speeds=[_velocity_setpoint[0], - _velocity_setpoint[1], - _velocity_setpoint[2], - _velocity_setpoint[3], - _velocity_setpoint[4], - _velocity_setpoint[5]], - duration=_duration) - - def move_end_effector_to_goal_pose(self, end_effector_goal_pose, speed=1000, mvacc=50000, wait=False): - ''' + self.robot.vc_set_cartesian_velocity( + speeds=[ + _velocity_setpoint[0], + _velocity_setpoint[1], + _velocity_setpoint[2], + _velocity_setpoint[3], + _velocity_setpoint[4], + _velocity_setpoint[5], + ], + duration=_duration, + ) + + def move_eef_to_goal_pose( + self, goal_pose, speed=1000, mvacc=50000, wait=False + ): + """ Move arm's end effector to a target position. Args: - end_effector_goal_pose (Pose): The desired pose of the end effector (includes both position in mm and euler_orientation), please use radian for orientaiton. - ''' - - self.robot.set_position(x=end_effector_goal_pose[0]*1000 , y=end_effector_goal_pose[1]*1000,z=end_effector_goal_pose[2]*1000, - roll=end_effector_goal_pose[3],pitch=end_effector_goal_pose[4],yaw=end_effector_goal_pose[5], speed=speed, mvacc=mvacc, is_radian=True, wait=wait) - + goal_pose (Pose): The desired pose of the end effector (includes both position in mm and euler_orientation), please use radian for orientaiton. + """ + self.robot.set_position( + x=goal_pose[0] * 1000, + y=goal_pose[1] * 1000, + z=goal_pose[2] * 1000, + roll=goal_pose[3], + pitch=goal_pose[4], + yaw=goal_pose[5], + speed=speed, + mvacc=mvacc, + is_radian=True, + wait=wait, + ) - def rotate_end_effector_joint(self, angle): - ''' + def rotate_eef_joint(self, angle): + """ Rotate the end effector of the robot arm by a specified angle by joint. Args: angle (float): The desired rotation angle in radians. - ''' + """ current_joint_angles = self.get_current_joint_angles() - + target_joint_angles = current_joint_angles.copy() - target_joint_angles[6] += angle + target_joint_angles[6] += angle target_vel = 1 - self.robot.set_mode(6) # 0: joint control mode; 6: online joint + self.robot.set_mode(6) # 0: joint control mode; 6: online joint self.robot.set_state(0) - self.robot.set_servo_angle(angle=target_joint_angles, is_radian=True, speed=target_vel, wait=False) # speed in rad/s + self.robot.set_servo_angle( + angle=target_joint_angles, is_radian=True, speed=target_vel, wait=False + ) # speed in rad/s # ---------------------------------------------------------------- # Functions for IK # ---------------------------------------------------------------- def joints_to_cartesian(self, joint_angles): - ''' + """ Transforms the robot arm's joint angles to its Cartesian coordinates. Args: @@ -343,14 +397,18 @@ def joints_to_cartesian(self, joint_angles): Returns: tuple: A tuple containing the Cartesian coordinates (position and orientation) of the robot arm. - ''' + """ # Validate the number of joint values matches the number of active joints if len(joint_angles) != len(self.active_joints): - raise ValueError("The number of joint values does not match the number of active joints") - + raise ValueError( + "The number of joint values does not match the number of active joints" + ) + # Map joint values to the full joint chain full_joint_angles = np.zeros(len(self.robot_chain.links)) - active_joint_indices = [self.robot_chain.links.index(joint) for joint in self.active_joints] + active_joint_indices = [ + self.robot_chain.links.index(joint) for joint in self.active_joints + ] for i, joint_value in enumerate(joint_angles): full_joint_angles[active_joint_indices[i]] = joint_value @@ -368,9 +426,9 @@ def joints_to_cartesian(self, joint_angles): orientation = [quaternion[3], quaternion[0], quaternion[1], quaternion[2]] return position, orientation - + def cartesian_to_joints(self, position, orientation): - ''' + """ Transforms the robot arm's Cartesian coordinates to its joint angles. Args: @@ -379,8 +437,8 @@ def cartesian_to_joints(self, position, orientation): Returns: list: A list of joint angles corresponding to the given Cartesian coordinates. - ''' - rotation_matrix = R.from_euler('xyz', orientation).as_matrix() + """ + rotation_matrix = R.from_euler("xyz", orientation).as_matrix() # Combine rotation matrix and position into a list target_pose = np.eye(4) @@ -391,36 +449,35 @@ def cartesian_to_joints(self, position, orientation): # inverse kinematics calculations and return joint angles joint_angles = ikpy.inverse_kinematics.inverse_kinematic_optimization( - chain=self.robot_chain, - target_frame=target_pose, - starting_nodes_angles=initial_joint_angles, - orientation_mode='all', + chain=self.robot_chain, + target_frame=target_pose, + starting_nodes_angles=initial_joint_angles, + orientation_mode="all", ) return joint_angles[1:8] - def calculate_IK_error(self, goal_position, goal_orientation): - ''' + """ Calculate the inverse kinematics (IK) error for performing pick-and-place manipulation of an object using a robot arm. Args: goal_position: The desired goal position for the target object. goal_orientation: The desired goal orientation for the target object. - ''' + """ pass # ---------------------------------------------------------------- # Functions for gripper # ---------------------------------------------------------------- - + def find_gripper_xarm(self): - ''' + """ Searches for the gripper on available serial ports and returns the port if found. Returns: str: The serial port where the gripper is connected, or None if not found. - ''' + """ _pos = self.robot.get_gripper_position() _ver = self.robot.get_gripper_version() @@ -430,7 +487,6 @@ def find_gripper_xarm(self): else: print("Not found Xarm gripper") return None - def find_gripper_robotiq(self): """ @@ -438,7 +494,7 @@ def find_gripper_robotiq(self): """ # Baud rate # Modify the baud rate to 115200, the default is 2000000. - self.robot.set_tgpio_modbus_baudrate(115200) + self.robot.set_tgpio_modbus_baudrate(115200) # TCP Payload and offset # Robotiq 2F/85 Gripper @@ -451,28 +507,26 @@ def find_gripper_robotiq(self): self.robot.set_collision_tool_model(4) self.robot.robotiq_reset() - self.robot.robotiq_set_activate() #enable the robotiq gripper - + self.robot.robotiq_set_activate() # enable the robotiq gripper def get_gripper_position_xarm(self): - ''' + """ Get the position of the XArm gripper. - ''' + """ gripper_pos = self.robot.get_gripper_position() return gripper_pos[1] - def get_gripper_position_robotiq(self, number_of_registers=3): """ Reading the status of robotiq gripper - + :param number_of_registers: number of registers, 1/2/3, default is 3 number_of_registers=1: reading the content of register 0x07D0 number_of_registers=2: reading the content of register 0x07D0/0x07D1 number_of_registers=3: reading the content of register 0x07D0/0x07D1/0x07D2 - - Note: + + Note: register 0x07D0: Register GRIPPER STATUS register 0x07D1: Register FAULT STATUS and register POSITION REQUEST ECHO register 0x07D2: Register POSITION and register CURRENT @@ -483,11 +537,9 @@ def get_gripper_position_robotiq(self, number_of_registers=3): status = self.robot.robotiq_get_status(number_of_registers=number_of_registers) gripper_width = status[1][-2] return gripper_width - - def gripper_goto_xarm(self, value, speed=5000, force=None): - ''' + """ Moves the gripper to a specified position with given speed. Args: @@ -495,68 +547,77 @@ def gripper_goto_xarm(self, value, speed=5000, force=None): 0 represents the open position, and 255 represents the closed position. speed (int): Speed of the gripper movement, between 0 and 8000. force (int): Not applicable for xarm gripper - + Note: - 0 means fully closed. - 800 means fully open. - ''' - self.robot.set_gripper_position(pos=value, speed=speed, wait=False, timeout=1, auto_enable=True) - + """ + self.robot.set_gripper_position( + pos=value, speed=speed, wait=False, timeout=1, auto_enable=True + ) - def gripper_goto_robotiq(self, pos, speed=0xFF, force=0xFF, wait=False, timeout=5, **kwargs): + def gripper_goto_robotiq( + self, pos, speed=0xFF, force=0xFF, wait=False, timeout=5, **kwargs + ): """ Go to the position with determined speed and force. - + :param pos: position of the gripper. Integer between 0 and 255. 0 being the open position and 255 being the close position. :param speed: gripper speed between 0 and 255 :param force: gripper force between 0 and 255 :param wait: whether to wait for the robotion motion complete, default is True :param timeout: maximum waiting time(unit: second), default is 5, only available if wait=True - + :return: tuple((code, robotiq_response)) code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details. - robotiq_response: See the robotiq documentation + robotiq_response: See the robotiq documentation """ - return self.robot.robotiq_set_position(pos, speed=speed, force=force, wait=wait, timeout=timeout, **kwargs) - + return self.robot.robotiq_set_position( + pos, speed=speed, force=force, wait=wait, timeout=timeout, **kwargs + ) def open_gripper_xarm(self): - ''' Opens the gripper to its maximum position with maximum speed and force. ''' + """Opens the gripper to its maximum position with maximum speed and force.""" self.gripper_goto(value=850, speed=5000, force=None) - - def open_gripper_robotiq(self, speed=0xFF, force=0xFF, wait=False, timeout=5, **kwargs): + def open_gripper_robotiq( + self, speed=0xFF, force=0xFF, wait=False, timeout=5, **kwargs + ): """ Open the robotiq gripper - + :param speed: gripper speed between 0 and 255 :param force: gripper force between 0 and 255 :param wait: whether to wait for the robotiq motion to complete, default is True :param timeout: maximum waiting time(unit: second), default is 5, only available if wait=True - + :return: tuple((code, robotiq_response)) code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details. - robotiq_response: See the robotiq documentation + robotiq_response: See the robotiq documentation """ - return self.robot.robotiq_open(speed=speed, force=force, wait=wait, timeout=timeout, **kwargs) - + return self.robot.robotiq_open( + speed=speed, force=force, wait=wait, timeout=timeout, **kwargs + ) def close_gripper_xarm(self): - '''Closes the gripper to its minimum position with maximum speed and force.''' + """Closes the gripper to its minimum position with maximum speed and force.""" self.gripper_goto(value=0, speed=5000, force=None) - - def close_gripper_robotiq(self, speed=0xFF, force=0xFF, wait=False, timeout=5, **kwargs): + def close_gripper_robotiq( + self, speed=0xFF, force=0xFF, wait=False, timeout=5, **kwargs + ): """ Close the robotiq gripper - + :param speed: gripper speed between 0 and 255 :param force: gripper force between 0 and 255 :param wait: whether to wait for the robotiq motion to complete, default is True :param timeout: maximum waiting time(unit: second), default is 3, only available if wait=True - + :return: tuple((code, robotiq_response)) code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details. robotiq_response: See the robotiq documentation """ - return self.robot.robotiq_close(speed=speed, force=force, wait=wait, timeout=timeout, **kwargs) \ No newline at end of file + return self.robot.robotiq_close( + speed=speed, force=force, wait=wait, timeout=timeout, **kwargs + ) diff --git a/Robotics_API/Bestman_Real_Flexiv.py b/Robotics_API/Bestman_Real_Flexiv.py new file mode 100644 index 0000000..9b89b6a --- /dev/null +++ b/Robotics_API/Bestman_Real_Flexiv.py @@ -0,0 +1,898 @@ +# !/usr/bin/env python +# -*- encoding: utf-8 -*- +""" +# @FileName : Bestman_Real_Flexiv.py +# @Time : 2024-12-01 12:46:52 +# @Author : Yan +# @Email : yding25@binghamton.edu +# @Description : A basic class for Flexiv robotic arm +# @Usage : None +""" + + +import os +import time +import numpy as np +from .Pose import Pose +from scipy.spatial.transform import Rotation as R +import ikpy +from ikpy.chain import Chain +from ikpy.link import URDFLink +import serial +import minimalmodbus as mm +import pyRobotiqGripper +import flexivrdk +import rospy + + +class Bestman_Real_Flexiv: + """ + A class to interface with the Flexiv robotic arm. + + Attributes: + robot: The Flexiv robot object initialized with provided IP addresses. + gripper: The gripper object for controlling the robo's end effector. + frequency: The control frequency for the robot. + log: A logger object to log messages for debugging and monitoring. + mode: A reference to the Flexiv operation modes. + robot_states: A container for storing the current state of the robot. + robot_chain: A kinematic chain for the robot, parsed from the URDF file. + active_joints: A list of active (revolute or prismatic) joints in the robot. + log_file: The file path for logging robot commands. + """ + + def __init__(self, robot_ip, local_ip, frequency): + """ + Initializes the Flexiv robot and related components. + + Args: + robot_ip (str): The IP address of the Flexiv robot. + local_ip (str): The local IP address for communicating with the robot. + frequency (float): Control frequency for the robot. + """ + # Robot initialization + self.robot = flexivrdk.Robot(robot_ip, local_ip) + self.gripper = None + self.frequency = frequency + self.log = flexivrdk.Log() + self.mode = flexivrdk.Mode + self.robot_states = flexivrdk.RobotStates() + + # Parse URDF for kinematic chain + current_dir = os.path.dirname(os.path.abspath(__file__)) + urdf_file = os.path.join(current_dir, "../Asset/flexiv_rizon4_kinematics.urdf") + if not os.path.exists(urdf_file): + raise FileNotFoundError(f"URDF file not found: {urdf_file}") + # Parse the URDF file and configure the chain + self.robot_chain = Chain.from_urdf_file(urdf_file) + active_links_mask = ( + [False] + [True] * 7 + [False] + ) # Base + 7 DOF + End Effector + self.robot_chain.active_links_mask = active_links_mask + # Filter active joints (revolute or prismatic) + self.active_joints = [ + joint + for joint in self.robot_chain.links + if isinstance(joint, URDFLink) + and (joint.joint_type == "revolute" or joint.joint_type == "prismatic") + ] + # Validate the number of active joints + if len(self.active_joints) != 7: # Flexiv is a 7-DOF robot + raise ValueError( + f"Expected 7 active joints, but found {len(self.active_joints)}. " + f"Check the URDF file and active_links_mask." + ) + # Log successful initialization + self.log.info("Flexiv robot kinematic chain successfully initialized.") + + # Additional initializations can go here + self.log_file = "command_log.tum" + + # ---------------------------------------------------------------- + # Device Initialization and State Management + # ---------------------------------------------------------------- + + def initialize_robot(self): + """ + Initializes the robot by clearing faults and enabling it. + + This function ensures the robot is operational by: + 1. Clearing any existing faults. + 2. Enabling the robot. + 3. Waiting until the robot becomes operational. + + Returns: + bool: True if the robot is successfully initialized, False otherwise. + """ + try: + # Clear faults + if self.robot.isFault(): + self.log.warn("Fault occurred on robot server, attempting to clear...") + self.robot.clearFault() + time.sleep(1) + if self.robot.isFault(): + self.log.error("Fault cannot be cleared, exiting...") + return False + self.log.info("Fault cleared successfully.") + + # Enable the robot + self.log.info("Enabling the robot...") + self.robot.enable() + + # Wait for the robot to become operational + for seconds_waited in range(10): + if self.robot.isOperational(): + self.log.info("Robot is now operational.") + return True + time.sleep(1) + + self.log.warn("Robot is not operational after 10 seconds.") + return False + + except Exception as e: + self.log.error(f"Failed to initialize the robot: {str(e)}") + return False + + def clear_fault(self): + """ + Clears any faults on the robot server and ensures the robot is operational. + """ + if self.robot.isFault(): + self.log.warn("Fault detected. Attempting to clear...") + self.robot.clearFault() + time.sleep(2) + if self.robot.isFault(): + self.log.error("Fault could not be cleared.") + return + self.log.info("Fault cleared successfully.") + + self.log.info("Enabling the robot...") + self.robot.enable() + + for seconds_waited in range(10): + if self.robot.isOperational(): + self.log.info("Robot is now operational.") + return + time.sleep(1) + + self.log.warn("Robot is not operational after 10 seconds.") + + def update_robot_states(self): + """ + Updates the current robot states by fetching them from the robot. + """ + self.robot.getRobotStates(self.robot_states) + + def go_home(self): + """ + Moves the robot arm to its initial (home) pose. + """ + self.robot.setMode(self.mode.NRT_PRIMITIVE_EXECUTION) + self.robot.executePrimitive("Home()") + + # ---------------------------------------------------------------- + # Logging + # ---------------------------------------------------------------- + + def log_command(self, timestamp, target_pos): + """ + Records a command with timestamp and target position. + + Args: + timestamp (float): The time of the command. + target_pos (list): Target joint positions or end effector pose. + """ + try: + with open(self.log_file, "a") as file: + file.write(f"{timestamp} {' '.join(map(str, target_pos))}\n") + self.log.info(f"Command logged: {timestamp}, {target_pos}") + except Exception as e: + self.log.error(f"Failed to log command: {str(e)}") + + # ---------------------------------------------------------------- + # Arm Functions + # ---------------------------------------------------------------- + + def get_joint_bounds(self): + """ + Retrieves the joint limits of the robot arm. + + Returns: + list: A list of tuples, where each tuple contains the minimum and maximum value for a joint. + """ + max_bounds = self.robot.info().qMax + min_bounds = self.robot.info().qMin + return list(zip(min_bounds, max_bounds)) + + def print_arm_jointInfo(self): + """ + Prints the joint and link information of the robot's arm. + + Returns: + None + """ + print("Arm joint and link information:") + for i, link in enumerate( + self.robot_chain.links[1:] + ): # Assuming arm starts at the second link + print(f"Link {i + 1}: {link.name}") + self.log.info(f"Link {i + 1}: {link.name}") + + def get_arm_id(self): + """ + Retrieves the serial ID of the robot arm. + + Returns: + str: The serial number of the robot arm. + """ + try: + arm_id = self.robot.info().serialNum + self.log.info(f"Retrieved robot arm ID: {arm_id}") + return arm_id + except Exception as e: + self.log.error(f"Failed to get robot arm ID: {str(e)}") + return None + + def get_DOF(self): + """ + Retrieves the degree of freedom (DOF) of the robot arm. + + Returns: + int: The number of degrees of freedom. + """ + try: + self.update_robot_states() + dof = len(self.robot_states.q) + self.log.info(f"Robot DOF: {dof}") + return dof + except Exception as e: + self.log.error(f"Failed to retrieve DOF: {str(e)}") + return 0 + + def get_arm_all_joint_idx(self): + """ + Retrieves the indices of all active joints in the robot arm. + + Returns: + list[int]: A list of indices for the active joints. + """ + joint_indices = list(range(len(self.active_joints))) + self.log.info(f"Retrieved joint indices: {joint_indices}") + return joint_indices + + def get_tcp_link(self): + """ + Retrieves the name of the TCP (Tool Center Point) link. + + Returns: + str: Name of the TCP link. + """ + try: + tcp_link_name = self.robot_chain.links[7].name # Assuming TCP is at index 7 + self.log.info(f"TCP link retrieved: {tcp_link_name}") + return tcp_link_name + except IndexError as e: + self.log.error(f"Failed to retrieve TCP link: {str(e)}") + return None + + def get_current_joint_values(self): + """ + Retrieves the current joint angles of the robot arm. + + Returns: + list[float]: A list of current joint angles (in radians). + """ + try: + self.update_robot_states() + joint_values = self.robot_states.q + self.log.info(f"Current joint values: {joint_values}") + return joint_values + except Exception as e: + self.log.error(f"Failed to retrieve current joint values: {str(e)}") + return [] + + def get_current_eef_pose(self): + """ + Retrieves the current pose of the robot arm's end effector. + + Returns: + Pose: A Pose object representing the end effector's position and orientation. + """ + try: + self.update_robot_states() + tcp_pose = self.robot_states.tcpPose # [x, y, z, qw, qx, qy, qz] + position = tcp_pose[:3] + orientation = tcp_pose[3:] # [qw, qx, qy, qz] + pose = Pose(position, orientation) + self.log.info(f"Current end effector pose: {pose}") + return pose + except Exception as e: + self.log.error(f"Failed to retrieve end effector pose: {str(e)}") + return None + + def print_robot_info(self): + """ + Print Flexiv robot information to confirm available attributes. + """ + try: + info = self.robot.info() + print("Robot Info Attributes:") + for attr in dir(info): + if not attr.startswith("_"): + print(f"{attr}: {getattr(info, attr)}") + except Exception as e: + self.log.error(f"Failed to retrieve robot information: {str(e)}") + + def move_arm_to_joint_values( + self, joint_values, target_vel=None, target_acc=None, MAX_VEL=None, MAX_ACC=None + ): + """ + Moves the robot arm to the specified joint angles with optional velocity and acceleration. + + Args: + joint_values (list[float]): Desired joint angles (in radians) for each joint. + target_vel (list[float], optional): Target velocities for each joint. Defaults to None. + target_acc (list[float], optional): Target accelerations for each joint. Defaults to None. + MAX_VEL (list[float], optional): Maximum velocities for each joint. Defaults to dqMax from RobotInfo. + MAX_ACC (list[float], optional): Maximum accelerations for each joint. Defaults to [5.0] rad/s². + + Returns: + None + """ + try: + # Set control mode + self.robot.setMode(self.mode.NRT_JOINT_POSITION) + self.update_robot_states() + DOF = len(self.robot_states.q) + + # Get limits from RobotInfo + alpha = 0.1 # Key coefficient to regulate the maximum speed, with a range of [0.0001, 1] + robot_info = self.robot.info() + default_max_vel = [vel * alpha for vel in robot_info.dqMax] + default_max_acc = [ + 5.0 + ] * DOF # Assuming 5 rad/s² as default maximum acceleration + + # Set default values + MAX_VEL = MAX_VEL or default_max_vel + MAX_ACC = MAX_ACC or default_max_acc + target_vel = target_vel if target_vel is not None else [0.0] * DOF + target_acc = target_acc if target_acc is not None else [0.0] * DOF + + # Validate joint values within limits + joint_limits = zip(robot_info.qMin, robot_info.qMax) + for i, (q, (q_min, q_max)) in enumerate(zip(joint_values, joint_limits)): + if not (q_min <= q <= q_max): + raise ValueError( + f"Joint {i + 1} value {q} out of range [{q_min}, {q_max}]" + ) + + # Send joint command + self.robot.sendJointPosition( + joint_values, target_vel, target_acc, MAX_VEL, MAX_ACC + ) + self.log.info(f"Moved arm to joint values: {joint_values}") + except Exception as e: + self.log.error(f"Failed to move arm to joint values: {str(e)}") + + # ---------------------------------------------------------------- + # End Effector (EEF) Functions + # ---------------------------------------------------------------- + + def move_eef_to_goal_pose(self, goal_pose, max_linear_vel=0.1, max_angular_vel=0.5): + """ + Moves the end effector to the specified pose. + + Args: + goal_pose (Pose): The desired pose (position and orientation in quaternion format). + max_linear_vel (float): Maximum linear velocity. Defaults to 0.1 m/s. + max_angular_vel (float): Maximum angular velocity. Defaults to 0.5 rad/s. + + Returns: + None + """ + try: + if not isinstance(goal_pose, Pose): + raise TypeError("pose must be an instance of Pose") + + wrench = [0.0] * 6 # No wrench force + position = goal_pose.get_position() + orientation = goal_pose.get_orientation(type="quaternion") + pose = position + orientation + + self.robot.setMode(self.mode.NRT_CARTESIAN_MOTION_FORCE) + self.robot.sendCartesianMotionForce( + pose, wrench, max_linear_vel, max_angular_vel + ) + self.log.info(f"Moved end effector to pose: {pose}") + except Exception as e: + self.log.error(f"Failed to move end effector to goal pose: {str(e)}") + + def move_eef_to_goal_pose_wrench( + self, + goal_pose, + wrench, + max_linear_vel=0.1, + max_angular_vel=0.5, + contact_wrench=None, + ): + """ + Moves the end effector to a target pose with a specified wrench control. + + Args: + goal_pose (Pose): Desired pose of the end effector (position and quaternion orientation). + wrench (list[float]): Target TCP wrench [fx, fy, fz, mx, my, mz] in the force control reference frame. + max_linear_vel (float): Maximum linear velocity. Defaults to 0.1 m/s. + max_angular_vel (float): Maximum angular velocity. Defaults to 0.5 rad/s. + contact_wrench (list[float], optional): Maximum contact wrench for collision detection. + + Returns: + None + """ + try: + if not isinstance(goal_pose, Pose): + raise TypeError("goal_pose must be an instance of Pose") + + position = goal_pose.get_position() + orientation = goal_pose.get_orientation(type="quaternion") + pose = position + orientation # Combine position and orientation + + self.robot.setMode(self.mode.NRT_CARTESIAN_MOTION_FORCE) + + if contact_wrench: + self.robot.setMaxContactWrench(contact_wrench) + self.log.info(f"Set contact wrench limit: {contact_wrench}") + + self.robot.sendCartesianMotionForce( + pose, wrench, max_linear_vel, max_angular_vel + ) + self.log.info(f"Moved EEF to pose: {goal_pose} with wrench: {wrench}") + except Exception as e: + self.log.error(f"Failed to move EEF to goal pose with wrench: {str(e)}") + + def rotate_eef_tcp(self, axis, angle): + """ + Rotates the end effector TCP around a specified axis by a given angle. + + Args: + axis (str): Axis to rotate around ('x', 'y', or 'z'). + angle (float): Rotation angle in radians. + + Returns: + None + """ + # TODO + try: + current_pose = self.get_current_eef_pose() + position = current_pose.get_position() + quaternion = current_pose.get_orientation(type="quaternion") + + rotation = R.from_quat(quaternion) + if axis == "x": + rotation = rotation * R.from_rotvec([angle, 0, 0]) + elif axis == "y": + rotation = rotation * R.from_rotvec([0, angle, 0]) + elif axis == "z": + rotation = rotation * R.from_rotvec([0, 0, angle]) + else: + raise ValueError("Invalid axis. Axis must be one of 'x', 'y', or 'z'.") + + new_quaternion = list(rotation.as_quat()) + new_pose = Pose(position, new_quaternion) + + self.move_eef_to_goal_pose(new_pose) + self.log.info(f"Rotated EEF TCP around {axis}-axis by {angle} radians.") + except Exception as e: + self.log.error(f"Failed to rotate EEF TCP: {str(e)}") + + def rotate_eef_joint(self, angle): + """ + Rotates the end effector of the robot arm using the last joint. + + Args: + angle (float): Desired rotation angle in radians. + + Returns: + None + """ + # TODO + try: + current_joint_values = self.get_current_joint_values() + target_joint_values = current_joint_values.copy() + + if len(target_joint_values) <= 6: + raise ValueError( + "Joint array is too short to perform EEF joint rotation." + ) + + target_joint_values[6] += angle # Assuming the 7th joint rotates the EEF + DOF = len(self.robot_states.q) + target_vel = [0.0] * DOF + target_acc = [0.0] * DOF + MAX_VEL = [1.0] * DOF + MAX_ACC = [1.0] * DOF + + self.robot.setMode(self.mode.NRT_JOINT_POSITION) + self.robot.sendJointPosition( + target_joint_values, target_vel, target_acc, MAX_VEL, MAX_ACC + ) + self.log.info(f"Rotated EEF joint by {angle} radians.") + except Exception as e: + self.log.error(f"Failed to rotate EEF joint: {str(e)}") + + # ---------------------------------------------------------------- + # Functions for IK + # ---------------------------------------------------------------- + + def joints_to_cartesian(self, joint_values): + """ + Converts the robot's joint angles to its Cartesian coordinates. + + Args: + joint_values (list[float]): A list of joint angles for the robot arm. + + Returns: + Pose: A Pose object representing the Cartesian position and quaternion orientation. + + Raises: + ValueError: If the number of joint values does not match the number of active joints. + """ + try: + if len(joint_values) != len(self.active_joints): + raise ValueError( + "The number of joint values does not match the number of active joints." + ) + + full_joint_values = np.zeros(len(self.robot_chain.links)) + active_joint_indices = [ + self.robot_chain.links.index(joint) for joint in self.active_joints + ] + + for i, joint_value in enumerate(joint_values): + full_joint_values[active_joint_indices[i]] = joint_value + + cartesian_matrix = self.robot_chain.forward_kinematics(full_joint_values) + position = cartesian_matrix[:3, 3] + orientation_matrix = cartesian_matrix[:3, :3] + quaternion = R.from_matrix(orientation_matrix).as_quat() + + self.log.info(f"Converted joint values {joint_values} to Cartesian Pose.") + return Pose(position, quaternion) + except Exception as e: + self.log.error(f"Error converting joint values to Cartesian: {str(e)}") + raise + + def cartesian_to_joints(self, position, orientation): + """ + Converts the robot's Cartesian coordinates to its joint angles. + + Args: + position (list[float]): Cartesian position of the robot arm. + orientation (list[float]): Quaternion orientation of the robot arm. + + Returns: + list[float]: Joint angles corresponding to the given Cartesian coordinates. + + Raises: + ValueError: If the solution is invalid or out of bounds. + """ + try: + rotation_matrix = R.from_quat(orientation).as_matrix() + + target_pose = np.eye(4) + target_pose[:3, :3] = rotation_matrix + target_pose[:3, 3] = position + + initial_joint_values = [0] * len(self.robot_chain) + joint_values = ikpy.inverse_kinematics.inverse_kinematic_optimization( + chain=self.robot_chain, + target_frame=target_pose, + starting_nodes_angles=initial_joint_values, + orientation_mode="all", + ) + + if not self._validate_joint_limits(joint_values): + raise ValueError("Joint values exceed physical joint limits.") + + self.log.info(f"Converted Cartesian position {position} to joint values.") + return joint_values[1:8] + except Exception as e: + self.log.error(f"Error converting Cartesian to joint values: {str(e)}") + raise + + def _validate_joint_limits(self, joint_values): + """ + Validates if the joint values are within the robot's physical joint limits. + + Args: + joint_values (list[float]): Joint values to validate. + + Returns: + bool: True if all joint values are within limits, False otherwise. + """ + joint_bounds = self.get_joint_bounds() + for joint_value, (lower, upper) in zip(joint_values, joint_bounds): + if not (lower <= joint_value <= upper): + return False + return True + + def calculate_IK_error(self, goal_position, goal_orientation): + """ + Calculates the inverse kinematics (IK) error for reaching a target pose. + + Args: + goal_position (list[float]): Desired Cartesian position of the robot arm. + goal_orientation (list[float]): Desired quaternion orientation of the robot arm. + + Returns: + float: The IK error as the Euclidean distance between the desired and actual pose. + """ + try: + joint_values = self.cartesian_to_joints(goal_position, goal_orientation) + actual_pose = self.joints_to_cartesian(joint_values) + + position_error = np.linalg.norm( + np.array(goal_position) - np.array(actual_pose.get_position()) + ) + orientation_error = np.linalg.norm( + np.array(goal_orientation) + - np.array(actual_pose.get_orientation(type="quaternion")) + ) + + total_error = position_error + orientation_error + self.log.info( + f"Calculated IK error. Position error: {position_error}, Orientation error: {orientation_error}." + ) + return total_error + except Exception as e: + self.log.error(f"Failed to calculate IK error: {str(e)}") + raise + + # ---------------------------------------------------------------- + # Gripper Functions + # ---------------------------------------------------------------- + + def find_gripper(self): + """ + Searches for the gripper on available serial ports and returns the port if found. + + Returns: + str: The serial port where the gripper is connected, or None if not found. + """ + ports = [f"/dev/ttyS{i}" for i in range(2)] + [ + f"/dev/ttyUSB{i}" for i in range(2) + ] + + for port in ports: + try: + print(f"Trying port: {port}") + ser = serial.Serial(port, baudrate=115200, timeout=1.0) + device = mm.Instrument(ser, 9, mode=mm.MODE_RTU) + device.write_registers(1000, [0, 100, 0]) + registers = device.read_registers(2000, 3, 4) + posRequestEchoReg3 = registers[1] & 0b0000000011111111 + + if posRequestEchoReg3 == 100: + print(f"Gripper found on port: {port}") + return port + except Exception as e: + print(f"Port {port} is not the gripper: {e}") + finally: + # Ensure the serial port is closed after each attempt + if "ser" in locals() and ser.is_open: + ser.close() + + return None + + def connect_gripper(self): + """ + Connects to the gripper and activates it if necessary. + """ + try: + gripper_port = self.find_gripper() + if gripper_port: + self.gripper = pyRobotiqGripper.RobotiqGripper(portname=gripper_port) + self.log.info(f"Connected to gripper on port {self.gripper.portname}.") + + if not self.gripper.isActivated(): + self.log.info("Activating gripper...") + self.gripper.activate() + else: + self.log.error("Failed to connect to the gripper. No port detected.") + except Exception as e: + self.log.error(f"Failed to connect to gripper: {str(e)}") + + def gripper_goto(self, value, speed=255, force=255): + """ + Moves the gripper to a specified position with given speed and force. + + Args: + value (int): Position of the gripper (0: open, 255: closed). + speed (int): Speed of the gripper movement (0-255). + force (int): Force applied by the gripper (0-255). + """ + try: + if self.gripper: + self.gripper.goTo(position=value, speed=speed, force=force) + self.log.info( + f"Gripper moved to position {value} with speed {speed} and force {force}." + ) + else: + self.log.error("Gripper not connected. Call connect_gripper() first.") + except Exception as e: + self.log.error(f"Failed to move gripper: {str(e)}") + + def open_gripper(self, speed=255, force=255): + """ + Opens the gripper to its maximum position with specified speed and force. + + Args: + speed (int): Speed of the gripper movement (0-255). Default is 255. + force (int): Force applied by the gripper (0-255). Default is 255. + """ + try: + self.gripper_goto(value=0, speed=speed, force=force) + self.log.info(f"Opened gripper with speed={speed} and force={force}.") + except Exception as e: + self.log.error(f"Failed to open gripper: {str(e)}") + + def close_gripper(self, speed=255, force=255): + """ + Closes the gripper to its minimum position with specified speed and force. + + Args: + speed (int): Speed of the gripper movement (0-255). Default is 255. + force (int): Force applied by the gripper (0-255). Default is 255. + """ + try: + self.gripper_goto(value=255, speed=speed, force=force) + self.log.info(f"Closed gripper with speed={speed} and force={force}.") + except Exception as e: + self.log.error(f"Failed to close gripper: {str(e)}") + + # ---------------------------------------------------------------- + # Other Functions + # ---------------------------------------------------------------- + # def wait_for_motion_completion(self, target_joint_values, error_threshold=0.01, speed_threshold=0.01, timeout=10): + # """ + # Waits until the robot's motion is completed by checking the error between current and target joint values + # and monitoring joint speeds. + + # Args: + # target_joint_values (list[float]): The target joint values (in radians). + # error_threshold (float): The allowable error threshold for each joint (in radians). Default is 0.01 rad. + # speed_threshold (float): The allowable speed threshold for each joint (in rad/s). Default is 0.01 rad/s. + # timeout (int): The maximum time to wait for completion (in seconds). Default is 10 seconds. + + # Returns: + # bool: True if motion is completed within timeout, False otherwise. + # """ + # motion_complete = {"status": False} + # start_time = rospy.get_time() + + # def check_motion(event): + # self.update_robot_states() + # current_joint_values = self.robot_states.q + # current_joint_speeds = self.robot_states.dq + # # Calculate the absolute difference between current and target joint values + # errors = [abs(c - t) for c, t in zip(current_joint_values, target_joint_values)] + # # Check if motion is completed + # if all(error <= error_threshold for error in errors) and all(abs(speed) <= speed_threshold for speed in current_joint_speeds): + # motion_complete["status"] = True + # rospy.loginfo("Motion completed: Joint values and speeds are within thresholds.") + # rospy.signal_shutdown("Motion completed") + # if rospy.get_time() - start_time > timeout: + # rospy.logerr("Timeout: Motion did not complete within the specified time.") + # rospy.signal_shutdown("Timeout") + + # # Start ROS Timer for checking motion status + # rospy.Timer(rospy.Duration(0.01), check_motion) # Check every 10ms + # rospy.spin() + + # return motion_complete["status"] + + def wait_for_joints( + self, + target_joint_values, + error_threshold=0.01, + speed_threshold=0.01, + timeout=5, + ): + """ + Waits until the robot's motion is completed by checking the error between current and target joint values + and monitoring joint speeds. + + Args: + target_joint_values (list[float]): The target joint values (in radians). + error_threshold (float): The allowable error threshold for each joint (in radians). Default is 0.01 rad. + speed_threshold (float): The allowable speed threshold for each joint (in rad/s). Default is 0.01 rad/s. + timeout (int): The maximum time to wait for completion (in seconds). Default is 10 seconds. + + Returns: + bool: True if motion is completed within timeout, False otherwise. + """ + start_time = rospy.get_time() + while rospy.get_time() - start_time < timeout: + self.update_robot_states() + current_joint_values = self.robot_states.q + current_joint_speeds = self.robot_states.dq + + errors = [ + abs(c - t) for c, t in zip(current_joint_values, target_joint_values) + ] + if all(error <= error_threshold for error in errors) and all( + abs(speed) <= speed_threshold for speed in current_joint_speeds + ): + rospy.loginfo("Motion completed successfully.") + return True # Exit on success + + rospy.sleep(0.002) # Check every 5ms + + rospy.logerr("Timeout reached.") + return False # Timeout occurred + + def wait_for_eef( + self, + target_tcp_pose, + position_error_threshold=0.03, + timeout=5, + ): + """ + Waits until the robot's motion is completed by checking the error between current and target tcp pose + and monitoring tcp velocities. + + Args: + target_tcp_pose (Pose): The target tcp pose (position and orientation in quaternion format). + position_error_threshold (float): The allowable position error threshold (in meters). Default is 0.01 m. + orientation_error_threshold (float): The allowable orientation error threshold (in radians). Default is 0.01 rad. + velocity_threshold (float): The allowable velocity threshold for the tcp (in m/s for linear and rad/s for angular). Default is 0.01 rad/s. + timeout (int): The maximum time to wait for completion (in seconds). Default is 10 seconds. + + Returns: + bool: True if motion is completed within timeout, False otherwise. + """ + position = target_tcp_pose.get_position() + orientation = target_tcp_pose.get_orientation(type="quaternion") + target_tcp_pose = position + orientation # Combine position and orientation + + print(f"target_tcp_pose:{target_tcp_pose}") + + start_time = rospy.get_time() + while rospy.get_time() - start_time < timeout: + self.update_robot_states() + current_tcp_pose = self.robot_states.tcpPose # Get the current TCP pose + # current_tcp_velocity = ( + # self.robot_states.tcpVel + # ) # Get the current TCP velocity + + # Compute position error (Euclidean distance between position vectors) + position_error = np.linalg.norm( + np.array(current_tcp_pose[:3]) - np.array(target_tcp_pose[:3]) + ) + + # Check if position errors are within threshold and velocities are below threshold + if ( + position_error <= position_error_threshold + ): # and np.linalg.norm(current_tcp_velocity[:3]) <= velocity_threshold + rospy.loginfo("Motion completed successfully.") + return True # Exit on success + + rospy.sleep(0.01) # Check every 10ms + + rospy.logerr("Timeout reached.") + return False # Timeout occurred + + def compute_orientation_error(self, current_orientation, target_orientation): + """ + Compute the orientation error between two quaternions. + """ + # Convert quaternions to rotation matrices or use quaternion distance + q_current = np.array(current_orientation) + q_target = np.array(target_orientation) + + # Compute quaternion distance (angle) + dot_product = np.dot(q_current, q_target) + angle = 2 * np.arccos( + np.clip(dot_product, -1.0, 1.0) + ) # Returns the angle in radians + return angle diff --git a/Robotics_API/Pose.py b/Robotics_API/Pose.py new file mode 100644 index 0000000..7635ec6 --- /dev/null +++ b/Robotics_API/Pose.py @@ -0,0 +1,101 @@ +# !/usr/bin/env python +# -*- encoding: utf-8 -*- +""" +# @FileName : Pose.py +# @Time : 2024-08-03 15:08:38 +# @Author : yk +# @Email : yangkui1127@gmail.com +# @Description: : POse +""" + + +import numpy as np +from scipy.spatial.transform import Rotation as R + + +class Pose: + """ + A class representing a 3D pose consisting of position and orientation. + """ + + def __init__(self, position, orientation): + """ + Initialize a new Pose object. + + Args: + position (list / np.ndarray): A list or array of three floats representing the position in 3D space. + orientation (list, tuple, np.ndarray): A list, tuple, or array representing the orientation in 3D space. + It can be either Euler angles (3 elements), a quaternion (4 elements), or a 3x3 rotation matrix. + """ + + self.position = list(position) + self.x, self.y, self.z = position + + # Rotation matrix + if isinstance(orientation, np.ndarray) and orientation.shape == (3, 3): + r = R.from_matrix(orientation) + self.orientation = list(r.as_quat()) + # Quaternion + elif ( + isinstance(orientation, (tuple, list, np.ndarray)) and len(orientation) == 4 + ): + self.orientation = list(orientation) + # Euler angles + elif ( + isinstance(orientation, (tuple, list, np.ndarray)) and len(orientation) == 3 + ): + r = R.from_euler("xyz", orientation, degrees=False) + self.orientation = list(r.as_quat()) + else: + raise ValueError( + "[Pose] \033[31merror\033[0m: Orientation input must be Rotation matrix / Quaternion / Euler angles" + ) + + def get_position(self): + """ + get position + + Returns: + list: position + """ + return self.position + + def get_orientation(self, type="quaternion"): + """ + get orientation + + Returns: + list: orientation + """ + if type == "quaternion": + return self.orientation + elif type == "euler": + return R.from_quat(self.orientation).as_euler("xyz", degrees=False) + elif type == "rotation_matrix": + return R.from_quat(self.orientation).as_matrix() + else: + raise ValueError( + "[Pose] \033[31merror\033[0m: Orientation type must be 'quaternion', 'euler', or 'rotation_matrix'" + ) + + def print(self, pose_description="", type="quaternion"): + """ + print pose + """ + if type == "quaternion": + print( + f"[Pose] \033[34mInfo\033[0m: {pose_description} position: [{self.position[0]:.3f}, {self.position[1]:.3f}, {self.position[2]:.3f}], orientation: [{', '.join([f'{x:.3f}' for x in self.get_orientation('quaternion')])}]" + ) + elif type == "euler": + print( + f"[Pose] \033[34mInfo\033[0m: {pose_description} position: [{self.position[0]:.3f}, {self.position[1]:.3f}, {self.position[2]:.3f}], orientation: [{', '.join([f'{x:.3f}' for x in self.get_orientation('euler')])}]" + ) + elif type == "rotation_matrix": + np.set_printoptions(precision=3, suppress=True) + print( + f"[Pose] \033[34mInfo\033[0m: {pose_description} position: [{self.position[0]:.3f}, {self.position[1]:.3f}, {self.position[2]:.3f}], orientation: {np.array2string(self.get_orientation('rotation_matrix'), formatter={'float_kind':lambda x: f'{x:.3f}'})}" + ) + else: + raise ValueError( + "[Pose] \033[31merror\033[0m: Orientation type must be 'quaternion', 'euler', or 'rotation_matrix'" + ) diff --git a/Robotics_API/Utils.py b/Robotics_API/Utils.py new file mode 100644 index 0000000..c4036c1 --- /dev/null +++ b/Robotics_API/Utils.py @@ -0,0 +1,174 @@ +# !/usr/bin/env python +# -*- encoding: utf-8 -*- +""" +# @FileName : utils.py +# @Time : 2024-12-02 01:01:41 +# @Author : Yan +# @Email : yding25@binghamton.edu +# @Description : Utils +# @Usage : None +""" + +from scipy.spatial.transform import Rotation as R +import xml.etree.ElementTree as ET + +def quat2eulerZYX(quat, degree=False): + """ + Convert a quaternion to Euler angles (ZYX order). + + Parameters + ---------- + quat : list of float + Quaternion in [w, x, y, z] order. + degree : bool + If True, return angles in degrees; otherwise, in radians. + + Returns + ------- + list of float + Euler angles in [x, y, z] order. + """ + # Note: scipy's `R.from_quat` expects [x, y, z, w] order + euler_angles = R.from_quat([quat[1], quat[2], quat[3], quat[0]]).as_euler( + "xyz", degrees=degree + ) + return euler_angles.tolist() + + +def print_description(): + """ + Print the description of the demonstration. + """ + print("This script demonstrates teaching by demonstration for a robotic arm.") + print("Free-drive the robot to record Cartesian poses and reproduce them.") + print() + + +def list2str(lst): + """ + Convert a list to a space-separated string. + + Parameters + ---------- + lst : list + List of elements to convert. + + Returns + ------- + str + Space-separated string representation of the list. + """ + return " ".join(map(str, lst)) + " " + + +def parse_primitive_state(states, target_state): + """ + Extract the value of a specific primitive state from a list of states. + + Parameters + ---------- + states : list of str + List of primitive state strings. + target_state : str + The state name to extract. + + Returns + ------- + str + Value of the target state, or an empty string if not found. + """ + for state in states: + words = state.split() + if words[0] == target_state: + return words[-1] + return "" + + +def parse_pt_states(pt_states, parse_target): + """ + Parse the value of a specified primitive state from the pt_states string list. + + Parameters + ---------- + pt_states : str list + Primitive states string list returned from Robot::getPrimitiveStates(). + parse_target : str + Name of the primitive state to parse for. + + Returns + ---------- + str + Value of the specified primitive state in string format. Empty string is + returned if parse_target does not exist. + """ + for state in pt_states: + # Split the state sentence into words + words = state.split() + + if words[0] == parse_target: + return words[-1] + + return "" + + +def load_poses_from_xml(filename="saved_poses.xml"): + """ + Load poses from an XML file. + + Args: + filename (str): Name of the XML file to load the poses. + + Returns: + list: List of poses with positions and orientations. + """ + tree = ET.parse(filename) + root = tree.getroot() + + poses = [] + for pose in root.findall("Pose"): + position = pose.find("Position") + orientation = pose.find("Orientation") + pose_data = [ + float(position.get("x")), + float(position.get("y")), + float(position.get("z")), + float(orientation.get("qx")), + float(orientation.get("qy")), + float(orientation.get("qz")), + float(orientation.get("qw")), + ] + poses.append(pose_data) + + return poses + + +def pose_to_euler(pose): + """ + Convert robot pose from a list [x, y, z, qw, qx, qy, qz] to [x, y, z] and Euler angles. + + Parameters: + pose: list of 7 floats - [x, y, z, qw, qx, qy, qz] + + Returns: + tuple: (x, y, z, roll, pitch, yaw) where (x, y, z) is the position and (roll, pitch, yaw) are the Euler angles in radians. + """ + x, y, z, qw, qx, qy, qz = pose + r = R.from_quat([qx, qy, qz, qw]) # Reordering to match scipy's [qx, qy, qz, qw] + roll, pitch, yaw = r.as_euler("xyz", degrees=False) + return [x, y, z, roll, pitch, yaw] + + +def euler_to_pose(position_euler): + """ + Convert robot pose from [x, y, z, roll, pitch, yaw] to [x, y, z, qw, qx, qy, qz]. + + Parameters: + position_euler: list of 6 floats - [x, y, z, roll, pitch, yaw] + + Returns: + list: [x, y, z, qw, qx, qy, qz] + """ + x, y, z, roll, pitch, yaw = position_euler + r = R.from_euler("xyz", [roll, pitch, yaw], degrees=False) + qx, qy, qz, qw = r.as_quat() # Getting [qx, qy, qz, qw] from scipy + return [x, y, z, qw, qx, qy, qz] # Reordering to match [qw, qx, qy, qz] diff --git a/Robotics_API/__init__.py b/Robotics_API/__init__.py new file mode 100644 index 0000000..b142588 --- /dev/null +++ b/Robotics_API/__init__.py @@ -0,0 +1,2 @@ +from .Bestman_Real_Flexiv import Bestman_Real_Flexiv +from .Pose import Pose \ No newline at end of file diff --git a/Robotics_API/__pycache__/Bestman_Real_Flexiv.cpython-312.pyc b/Robotics_API/__pycache__/Bestman_Real_Flexiv.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..f75125b2ca784932aa945447b8630d9bb88dcb32 GIT binary patch literal 43835 zcmeIbdsG}(nkN|f2*`v45)zU}@<>1uAfc2HdZH&!NtT3c3CU&IE-FPN2nk3OnOGtV zxvIU@r%z2E!$?$7h`ayT468ag}F^51jZ|3yF4 z!>(N1{|+v0asqdb6D)#t#B$EU&en5QcD9|f3pT+%!jJOjcnjs>N3uq<&pFt&b;Nnj ziF;WiIitDfa@oD?5!Yzmxjf~0{<(ZsPr*4iI~SfSWapxDMeOW3=dp5!Il=KQPH?_u zm3wurc+%RG6Z!IA!+kl+@BPN z$A-4;ICLJT3&GK7cq|YSCMauHC^8@pk4J~YV{%jN{`0}e@PL0{Bp8YK2T{G;HNO}> zACBTdP#mR#XClEN^<7vWd&6U)oKNY!dq1@xZL&!AC&Q5t&W=;zXpow~HQ#w5I7}Cg zGpD+Gjt`Gr8j#0IPL+YDMLOxQrP=K^ha3d{pazedO^exLdkFZYg;9TtF z=%27&D8{c6p#-_Sf){D2P>QrnC_`Ebt2p$_HhCkvaJB#+6w2rgk|dsz8ohEE*Mu&rd`{kv8_5s$w6s&EJ<%&L2L1F*FcG^~2HO;K*<+B*+bn zi{TfB1^hbtq(3MKVki;`MFLuNLzqV6AyKYS`JvWD(`X<(78S!IBbb7$uhe`)#2*?H z{Gq`?v=|ok$_B;I<%!VPz@%KAQr55CW)qX%n7xEvZX`U^hB3lvC?u+_W2|t)KZ*`x z&dQTo2%Vo88lt`g(Sgx0UN$V^r(V@)SO`&FVrVcVqAMXk>M;y*7%$5vHA)M{fnZ(g zk6^}QwCRK4#WCg49~y>;+zzX0U_wOAQQSjbe|S(Em#E$ixwZjXG*sCoR5Xfau|Vlo z`ZzTE$Ae-7UuaMakFv^Wjrj)wdGrc{1JU6ZLj4!Rm_Mw4Bg2s>H7-B!H;SPb!Xua} ze)M)+9FDN&n*4GBObfGKVto6l8ToV36}o~+NpDOag9e(t2}WZSKu=I6&VUNOY4vC0 z&xXJIA0xfVMG1&u=L| zQ6fuy5>+p_i}~86c7dD@^@pMEBK4krHq@=P9QCTtFP5oS`uUh)X7XsYLH_D9rKtCrNyQ+7cc^K1Mg%Gc}V)T=38Z9S47U9a91Y*YMe-1n^NvzIK< z2KCv+M)gWR>$PC~OVs;GjMtw4A8L(Q|+g#zA@`8<+k~xHl=s2pOn+O9iMyi9s zXfZSbC@ByC83(BW;HBU;D%~y)8892&k>Td>xW6$H6`N$Zq$4gB*cw~WK_rzQ2@ilQ zOE1sv8PFdCZv@aL#)by~lmX;4RLt5>uTsZeje{fMU=+RTl#$}25nFF;Lm;rF9Vj=t zWsuQ|{o-&eJhr`G!w8YUgeVNgN;H`79}7p(#fdSY&EM1aDS=nXp+a}bsloQxy6&sv zK)@K>T`Crkp?$L-G=p4)Kh_-ZcU}k$TuMhQ?As_f1-SiEq<=IRxfF=)G!E@5v=x~c z7yvpRoERCIG*TLYSb>5d`hhkA(TUL}zvPr(SQIWv{3)VBC7zA0l&z2%$f42;lI?hS zNV1`@WIqACEZGS=OS!DvX@-*}7b~q`XvrFmNOnS2QnoNGj)6j#vV-R%?2e{^U{iY&Ab4VGN0s?C+0Nnoza@^=UI&jC=MBC^ri}H6NR;N=RPdl zvalo7+L>tWy!G`DTTk7~=JJZ}*-(NSuJ|mUa}`WSQw6n&g4$$m-CXd)+=i8+%H^V+ z??e{2r}mym>^+g()f?Y=@-An2(y||E;eq=$njEsL^BkeU%~- zUqcySgEm^o|uKLsrmw~oX-p%r5-lKAj_b5=?)X##3 zS1sJ6eT{O3dVcjv(C8=DL$gscE0y)g_wuMM=x4*I=`>chT2mMId@raUq6+jP+SSEU z?NYl?>l3TK!23GX_ADGm%K7-;@8u`$^bO5M$ZTHa=$#{#kFVpzmTT5EYBKw> zxlxTu1An5-CnI%h;hq6ibj|vrbK_bFj6@?02ZzRj=NWT74DL~@qVK{m7~Y97Fny5lgvKHh;M;W$&X1=WBO4q+ z-MW0B1^8ZVCpb3gXSh#qRHtpW1^nG=<0d?LYry|>5V8bxcVa9G!IYNAe?A0kkNz0C zVCeg)P;^2Z%b@Gdhr=VF=ERARf7rk<(~GP{R5bg0f+G>|vuIenG8{25!yr>=1pVOw zh7<)q21y>}NJxAkBsTkl(P(HC@-g%t{68D`Kp-FzM2pqSg$IITL}{H5$&W)qGmRA$ zLC&5BJ|!28B}gn|F9Q!sOhzsc_>`E$2jvXC(wmKiucVDp--C^YXXw5vi@+xg4ni#C zZ`taPU~q}ij^XvekPFm*U8tjfg*Jb0Q@-d%i&FM*gmqRdreF3eK@qeDduMiAvPU7A zl=#R0$pB zOplNtSwaySR?L2p7%9oE_qLyXl-N2m3L-OCKgvedVI6&wyIbaSmVaE}c_nr|CZort zg8F55@lD5!<7VDWUcCD3lKW}8ku#H%a<5Og*DtvnR^;EaE#Jvo_LSain`ujV)+apc zmpl#2#bq~-%p4(lBVK=Gsrcx!&!1M`-X-@wAkVzQSB_ji^2&+pC*l zTz?|vs!F)3mRvQUOX?fvt$)#UkF)1h-^kb6*_7~XTJmgG+c`0FB3`$DsrbMzeKlIQ zzMFT~iBfl6Y0dO3xehOT%0B<3pzI!Jb(T|;)Q8^dz45C3KOMML`;)=Nuf%i%Ct z@N)CZpv(V;_RZ0g83OxfjQ9|+>@c<XHhq%z0y#5MAb%(nWi*B?G!cm;e$&FHMuf;(1xI@(Skp3oRzxbX2D8aw zl5r08L@JT6Ndg{}eWKcgqjqjxs;)Ir*ZPrTyLb#4%tv(=PreB9F(1{7nzSF)DThAN zD5KsqoG+U2nMUpuYF}HdUHd*K!Y`yz-O!;jY7C^iLcRDKNygITABVhx z)Z4@>1RCUjB-Rp1axInip6cs=^3>52NBfSR?e6dXN_Xd(zN065C3om*XaFjlC&l4W z5TYPtX#yI*TlP@6pIVIh%)XWK@PDL2M2e+-pNfQ|Vs`uMdtTj>^6g9b_ANQ~Ys;%X z;i#WG@{wb+{MBggt4A&_*jo4|rq4}iS9OgE#+U$2#)0uv?(X2O*`{n4A=l45rYw+C zTLr6`v>>W|qbQNn?9g>a>K8_N+xP71FG#HI^iOS>@!4i_Z!_MHXT8aqvVz|;)JPm< z3kLrRWZ%zl#L2$AN#^&!Y0_F_2jetJ7GTzz34bnR>u7OGYi$*$X}@NF(GE_Noei9( zBPMCH^As2bQ2@46m2-*|8?-)zlB44c&qaeGAOg)#Oeok7=q%=H**kFwa!F0l-3MJQ z-Btzfij-HM>csQPZS$S}HDa_35@a(hmmihX>Y<#ovomPkepGdYIPHsFBZ#sIX zDDFGH3x9NW;Yi%KKjk`*a2@!^yEbHc5Fz#1OJ2wZXLq#kvfa*W!sYEFD!c=V*)dqF$NEj#A@0>7Oh=>8{=?#WBhSy_<b*0n~K$&t?CR~*%S6#wYH`g+^E$-TI-)1e#A>L#6 z^~2Nc(@_A{yrLW3v$eCqSJ%&$&ROTS|6tD>d*(L9ivmloEh@AHh@S19tDOtJv3{;} z-a5biFZaB;XMR(>V&_M$UB3ZSvip%U`^#u()`X%aP|-}&1B)C`X}Wy}Xe!HT4z!Op z%QY#xS&bTO#qcVJV2x@s{UK6z8yiKK9s4+RO%AH9%wV^)Pw!AMHw;%a1dY zA=Z>_43sJ`9nX^m+A4^y&Gs_2X+e=VW9p&hHh-*Ar%s6Ek{LPmX>U`u0UkT$=6WYa z<$2zT5t2FkE?lWIK8t4cuppBlX|q0uz^wcjmfE7fL$X}f0la8F`)xva&}8ao)6_?+YjD8w!@oxx5i$a#NuTX3Veoui z!nJP6wPB?Q^moeJknlF7yiEyjQ?h8|G!tbhqW{@r^R~H(c*W)=S76!Woqj@{|5S}G zo8R%yz+&x>1{WZkKd|IFxZ){h^S?x$|M~5O-0ebVN3QL5MMFoH?cFRM*J+fpHvey+ zt47O7eV#`(?ip*t1kS3nJM;Xz&vrd(%6zjZE}YS@!#*psYp zOIGbomhGF)BQcC>#!=R?uz8^-?rmRkbSxKC#vPRpAr9<1*hC|lhq-X39NE4QX`qQ? z^!f#`OQxiOM%-%P(PYY+lDoF31~eZ69_@+(o}JUc<65Kv(UK}G?CX5e-`EF5w7(N} zh(PmCk}-7?qlh2HvoS(}`GF|30aZris6H7*LL{+b^#Lr;)$R` zGSxx)t&~flI+lGkDPMEK*PQYN628Fnadla+)w6A3-$Ec>(Z1yBSoYM!T{SAnq3OsN z$=&mQkht@ucD~)g-F7%T?6%v5jkJpPSm^p7 zkJNk>?ffsXikJ^y1&7%W0f}J2l`Trd4m%)hv#EmmYa}{1BcaWRa@&+W1MAosLz+&M z^J~-uPGi>XD+FQR;{vC-c!aUyP7b6 z5_J%1L}%4CN(_y|9t=oKQ4FR@)lom%W#WD&EQEs;axz(uz7TsqB~~it>r|Gb(VvTn z<*_EA_*M;&R9u$UkdjZO?ipd94O~M#744{g?S+{oOWqxXnb6sgWMWzYQd^GH_&4JW7J8DHHV=oeX*AMQl*WF(#BNj)Yyc9UG+bz&7R^tD%Wx2v5UWwzT54ILiayB;3bX{(rMGy0DAJM>kIpP7f5 zct>TbuUT{!BTa0ouUE}{7lKge2C%BFrixU}a8k6qT%y-XiZ#JzP}h$($V!x_xXjq9SuQ0woD6|E`V0A(;LbgWv@{59K)wn<)KXTyrlian(*MkYK> zbInYXnvzvvcx-SO(x2c9SSMshM^=Z5zox#@VqT_?ho{V7m`q1hhfc}TFJ46FOqZFA zk_}Q?A`)|S36gjTB_$iV0^~qtCq^UW!2-{U5Egn=l<^=#i#o|@mkmUS`H}1_7h6}f zP-$H$#(T1ul(i402*OlT}j)(E=OiOLTUa3pdX!(xXwMC&fVYV#P zNq;R12n+saru{crI%=!e98->qn)9g~pm<)n&b4e?YRv#S*BcS4t10O(pnYWP9D?nKkntQ5XDu2Lw)q1G{h%9T$&CZsW z%f>ADE}G9K^gem;T|_?sqvgTt_YPGeb^qh6R9B}3x> zLI^%m!_i4b=LF%K28F#s=xA-MwsSCY1G2@!voccy!vzFZ7$$S?v__4*4AZtU9WBeE z!+CyY>UY8IL zF=KlFv*I%-@WAqX%tzB)AXzAVQF&&m+-z)}PGpZFn|ZFLVj1;A4b90mWJP2{_Qc7f zy|5%Zc^ZafCwntw>@sW2uszQPUkp*{0ZPtOGD1lc5~&cZguvbjq&s6AWXipha_9s^ zkxW4R9u-jyWiXAkYkU_eTbVcV1wN-_gRoxWgCpY?f>Mr7S4cK0S|G@C9p0x``&Zqd z3Kia6HnkAPs7J)}OZ?@2m@2SS@T$xq5YnPmn10A~O&JX?Q<+7>kHO-vmAi-NoOh6b z&hghdk25a8=JnU!Qz+GF7uTQL}f+wa>`^n`<5JW^pBD)7{JERj*gPTJd_# zt2IgZ?>(`+^H6GMPhw}!-&7x-kIY8q&b;>Zc=h4wlSn>xd+yq-HTgJ$Dyk`-J_$d- z(&`VrTjy$~4=;PG)T@S;c*FizD{gh(+Ht4#_FmMw>JA91%9`2O>)&|w8w)vqRq(@t zr0>x5@s*9O?{qG@ZjB_jpH6Pog>z56Q8Jf3cV+(a{J=uZt&&^Wx5PW^@07-$d@5dc3fgm@_bz8G@!sW8 zBdkH-ZNY}FG2v=lc2~|`oZq-Gw0Pw+n-$KDY0emxyB1vwFUqz}zrDAGt2tu%{Iji! zS<~@!YOwwN`dh*GHr#?<5{?_k60T#68axhakZdAf>Tq;7bMNN3x*P28dfT_-$NRbM zKAb*i;3;i(_H3|yu%n@;*7n!6Jg&`yUHspe1R3U#mMCzWe5hnbBveWTfv{AhS zS4_Ms9gGAqt6k%{XB95qykjaiITZ z^=XcLdP6X})CjgI%{ei>1;}|E*Bmc8CLQwGK(si6lNbPvpde^28`Y!C@;YXn;Tno*~Ux#edX5fw-yJhlc}{G(e8fr8W;;VCvTvR0MK!!_&NAtPhMut0&g zTIQ98iepU98^1jq31;CU&u50EX5ocJgB)}Im|C*|ma@+Kl#kt=?zlD-(i z6&XSn5hA7mhLme#1_I8^Mqf$E+uKwi5>3p6TavR6mCMfjOv|EvDye`ZQwAwkCho2v z+}+uJs;~R>(e~c{6DJ9K_xGGU)!8kT5i0HkLyuVy96N!~UE#5wu!sl)YYMrYiWRFi zouvHqFE16)Z^L(!vaV2QqzmG=>1C!VxlRd_2M|L_-f{st8TZI%@{PgzzxRkOoy7cY4NE8d#8za#1GjJrEO_Lko`GyBxcGcZUg^`}ZV zCrURbOSh2Ftv*%Jny6?c1J~AMMbGrHWsh(6Qo_?T-9@Ibt59%fqJ|VqBqw0cwj^MX z#2n6DTY{1 z0kIHdda0weV<-3SPG{#<+q(xFIyc$g+r;BKO}+rmS(Y!H1*-V+=;IM6BWn$mVSQT@ zi2+dn$TE~cfYc1AYqnbGvvPbXl);O8)@Z>DZD>gC3n2{f$?4#HjTTA!opDS#WKR1_ zA*!`}2MAq3ZXh2FT{+PsGjydPCN($I64_BrA~2=N57OU{p)2Q$PKK^RfeyOvkn^gZW%@}BV+`CGl1;J!DK2QO+Z%FfTTgzMipE#-d%^<@Db3|6&d(U z=@9cV&^dO+)qu3=N`_|#uQvM!C-E0+_K)Iv6xXA%=Lu&LiUzU-?jYl8yq6(R+@J_! zfpnfU9gu!u6uRS|9fh_8wyG#ph0%0omf{1^ev=_e6F!_!n06(Gc|Bcqp!($3z$3L=vkw4KvA%o-jTSSlJ`v?C$O!1FFuKJzV6MM5fId zs9VXcVeItmy-jW%dy7P0i*Hl%uPGs_SJc>R@!#O5oIm^Iu;K-0|LV#4Dcq$aPA^*=N#m_7lHoy>t2JJMX~Wc z*`1p@1KfK7XIG=`yHSd)Rgv9S!Qx>bxc(T6> zr@>v+Gz#j-f=ShwK}Kzu=ov1WPz;dp!~6{reo}1ae9u6!Ou9_*zFECyct7{C@V@IB z;e8jP(4^!2ydd-~I^Ji&d(9YC!jwUg+_Bs>U0Z+a`3aSP&Vy@n}RO zY8XbcTxIshl4Vk|#A2;S#~^kc#%rUqaO0;gt6U|->_nzy_BRkKmB&%=6LK*OUCgetU^ag%1|ifNP+7X{{VAO$2yYb6ve

^Y);q+E81VlRXSm|&J!up+Eo zgguu{D&CxG!G5BUyfA`iW0U%Wi&s=Ie89$QgZl-ruOi-t|-4V*5|B7QkH|O1at-u6Bm0I#f*6mDlCw z-gi5@b8PRIH+1v14|pEe1DePYpH#zCeUFE!$dKb+pzxzSzz%7H_!DVtQoxezxJ7;Jul? zT*D{mka8fWiOADT1FrUl;R>_b%`gn27%j+%mC6&Fbe@L9WSWNqzbTpNBr;u ze1+I9S%()C#UMhN67CB7BM8eMW&8#rw8KA=!F*X?r#2m6vc|th>n<_QMMHlY*kSXG5}}VZPsaq|XT0d!0K{NhD<%+K5V5~Guq4(#a8Seo#B)A#gh-nPmh+`^8<)u!; z1%X8aw=qj1zgjP(oSR|yW!_@FnoB;wm_N&Km{GrsDWM<@nob20CxlHI9;3)h4@E*o5SeC_0!1D6s!)PA zq_stIG?FlCsL#-VDdX}a(?LDLxLfb~W5`U6Ed~)ZGb0Y?SuFx}oBycn<*Yk=%I_j$ z3cnBr^GE?Q5hj2lQsA}poNM{x^^gWr&-P_2>=nO-i4rSSzOH7ErF1RkAtbF23yoqR z*35KqBv7OoK@>+?^TrZz5A?_=7a&D3g0YSvHfAg&h9kN!mc+v^Fm#of%1A}{x(KZ) z7(%y;|1*j!d>e*I9KI^vrV4cgZKt0ZOq$DJM^i|#)YA-7#1^WvSRP^3?NIhkWackY zE_*E*p-b)oRaXcT8rFetQ}H5$6kUD5*kXp!iFQ>N5S?W~qleWSAswGqsd*>`lZ=Ic z+>x-1*(*w&MJokGH})k9{NL}Kx2GE05{+%ib$dT7*vpXkb2<|L%)!;x|G@Ev<89A; zUuyII#OD2}&7FzOoyn%Iq`y1vs`|LSJJo(V(SG_r*{tjGmv`^KVNbdJ3AaDx-T-He@P2N$6Eu*X>-m zoT%ILp}P$sZ}1S-CjND)nk|W%EvcH0L`_G$voBe5Chn?SE^R=_hC=+c&+`e!c-FQo z)Gg58`Gw2)+mNW;6L(dwlvli7^=efdo}J}wsq&6Qc}KFmYx)VIDAbsum8}cM7Y@YR z`{EU6mRx6L|ACE3&*r#m^Kb9@(Cg1Xt7KL!R%cNft#I@g^>>0l-*5*;EvJ@TroZVY&A6Of^H`+ef$m7~vvRSc; zdN6aD)A5*DOfL=V7+ihKXa(I22Bt5ta8>Z3g8c})!uD0Y3oJ=@h_f>|5Sa3IDjgy7 zh*8b7#gw}^x3k`&g^UBVfVm`5i`EU3A1#iyQI?5vngNSk7o}$V#i;T{ zBQBoG5^{vx3XVBERjTC~&Ltafvzj#>qM94y=!({=zaSmOa;3ur=Vrf{ZNRyC!5<^* z!*Q+-jScvfz%(IW84$||*?^!{GkzL^%TV&^wQD&txy*FModN!3;_jz(vl(Nq)%9sF zY>K+moFSiJ2B8YYfv{}w&cbddw0#>50pU(rsH(q*VU!Fba_aF=c7%3%B2^t2*I`Ad z3DQHcKb1o{>ErP!&0QI#%HmKPywscQ5L!hLK-Bwgj}5* zsQXg~`fY;N^(r!o0j(D@lq~)XB_$^WrwUSx({Ed7G9+XDI{IlJzBE256=D2B*nP`@ z)urlH_xj0zdvrL)jLRi2A>Uz&-KpSb#X6Po?>g?!K>H3E?Gt`xHaAiMV~_C}m0n7@ z%9p^%grs66#AG^g@OxC6A#oZU6^Sbdd_bcGR3q1jIRZDyxQh_D3(FkR*-9@Ecy-O| zmtMUzzbRS0GgW;kQGF;`-7$UglZwjMH@v!GuJE-c*x=N)Odp%I&KBaYWA-WhWhGoS z%e8o<+-;xbVHf-Hy7hCdI)YC&9Z1w40HR(Yr`T-?-?o(Rsf6#TWp72wyCLD-uw2!+ zyrFHmcEepKS9!{EpW`d@r;o#8w74wg2_!s$`Kcw(K9Cae+7n4{Z`|FRhGq9GHZE2G zgUeVJ4z+WZx!SmAUEH-!X2l;LyE4O`&ce=O?!97XSB~wys>Uwf_CC+!I*k#hC1%6l zeGv%@dZz$a{L~H&MOyU$AI1&Qz;vQP#&p_PYKOxNkfi2@l0C&MZYU+#Si!=!SAbgy zg)I#h7(w(|WqtxY$V9$zbwN8LZ$7BlL#S+%`Qm+8#7I`FB3jA>vrjOPsDKA7RCUQ1 zp`DrSU;0h~6klYmzRRobA=?D>^dZW}1q?qkeMdx)X1b;&q?(n>pi)fHEq)|?B_vAx z1nq~&woZ|k7h7E7-yutEqnVb&92h)L>!}xou%3#!vdUCxbE34FR2Qz5O_;!o&m}iK zK>-pFTE1cW*sImE>*q?A;R5&UQgO?Cce1$Um!*|=ZCuGVI`hZ}BGp~IIzTJI0XFl9 zWK&$$Oy-_(o-rW`mB~1CAwRa6qZ!Q(GZ!{9i)3BEZziD8MJtcUXE(D|R#%Qnvcw>I zQLmva|L5+jrig!JUrj_zMn%5b$F%;YTWgysA56$odAmI*&$F1A#-Mbb%4S3%;T?7` zqL36IW;JW1shS%^VOEsnTI|pj;;V?lY_g6v5QUE59~vP}XK-WycEM^mYXyVqJBT4R z?9pRQezy4p^LP@;<%tNSG&3{OdnfR+Dxr;-oXH)j-~4 z0y4E?a~n0)xb3k;KFrljHuXMwOc{?L#8jXWyC()fR)k>TEI_)%ES%4WqE|xD#?xbv zW6Uy~qBuf4FrnBb>x7=J`HrpEzK7sfn+cGyv^;j~Gr)qXq}hK`x3`mfgJ8RsK&)C< zWP_fQKdn|1sW0-xzsHmjB}y21l?8k9;0QKw8-wZu%4gE7=(=u$Or(>F=sIh~emO7g z>q_p-&mzO^T=~W2paJ>n7w%{8BNZCHqH-%Y3N6)${6nUjOzfMBv^VTOdQ8q6+h7Lj zSLYiwkWF3>xOO6GS`)t3g%k14(@9?+6x-M`tYTZD0zp2K6^EveeNt8P`ZKRS^ZHj` z{pyOli1sv@txCEZ-masdzYBX)t;Z6r$C9llk{f#y?p}yWs`ICN;i>~UiSrHT5Axs0 zSBk~Uo09I0D@DaO8)q74_ojS16TY2`$K$@8N#D_A(G$P1SqhI?uwhs&q#{*t6Y@31 zH|;vVdOC^?zb}i<|cO8x&elGs}Sp2zgJUkwEUskmm#Rx}IZB91r{b}bd%TIb1 zcgKBQORjE%P(mh~_H^#!-rMKw+6IEDv1_yK{mneC(*hW1$g2|Q$QS`lml8n!kNqc^ z{68D~H(Y0A-&Wc07<+75bnU}KO4%|?#V`vglmUpY|CjU%h-|K*M1WaaIsh~vf-Drw zIH2|Bg{-%;-%@G-`lM;b4pU@k7aV#VO*8Gt+fJ=6eu_8KWC%G!oRIr3w7A94EAwVH zQi4m!6Y{m%u&b(2K=590#{j>XMxv0}ZXGhbe^KW9;|)E!ZIz0SU%)PND zCdfuDNg4zTq#4E(J9nG55wX1d;WtcM!IS^6F8wOQrO0>9VHkm8 z8UdkX%7J;H#c5}|fO!cKzI;|-XB{EduhDz4+Pv>#;iGzfg`9fwX^-fB#g$!&&9=q~XFY@bmB(lr%zWJXD4h2?0~ z#?~Q>fbe~c?UKR_zQ~P-8qqd1HK3}an1V6q79sS)mS}YHbSziBJaf7ume(gv5@|pU zwfTp+cP*UwzoI~sRdSpUP6!k+Sjwib?GSl}og; z$iY+!;tJiPRr=|FrYj4UK&Xb|OgvFlYxjPXy>P{m5*4ZUCdy18xJzd9=7hPkac}dId-IC7k_F9&J=Lw&CGX)Iwv}=pN!;fm^X>0+{ME4^ z9$P9ubR!G<%E*d=x#+)ocFDbGxw1~)TXbRFqGb^~jPAeTTwWiTzx>8?@$Ef{^*wQS z-N&Bw^PUeq&4@q}hm0OgAneFe@m?TvV9nz4KkIqzNW!yj-t(8ff8|>)F1>kl=IG61 z_iWt89Upqu-7VmXHthP{iZT2c<8!pN5sOT4a{8 z?4c~B3d<2|)FS@54m-`_piEib*6b0mx3q#MA?r7{1!Tr^_?4UFGp_GC72n33_5cfEYv zy0Ualg|6F4r+eUMl+A)}X3J+C-!(a6M>LEC`7XhIrfoL>NaowaG?|Ps9A+p78L@|S zP@L4UL6e07fQeBGV#$QXG7=%NumXrwsO%T-VU%Je?G$qs(pVq~B?mTbjI|n=7P&{7 z_+5tB0R+@w&|PtI2?^<(j$X(B;$5PL;HrCk@)YgOJbd}ET$z-g{#&H4Y~XVsql0pI zIHL3wTO~Twv5?sx0_I{D5;dHsOKBDC4WKy_^U_KIlPXJivCB3aXAL~pn{p}XnSMwR z`sb(yk|>AE*(2NCQY^wG1tR=qfp3M>o(B>&2ky9*YR=rSf5(;PqOpHb_}R#hM^Xn) zCk~vB@9&HEosoB7{20-jre>y6rCSoETb4>&(zZX`vsBzhH?T!>s<#OAkrZZ0>*2=@AnYEy-& zB^x-{MwZWjVJZ~UpxjV68aAge zmCpxf^Jw@zf?qdL1>jea(|O23`~FS+!4$zL*NiLWu1&aW=XWgfx6Z(}W66EY0KAsH zTNg?evKOXqb>G@`XH)#_v+*;}#h-gVUi{VnXp?P73rpCxvJ51_isY@!>a2;f@=#UDF+6&qP-0dPCr2%J0qwV&thK_pMyY)P-&8;p-ezvy2p7!6Evcf0K z2wyN1nTMGz1|SPr^TE>!DWB${L==N2!C(-AEGo_FHH9?>D-6w&(u885@QOq+;B0^p zj-b;BAx>glu7VI`Ev7FtT1^~^UJQ(lP(CQtsygA#JyFSNMTI2gq7VYTaI3QgfJ zzS!5Sj`5~%+C)Ds6m7}GDrj7=>{Sq^zI;g7|G==`_sf)!Ve18gb80E9+&8ntTB><1 zh4h@xOW{nDeAQI8~6!3M*0xrwwLxxCEE4V(oHo?)Gd>nRtD zC!D@^@))(DEuLCaTAeCWv;6z?)zfZRJ)QFti)0N+5yExZeZ2A^tA`cW_bb_n-2_{H z^uhvyhaXJ24kcWN7y@gzJP;f40t1SB5<@SVSiS>2`nL7kHlt|X3|XkmEOfD`OFkZ! zZ;B_4=#>2u>KB)QfQ+px<<&YFVXdJowRRBJCTuP<&oj%b5yg6WI16hbPT-=Y3DTi( za3IvFjj0^Kf-_4D$2F(H8QC%AG-`CAiYQ?rP$`$j>Q7q-5i|T-FrDwxoWoUpu(;8X z&xU+fd2>kCCkf8hEgW( zW@(H_HWRbkt~swcu5e9`;9sE`jWu{WNLYRX;mb!Dd8`_sjIwY?gm$sHCN@Bf$`mw2 z-2<11(*?vPB6a4Ib_TVB#&6sUGe_ zr%akAen52#&fpi=EA&S%z)0{y7~6zvfw`zEkm=zoESR+Xrg}C|9)>WX>Gd0FjIDvK zE!1*4-NIh>(T&RF(xV#Pf;}C~n#_zBrby^j-iY2$rS0euafDYe7nnVO^2y1nk%lgU z-EFZUWqLa;Th;rIQdy*15t&UaBCE&%N>(B~#EK|QG)lyHEB@{`BKfZ67-s|)AF}~=m}CS>JJHf&p-VQgBY}Og_TWH3t7#899-&41`QLG$f0O&L z=+FXxW5?{tk35?aMTdSHASv{NqhZb_4UQgzzGoX z*@EXIV0g&|oRPayLB^y^8wRlj`Tm&)pjI0I$l#A!@|U;m`i|{J_n+j*B5uypfss&QTo+g*X z!wEq;+!MD_ z-tCl-0!xYhE?55xx{=}bDZ@C)G9X!^QWl$bkw`8w`u$(~8mN3aWJr-Q$Krot`=XmebzG5 z{oNcmhIsR*d!T9Z<|Dt`lPo&4=v^v0v{Z2D{nPj5sj&AXOdjz4KjW-DNVB*?@6EiK zymC$7cB@SM_IATtRVq*OU1>pwV&Wzv8;?ikCDjIU1J>ihp~* z98EG?!rS}Gx_nt_LP?FvB1RBU#PZOvG889N5d~l@wZmv_#;iqoX4qid)j=_RP-zc@ zFV{qpW;B$F{jV9r$(YbuYCc3BC0fqLXt|$(mZRyFaj1D2E$7O3h@WPWNI{=z7Hk(y zXdknHE;?0Z>NbfG0RpdY?E8q^mh{|4ijRWiPsyR-y@t^wyO)e3ohmZqt;ooTpMoJA zjUngbj=kmOpBYMNP9l0cg61{~YB8rNH#m$5rO`Mc&b8(sjd0bqs$r1!zj3n!JrS=6rKVZbehTo%NAjerk&LM z3XQ50QAE#-f|?aG$QeedVY_e{49lsZkB{4~p@?bI+BRHfBwKpx$|sn7^vkL_L6oa> zED@+gqSNTJSKcBAomb+_(idl;*crJ8zY+=~n@-=Ur0ai`bfm#L)15Jg+Z6Ad9l+~- zfBW^~e{=ZX#lCe=+=-t(h&lT%E&diQL`Y(+&1IRY zsD21lf!IN@ZC{Y2K$$M0CrF$H8Dk??T%Yo6NqDv-JuTCYW$*fwcSpiYQG>iY5Xo*j zm-cIVHc_#4y5JK>Ufk`UL%7+Nc>cB}N9!jg6^~Df)YPYHwkB$}rfPO4YIegj^fMP% z?awgeghl3j-F!>Dd~?zrc&F&E%6?e3xaCgWotAjpiDZkxoD*gx?&^5$o(~K5!1}|K z&af}Jsy?eXr872U(iso&3iue`N$x~$T-NgQwk+RT? z_nep2%?2gM<$gJXl#6=%$HlNPF%T8sLwyk<(v;OCucdNg^zdrN*wTu%@@BJTv}X1R zT0vh!^&rS|xRTPFM`n)1tG2@DWZJpnDY?0OW_P?Yko0Vsc7P5pEx$Q9GdTOqyd&vt z!GdxZey4*hpjKS@uN=O9_{Ooh;<-rDwF#EWC7LxCsO*wy7n_$&_Ff$D#6hdF?OhXO zYwY;=ARAzMNU`f!0uHg0t||GH#DY!Mp~dRo{|`?s{`o=i6mB-HlN|kQztjGHDW{*j z5hq4MxOVkZ5VH|7BEhfh{(d1m(BCf-2P+y&(h zl9wp?14?En`InTuL&+i~cPRPaDUm4odrJO}l7FIvj7CHsCAE|^P_mJdc1jLY@)RYc zHD?3GzOqO_lqE#KMgBdKZ*jk|=JAeu)tmW^_XaHueEGdo7BAmrxwqBM@3`mU`M^DI z9)IxO1}EQg&+FoM->b~y%kEX@^CkEEPQHqsvY+02{z6=B$mM%2cPqIwmS$!$u;*qrnkOr1KLIF^d}aQj$+u z7`9~}CH+G^aO{(3-U0DBWX69kLP~Mi>{%?o$;!6a?-z5HoL_Odzv7C1&3S*#wcz^K lT;;F1>R)krzv3!>#g+bwbN`BS{X^DZo~7g-M=2YJ{|}n;a*hB1 literal 0 HcmV?d00001 diff --git a/Robotics_API/__pycache__/Bestman_Real_Flexiv.cpython-38.pyc b/Robotics_API/__pycache__/Bestman_Real_Flexiv.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..6b45f9e3abfbcf56e19a11d9915742d44bca6b9d GIT binary patch literal 28893 zcmdsgdvILWdEefB@8U@ie2SK@C7C2-3Zy7eau8V}O_4HX5u`{+vR78ui#-?Mg1|0# z_ktvD7ECJ2vHFngbX>QIBNuejG)z57nx>82x=rk+nLgac%{ZCPoph#cI^(eIbkdnl z|6pCSzu$N6b8$h^>2xL&U~um1+;h%7=lg!&<9z4fm7$?(20vfE(3pSdFJ&@+$cy+- z4i`u9*L@d>%G5H-P}xPJX2>yH%gQlV%d4EqFBX;xwSvJsg~j4hsaBTr>|&)>!M);Q zb!nhBAoog(gG)oTA^-VsZCJ_~sg25UOKpoB$7*A8+*;fELPnKU<*kgWT+i0VRrY*l zqUzqNK5QO6*;=%w8%vg{f2PcjT3u(U(XOAf8jJOli`LcF<;mq$W}Rt;*_e6CJbdWz zlaD=q__0Hen~xu!dh)5MBZryq*ot$ZWBUa%-zOVw-aoO_Xf1}faeq~{+Vh8x9G%7C zLSxD4v?ndK!mQ6*-KO1Ic3PdbuIiXM+vv8MW^=L8?V58aUN_COJF^`J4;uCoUwoz8 zm=C^1+Bn^5Th&|W{>=0(1L=g}< z!Lh1F)fOBF)R@|e+i)CG+tm&nht*EC3&#<)TRni|sM@36hvOFYpn5-!W9pRp zfO_aHqqbGOs6MFn;%r=fTa04UPU7q#^@92s-h5b@Ag`Cbttph$_-;0nsd7k<`9tY+%vRg6?72qMGOx6p3#N0y zN*UbAs(#IJ?AGjxV|AzGnnei-Oqnx%@0p$11*_?x_?FXZEVf)r=?a$Z&gGWE)v2>) zLn+(pcCGGYSlm1aZ`rbSf&N8M774G}X*+gjaS=o-ZDsZQx~A1urZqQ*8asAW*qm)$ zTCv*ARb8C_t{L7IQsx)SM?~)}cIKzhM>xz|c2GO|3OCFpG#FH_*+5ydEA#VgO9Ks9 z>fmELHZG&0mO9E}S++H2*=UMoqMTSKckr>o26$V=J3^XN?}A6r+Z-@_IG6 zmqVJzlfsP*&WcWEp@d@@cPetH>I^Ilt`A8%?2OznZWwEMxf<=|70OyG$kmo!;fB$* zoiUZ`6~2^tFMHj1({Q$~kMllC+@|vL5GnX8j%Rjcc(o(5R_qmfnT2ij?W&~8Z=nSX z+ZJ{_pSjLAb~?Loz4JQWxR|rQ>+DvQ;Levas`_4zH6TZE{Q)&_!+;nX#9Mom0lpt< z><2uC%ycSbzOHn49JPgFZEF!Uqu8CxfNBh-&*|jsWA=QE&v{2vQES=U-*xN>JxUQ9 z2c#FTB_bHUzu0L)tR>%w+=N3B+2dz4;A>!U_ zKJD(EoIH380OTB;6IolgTW+WQWIg1M?&ONC=G<)|2bk@SgC?%D)s%U1<`#*qNy!%c zWN||3JCDEaB$9cI?i+j83@6hw9OHV{&i9Oktjb)>cK6DeaZ&F5RQ6IHBbcp~1J0>c zBEHFa)mFFF28#ni>>|poRfU!6&g!ytGwWq157i2C=Vrzp#Pi$F9K6t3vJS2`+6O5M zPD88EBXjVamfGF=v9qV-*+z}St3E@9CS5#xil0JNtC-CiRim1z{vY>Hig=l<*NOJ^ z`s6b*sNH9A5)|S-a^mVT;2FgBbbt{W3l5l&D7s5c_rRoi{DRfIn1mgnRUdJH{1?0R zrAGJSr2ABy)HZ-xccs|`SkJ92F0RJq(4;%!!$%#UGU===O}x*mXaca+#aco7$14c6 zc_m*6Ax9uhyxdEjc`t{TYx&baZ7)ykt_{e0=LNC7L3!K9Z!g>FdU-;bS5hsz4UNw$ zHDFYXOZ;L309p@y8?SyT2Y@38t8zwB*NtW`SH=t`0AosRx z=q0ZpJ?0H15F4$x+Fte5#^Q=4WqbKnd#>Y^@ljIjM9Ir{t;IR7tc!0g^OLAf++WR1}~`NCk%-E%j3OE4g{eiEy||Nq2^`zruTh=3azdj@CO^_&8j8wB0V zdj3Yn$T)@dA|O4u$DALNoF$dx-PZuy=b1mdURHp0d6(zK@Er&Zlv7ou>&Ek$Hx8^3 zpfl?ODCrCSdu8U8vMT3eHD0pic}`WMn(`TE@M6{e??z@VYh+wga+K77Gt`54p^(ve zKbiRo)ZX(D0BhN6*-vCXnQ0?cgCYnn-c1n5n3}VzPy?+76&7=@v9joNh0&~bV^;L5 z78Hn3mdspeLFZX%L+-;Yu-e@fsBV!EH}N#oWg3epJ5muJ2U7?2G}^1C5O`E+1n7q+ z%@aZ8DQejhljds;sFP^!O51@o6Xr2zEwD7&6Kg`O?dL3K#cuZrv)N8(5hBc9vCLLX zXX1x!h8zdXla0kLR2o!lUukt?`Vg>)J!p2C!WPOzC+S}9TJ~kjK43N+$6A6>h1Np_ zl#ZXAoU|!0+=unc%|@HzW!BPo>#aEE?OX4?v?)d+RbRb-dSo6iH?$TAlUFb z2uK>t;fjB_DjS1Y+{=y{+YR@TG!W`9H~~f+T@QIC zLjuvGF(7(Z$|)-!h***UUxi?DsqME%V2IBq2HZD7<`?!QI;;jj0rHNxPm{P09(MTU(xd%2lfz7&VDcoBBTPtw_ESurX7XVq6Ir1#`$u_$id9Uk4G2Zo=g9eQ<5AZ{ zlF7%(DK7+61rhngm37~rMnM0*Cph~p+&f7U%8j?^p5iWZ{` zPGA`NI3$V*L4VG2nim9zE?7&El_%Mv@tFp9k(X=3^OhqZp;a4uJ8s@Ibg%Sv$kr99 z-e)l}NW1IE-sKo(^6tL0PfvMg^DZz^PSgbp!W43y*HT@)YdQB=*deDomlJXdt}4MV z2s@v3&HYrAC(LCyrlQYxiyySxcLI(=&9)T<}z zCq8lF_$xD~&P>-vt*cfOcJEocwFKw}^dCk3e#$A$cj~P5=kcs7)@y#$a34tP9zSQ3 zR1o}75e$`_FW|y`74}7D0j|yK+0X4?gMGW6?Lk$5cMJ-TlXnVG9Y8U-lY1|()rTXQ zmow)wpWu7N^^$^Jf~Uv_1;fwL{xDRLGE|Xwp^D_8itLuW6;(ue`M4@_B%?}e`D^)5 z2`K?V%B~j)7`h~AO=7tOUnWtPQwPRaUjrh@JB}4$3|O}0z|(V8h$hI zg zimwhwd3j}d(dr&BuRueyOsmm^OOv-S0OCK>$hZX7Us_obqFZWQmGjHAg~a>N&|a#D zZYGLaf{2EazjV5B)k6wfEzjz`oZDLVRgqd1#noqNvUPFZYWwH?gE9&_*UmFP!*8J{ zyR_>wr9tQ&>AFV$OB)7;?g)_-vp8`e9j-%0I3g_;#8dv6>@IkjV4@uT6ow0f1k^azEd~u+l6_c}n!Sv`6JIs71FO4k`c-{v#1QdPM0GrR@ovg#(ZNKPV0H z@rB>^K1NvP5Wu+deyM>on@$KxC2}8C3KT6y2Dpq@CiQZPrem}a$ z{s0m$Ph&jdw?LybS{&Q<#r|>IDn-@NY(09X@M*t;ax_zw%CK06?+oU48|k7hj2Vm^$-Mh! zIFewr7<)pwg9G0@^?YLN?q}qnp=%$ZUj({;78XUE1avXs=rn@JC zQ;G6Iltt|5d9wg!w^qrj9S|07BRXDvC+*#UT2;1CZn5Aj4pNg zg8S|!qpPfW+Y*Th_m-vL@ZbOZnUfPKgA^}dqcsf%(_>nBx5U9kz5fjbb*bQTWMSt; zW1Fsq>9-OCn=tS8qa+7l>tLE;0Zw}UrVlCxBEW!wvd6u+k zll$R7il^5xG;hR3WKaWVRcON{s)!BIu80{m;QB8DQ;YT)e62Q;WKwHygZ`xO)ujjUHa z)mGM3?T!|cnfhvgsY6b4xz1+PMXIK0qk|muAX^FS9fIp{#DJ+`5b+l`AL=&0dVc_~ z7;;Ru2p4j&)1Z}ueZe2>hse3~1t$l4K2wAb7~WPyZlsFNFK@!jfdL-qEAvoIgW(S) z*uBPDcv$AP5?q{G_I}K(+6~oew7U}#DY)B@`%}caA)@JG($<|wp@U?(K~~a)U@+m= zt|e$99e}+-&HTEAR3zTXA@%F?0YK;8PP&Moxg1f(fz8k$t(XLrP`3^N;ayV6iyNo} z;GnWpk=FnVFr5uK&pEkXwwJq}UCV<$D57&cs>yoBAnz++6wz;kju1m69(#)DJVbOM zZYGPz9@daD)(Y1Op|MQ+sIaMh6pRS{6C0>!L+nGG6gP_LvnYeIfX5)P!M2v+$_EDd zE}?|gi}F#gnB!tCA-n-OMVF5VZ2A`A)_vf(PiFCG3^AKQYw-33zT*Ij5*7LR0$DF& zz679POKlb(flxoe6}4qtCaj>bmRAm%>I7&Ip)TMl+J&9uFs#4N=Wf(-URvw(93 zqNu=dhU;|HK+m8t_F6_oTa$(;B9U~;YhR_d-X&KW4UB5z zOd0@{Wv=rhAqwR=@6GYA8V}N8k~$I4|N3-O%y~4IKSZ+u{E?EoFw`=iHv}epUvA*Y z3??K&62%2fCax8g@fWmkV9LE!?v)lQj2KzN5YHPuM3<~pdimvAuMC(f zEYJ2TYw+c%!o`Zc+{?BxS?T8jl*!$n?B!y2b50c{4&ZMPKv#8!aBcLebl!jrUxK$_ z0Dpt{8(JIg4fO`|_>_x={hi+MMWg%GHYO^aVO8o4sIvGHD!oCFYPC0nbf7nkbnv|j zBUdhcRmgpWB#bB=wDkrc6Zb|ztV5AD29lnFUAyIC_49`PDG>FDPt>F9V`|vh+8fzG z)FanM5=1?+@;9NOhjhJ(CzvX57oiEQxfZ6j5~5lHPdYN+1Pl1Gg$eMMvnn{)Kx_dV zZaxr(b!_xg#-*V4IKHlhIUJ7&v}|!EFHvb+hKoB&oLG`2;w@UtzV{E!N|bv5lX!Hs zG!`S*rr-1y)DLR?u@cb+nZAJIreo68l3EA8_MUabd*p|$=Jc^o)L%XE(&lx*<))o< zdJOLMA&NdaJ+mPgLiFc?0)N4@1@lxyjtJ;nV;s* z*PlyBiom+uO2{CjNhr}Ev{~cr%y>oz3`Is2j4lF9Er-FrY19gh#pMeP{g^QrHk}xC zLK-DdF_X=>N7Dq@66Q<9_u+IBI$2s!&pb-P&-l^l@98sudhtb?F{Yl^E}5dxneK?$ zL68rO6`9sGU$ie z%DA`P*q$4P_}dN_r+KGXC>mMNko?}s=eE;SPgTx0tJGdXeLmUzhv%9k%HXGhlTbza zC0w}6LG)aAT&qZLLJA_LG6$A}KuvJpzrf<%G81F6|7^ooeHL`>%byx2%f3o`F5 z*OgugatG4qMqVUOb$vh;d!<07_mwGz;QRvQPT7|`)%77&a)x{5KDkp~D_<*za;FSs zs?u0RkA^aV6t4Z^3B4HPo)kUQPGhjKQIyPg5IxCU=6=wkMRK4^A zuNrmAO)O4eG^M+TBZheLSWG#=i5(0>=FvV->d(ee^7BqljMyY*v0uE(3<}r5cH#vI zhRPeE^${~v`tfWx@$C4qb2BH-pE@>OKYfOXRzG>>-0>6MPQuf1=)s_ZM*B2YV|3al zJ2rwuLvLKaxl|FT@+u)YdBe$(_eOXf8#k|bg|X`w>?S`(F|n9LC?j)@f->8L;c5Cn zfP_`RD#>&o!aE4Jp)QS(8|u?#z)c7w0wd6_Q$2pEr6x3NB1VWB-^PXe?ncz`v7#tg z0X-U6Q9>+s30P6yfE5LM*2jw==#NMNjWs|Dc{QN;M_ ze&8n1SIG&1Cn7=q`vaO59XVnx%%z8JgFevl8zgh_k!I{q$H%|Z6@Tsl za|!25IA3z#Ag~cffoNcG&hp~BV#_V&Z1`N0x?hso|8(ITQ8W0FDeI`3WX8y6sRewEsLVYg-w{)3|z^ z;Q&qNHH~W#R=&ws`gFUh*6ptI(N8jYi^*q@)P|E|bUy&OCl$*4LH-hPaxY5GY|}XD zqhuCwWX4!Fj+GL+ks618w+a$rL&2l+VMbWe3am zWC;ymc^xhfI5(ySM$PKIysu6cN7CYv^7rq%y6=FwZxsocdY`+m4@9Mo2*`q^mAY;sq(3PTYsS5K-wv7D zGp@=^tY@rxhU-3YstpsZh0jJ~5w^)D2ig`q2$T%{?VWUY=zdft`8n2gZsjrge)QPw zO4v?!cZ33dSNQMIA2@2ykpn|U_l$E~o`Gc%6gjm4ImK(2t)>`pGFyoi7hQR-y~&?t z2@&6?vV1Mirg;VF-r8U=$3XF?Ih{R(1v~yq*4l8}x*}C*ExT6cwd79Il!o<()kRlo zF8?#KK21)lNVK>HMM>H&wioQLY?(o%xF;^ihth2KWPwd64B=0L)&CQ2yFcf%Ivvbp z^&H&Ga1<*8UcJ2zqJ?Cg85T1p`;p2^7Mc76eq8f#cK4Ub|TERP8h{|xa$ z{uw4e{4?rRh%ZE1UXWO0xvoNEf{g}CtnB;$$j-jr^q#-$LY&%UdX%6f?}9K-GR{x*=Au&PuN>#cF3qyChWB`Kha|3@ZtIaW_=r%J=N z8(7LjfasWAEc13mj$-{T;;V$Qv;!rmdQuD>-^l*s=iF_&7c>v*tYAC)FSCkRfQ`Iv zMdfLh`!sTS`DSN%RR>~_R3#ciJ5%~(?Ik|7ncRcN?9a2@@qo9aRdwwF^0xXe&iGCH z3JdKrahR+yxy&Su$42$H2#-lAe?%@jgzT9C;WF@4*2vz;Bm9=`effvLd>%dz2w&PByTF^5UPQjaI;;^9(cnc@_zY$M<~X4Vaq3^oUAb3w zDq5U!q0(9fZ)3$PtQOiXCC!8Ica&hAWc^9b5%A*w0xwo!zwD9xqpIv|!8}1|zr2a5 za!fJh*Q(d5p#{Sjq)Owj#lyA;hgVPSNR^al#TjsqiPWg+_hA>sYiwX`QQvZ2J|oW2z3c6|N&f$R)#H@mW#M&4PVS(v=it`;!*9!GT=>*^2{vH%`QX zJ+=n-H6w8tp?<@Nxv>#O5(Q=!5H)07waGvBQ6`~_K|I+HL%3;AHvEw8kETgO`Gzjt z5Y50plIFyowi_tk-Qg2tI345nrn{9G8;v9fe$qW4ejM64Bw`ATM32SaK)BSHM$Cu! z8H7zQ+g`2}T==27(HD!)xi$t$#A+!OTY_xAgI9c|219DMuG*htNfEFTVET=gfyg0U z+rl-l32%OFToXbNcjZTD(VgrK$fsxwu8oF$2bZ+e-DGRVVg`NiK-9$ugpaaO;u6}L zG7=*kKZeX@hO+P-YYZL({^su#fyGz~3e3f_(Cj#HSt2e|*N&u=oUm{h1Vs`$j=%2j zBXPed+W`O~*R!JZNL&yX_Y+1rwF=0TbT0c0z&gB_<298U%)UdRxlUWB;J~fCP~kNv zG8Hx-c_$C+h*nAwGxnFA0d3772cgxsRy4E%kg+uhBTRg79R|352jGgRD0h#%F`@>Y zQCL3x;F?`4T`Ps)%Kc=98ehLBxJEP_15y$ z7bZqQ{UH5vm9SXb4#Al-dbf3pu{7M4x|+?Fq_Vk;lGsP%~Jd8A4AeG&Xc zJrIthyM>*k-_XVBg{!wD$Z#VVMn&|!1~A+rcxJzgFRoQ!lKHUm*Lj_@RxOQ{6a-k4 zesOuVHirJQupvnd5^E1`xL4<__fpH18F_Cz;j6`^a6ZEM^JSv6M!?x;^_4Yd65+(l zT^kYo4#ViTvNqt41bW7*jr$A863A-T`E~?@q1f`aHUlw4nIxUrJwg~1f6p*h4#2n@ zH%3MM%o7rg?DvZ$dds;!esd5^DL0{q7SSRSn1J9>oCKC1R)$C*5%kIZ&?j@gP6;UV zqdN}w3W)9)V|0gJYj|l)FR*JM8Y6O%yz2KgNL_0taY}&?L+>r*J8A=6FP#wYw&Y7n z^=*TG$aqoM+b4ApT`>G{&P~IJyev@kL_V>&WpsNrSR0Q;Sp>i$g;Ni@nH=-$#T%I$#!mb};6=|NLiO{7l6DaK zdE#@g<@Ji*7Y9zhBCLvncG2}pzR0UWG5C9iRHR-yaR_7&)=h1ZY~AF;y^< zWX5#Xa;{kLVp2dt*vL#X7j{8?T=C~QBQX|v4BZcgb{dR;$zJ0~yeMXmALpBu8dZbA)54D&CNc&Tkj zh}n~Aq&WxEzV2_UlxC6Jj79nrwss(B?JMU$s-@~LqOgAXI*fZ!L*YTdb|RMT4$auB zlnCtXDYKRNmXWdl3bJxnuGz+l;@VNK#0A*U<8M)@P8bq$*7~CyDghT|ZYdv9WcNgeJsy-5rFx->|Fy$}g@+>ing$1Rw;HEgc} zeHS}86`G5jjNmAGMNNIR;(4J$FT1oe)=ga&6fZDAGZ>M` z^#2MjT;=P%z%lz(2MdER6=}SK6`0r$P%rGr;+R{YrjqlGbS?-g-9X5;pF^TKn2VE> zMy8Fu_pl_SqPK@OjZ0NUk4~9TVcKaP^J}9}5u&1{wKU{Gc&v zkjM&g-iWT#5=cQtZcAyeM%>QbR5@f^i6z#Z1_|16K3xUWXWBo<-r07HOATQ@fdK;M zoSbx@h{<8yqR?K}QT zAX`jy|9xE623lRet(K~lgTBx^xQJ|@Laty<+Mr)6BsSghUayyF1 zAnsq2S|&Pv4a(>^$JpyyW6K??B&V;LiAhMSMH$hF5PB# zAnGfh{hkcorQ{A}I(AN@P1;9Y`zNp)tKD@b<143GXTZ4Bk4wTkwfuZ%rW4~JSr!v< ziQZMjE6AYOU&A-qUuTlWC?PA&cbv{Q$tB#7C{xKo9p$R1?c%xxhol0vKeC7kbUF#+ zlw1;W#v(4W`Wyj&EqH-bQ-~K4UWi9~r1y z(vN-f%eyZ^-n2MTVs7T_e~592$idysegR1b>=}H#r9N^yM#~@3q9nGSpFAAA1**JR zwEq!H-pmDe(_gD~4?<+A0bKtc2~qqc*wbcIM@OU*VEm;&RRoCH)QA9&6Y0_^(BHJ? z&pHKCblTq_4(yiBNToXaA2XLIF1unH3Og@GZuUQ6uHRsiG*!GIkthKT{b33XeIQlL zvx4BQ@BP^LFTL;|-M5~*Md99L68z9MAg)mf?b18JS*IJEF_L4iuoLF*#f<$Ip^{=A zN@RX+LFSI2fEL!or3Q1H8%1LWCB4wk(aZnv-wLq&ugrVb4f`7I8dwxqkh`VuZYJ2D zQ~~9 z&H7XV`GmT20kcCFI@oD2Tm!+PAc;FyWVx&U(kL6me{4}*vivyI9-72|Yd|bs629eQ z?_H^u8uzO7AXfgTRoTBZ!?$7s^@raVh&Vo?^1|IT#Z~GLIj6JOb^dOf$$Db@ghoF6 zRM$q*<(@`ZlrEet(1!M1T<#kTKJuWrH1^;`TpA3Nk&UjXP3#2E!~zbm;v-e;3&ohb_s{~g-iP` z8m|HnE@c(I7C#VZ={b-b)#jnz4FXFB=yy0``M?1R_D3c@2MZI)j=Vf zH<|$*Xf(n(bWl^W;r2hrWBa#|1Wl(%hfzAxN9|G5?SF|JQKR9Nx&5GC&=SL(#HP11 z^WVSuKYsW0?5Cf*JvyU_MZ5K3gPxoi;1=H9zAY^94@55Mg?~Sbn!OUX zypx6JPx1<`7X0%E8~YK+G|peeJ^NoU`BzN7$}D;Tx-R((xKZniV$&doEp0jH&<(~h zDn!O>q>plTbJR?fmKle;7xpK9;Q%h*DdrwRpjtVH5DDy)UERbE?JMnjUwiN~|Mlgs zeR<;oJ34@ms1F6Iz5DfG7xPSCXnQsu4VsPCY2z|dRYXWegS0;YMWMM zR*n%9v~VvS#*^WM%xa;RLJ9KP|4*5PO(kf>Sz&E`@`}%~T8=Bju_`&{*4;RL$M)Z1^7BmoHk0o#Aq?C97Kx9+wUNHcYdZtN?I$?F=h$ML-$f}1 zWz*Z!$o5lYY5NygL!V(V7$PX*@;^#2egwG)#$2vA44<@saotPrXgdj|J3rz#gx?}c?XN=-dI@o@qOOa{Q zIM;z_e}nm|j5M2+XgmF1Rw{A>0^p+jw;lUenQKV)4Q?!A;AU64_TOU}|A5H|6G1qB ie_F?Q| literal 0 HcmV?d00001 diff --git a/Robotics_API/__pycache__/Bestman_flexiv.cpython-38.pyc b/Robotics_API/__pycache__/Bestman_flexiv.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..25f83aebe426009cabef57b1ede516a032bd86b4 GIT binary patch literal 18731 zcmeHPTWlQHd7hb_-JM-tL{XG=u`Eq&#p2o&CEwyic4J8tWh=2Lg_3M%9CtI?GbESV zo6Za+a!R^weL_zPRX9-|`j&E&5Uvedt3wD2f7oNCgyVfxZO+-0%O- z%ow80aa*dJZs58oSvoY2jcXn}I zZ%j0IJG1JS;Tt;9Vnr z|B!PC?-v@on~yk;=-SJgC<^NxO;~pf=djSPXqD1E%t>3Q=uv)d#jUqcIb3hu@}%!o zZ`4~}(+%piYP%EEoAnU&HubO;+X2_ZL$k9dF0}l>Z8T0SHN5rut*R6^PBiM*tDOz> z+|_XjQYWaQPvH4A`dap9;^s8|d`;IhkMRne>cV(O15R_o#MKmeQNT5?M(-3vNtAJ4 z6eD63S4)hEaa>Dcmzcn{EOv`YTt~znF@@`>*ef2wbxiCN`*9r?2gE^KcZoCNka*-B z-I)+)#bI#-wcTP)JSrYTZBm>QkBeid?Gf{0T2xS*5>JRH@#bDJBaY+xkeC%GaNQ?P zil=bhFP;|9;CetjE1tvkp!kG%9@mG(C&j05JtWSH7sQKz^oaNk@oBUi7B7j@xE>J? zV>Mom_MG+npy{@%i=Nx4o@bQS2uaD0h>`B+@aMNs1lqQ~sc#y0jZLizazvTi%%L># zB!8Fg3xT#;#MMGeiCfCS$m;0!STHX1yT+Co?Ao5#H1F!@vs)M<_YU4#-Mu<_PP;>G zdxVK{N@y5jej$7ui+;xUy&zrkdf09~+oczOc1?<<8dfy@8K}|Fui(#r8pSd`@zxVt zdZ2CUfxc?2=I$8M+|*aWK{pKl39jikxa}LpO%tCs97}q)+Ksiqb4ndq_n9~=`KVm? z>qLaDnirY%8=Z~FY}8vf9P8TRx%015t-RWJfbKsX<$)tS=KupPojtPZmxN)+J=*RwY?nW@UmlGYzv^IiP9P2_pUG-Mx*?8 z%#@MQ_9L_71}l+CScyhzYf^f^m#9lgE9ZyQ%Cy|vPYA^2m{U|hJ zTrc2XS-12u>Z5e`p=E}L?uX|ATFe0wv(R6n0%8Xf-PPn})Qs&Mm_pZ8t9ciKDadaZ zgpsyU&vV?fD00+FjKTMo02A?>#w`*4$RB9oW}B_1qIPHe`1|@1SdF$MAc>+I2ta9!}1;-0N8O<@m#R zOA@=^SI7FUhRe*thm83 zlwSPKQx$u~_3c*MUZSbb*iTnzIM>_lz!NrDyKSFYLqU`t?q4Lw5g!vVD}E#V*tL!zoy=&CIXABRn>2B17KUc|tbebSJdR9LV>k;#*-rSLDJ2 z7u&Zy+m%gX1Cq9Nw_$hMzBd~dUIFz@R~9Pym>!_ZNK(ovfOhsREMBc%Uc7Me!qp3} z%~$8&n4i0L^}?kEXTn?eYEV9xC5#~wb#ey%`w>uFZdXZ}T);D*HMcRLhYt^%cHFZ@ zYnA-y4I)bxP7)O;NNrUY`W@rzU)X|X-!?Y&@4}eegq}X2#Wgdn<g}- zq`J(|Zon!r6$_4f)WLc=bDj3N`f-ja`?CCcBH;Esx^c&3zoA}VL#o1|v?Q9D8^ zC033L02QNmTp!LJnch9rL?fg>M)4DZY_%LvRMPo~u!hXU*QtblhcV>`Q7)`^(nSM2HB+aA&&1Uc9-X@BU%Ck*9DZI^ z9bU&*d<%tE9EAYH8Xg_WB7LvbSd7qX_|YN~k?}%)6BVdXG8xYY;Ey{x6gSJfRdyu4 zY#e|!dro`n@E43NV=IR~`Aq}-Q`pR{n%~vlH|dR=M_aD|N)b@Lh8|WA6jS6n2EzY*~)fkoU)x9;DkrcKziQi zHCT3e*x{`pDtWE7CMk1I9>hAxhfzc(4KXGed4^i_x-8LSvbj~kR^DC)5|anv1J?0U zTcDFq`dTZJB0QuYL`m1MGK^A&!sy|9Qj0`BOfEEpF5fd}-%jz?%h?ys{mlFfoSd?+ zf?;8iLGLVp4F^R6F+TlbEvp!>l)f=kyOfEKur}4Y2rvVsRg2-_VHoLl-5Cv}@iw(> zjm_McTkcR`Ko7sY13HAaEIk-+OEz2%Pqdr%^tnstD?=uj1u!V{QXCBeybrESc^pHA zdjoYsW=N#p9EzwS%^u@=o3KQJf~aT*HLQ9LASu-mWI_cHCIMV*_^CSmV-$TqJHw1F zsJBGD200tggIUN2lmFk8LIRbfH<`vef>mS%!ZPIx7&?4Kx3%m^GQ;E{Du&rDV_gYd z7wdn7ZV7RHY$xL40e}+M2T+%?**M@3n~?=`016gpGs5?FK-<;1%l0%m+V&iDo+s_g zHz^Km;=&? zADI>9dBURdg`!bK93VW5tY6^oCnUySip9R__uhM<~!hXE8H^KB1G+*?}mYC&6OH1^dMIG{^wEd)hKsbB9J zW&i~D0ga^o+xQZUwnXz2U+(}AN@;KjX;7zF8oaqaV{gpZ;S6kt2Km$Vptgc0_x4*h zn|(a?S|^<)q(i!NP7zdk8L*-;f>;gORc{Rjc;eZb)Y`v_k7QR7gRUl6AP>R&0613$ zUYXQ(*$AKdsKp$Bu~1`L03`YmlhPkA={u;9U_tN`VkPkcw@nqVL0e(92&=sSH%Y%? z$xjGlS>G&7XIixjI_uhX^tj#sNM)oZW=5U*KEjVWS^88X-XI^0gO?mH_RKEjL{0va^=vMiPL z7KG>5CE_4G!eAQeM!KTzw)_ek{Ol*FF0r719nm%aiKAl&?3UL!WnV(fzuj^hvv$TU zVF%F0^L7m+9D5R7z(b+Awc355;kgG=V-`@Z$xZ+-o_VAC+Wf1-p`|X)gAI;gotc}v zKdc8E+QN@Pi~Nf#|NT;2`L~xUCFNAj;yH&GqDcxbeCE=H1q2x`T|tcD(t_ecR?lS8 zupEtc!|xNHAq5D&WO-33s))<0=#x6jSc|ld;YP(6NveVwMmbnP{j9sciQov^k>KSF0uk zBp|G@bpZ#Co)lN%hHVwW$p*OeG;?V&=45JFOk0SPsYNi~QXk@EYPltJa5A;PqlXiC zcFW>#A~GxA3(6vYNB(W%tN_D#osld-YI@#y1zY4z4zID6k~Q{^+`cyUc>w9@*Z1fxsBa0 zGMemZ2zw6^9f}ARD)M&fdR-ui4Qu-Ld#W-ZBdL=r4W7a~p<3pWwojUu%3c z;JDikke`}8*@ev9C7;7eE>wz9ehoWI*l$!*gaRW3^|48GkkstlnZ>K~S1z1cs9wB8 zineU0rTi0_IKN@^KDu45CSFsh#nfgI?;G9+M->3(~SzZv2R*i zi4G12P{j^yKMx&dBJb&sDWw$gQ;bKM(Iwg>!dAl5!_4UW17R5z+ayO%*NT9d{750E zymAgVDTg3qppQeyC@#raZU?6*MU6hiO(A|f}EvG1%;7iYJuAjR;M|&o<;B{tPS;G^&Nm4)xDoCeGkBTJ} z&Uo)koc**xgX|NeIuHR=gB9Gbv>QUM&^vwl^5ges(-=L-5K~zstT=OIg@>Q|*_l3u z)}tAT^XZQ@<2ou`%_vA0k}*!2ai}q#X=f9d4e6(2EHWX4)T4u|ld(tmmFH=3 z2z^>H{KSJTML&q1oClIfv;ZuXNX$|KC?$V>FNbY+eKU8XC=bzoEz1_P93aP@Q??+j zV6o@51KOtXJd$2s)B>{VA!%4HncF1I5DJWK6?5BC?z<>J1{K5qOxK!IT0M@W!WQe= z*!?ke`}&6MCUHH)+&FGcjckqH*W`ym?I%07XQ&PXT2dx0rKwQffIWR|{n(6sYy$=D z<%Y*9-328zsy?QS^*$$o0YNp!eY(NFE(G~BP%EFI;#m~P%Fx#%eIwGt@Gy_3x#n|H zSpO)H2vmy7uTk#6JS9WPO9bPKRJc_1YFFpT&Zc+u?3pX`O3B93!z8Rkd7hawPB+;6 z72bNa>2{oQ_ud&zLjGj!J66lPt?d3j6}wMiwAyP>*E*+(W@#f&vnZ%#!S^JQQl{jb zCx_AS4sd!z2t~9~b-IZgwaI`OvqAZUlrti9CQdyePLLyFT!?2OQiX_=NfD{ER^Uh$ zwIQ=GeoAqwJRDPwgT-W=-!8%2&O^Z#H!Y4tQ+{kd{EJ^4Idf3({^U$sJF(~Za}XnW zbIPyd07|7SFQO$fYwga4+Nw-rp?w6VLLe00h~pVjqV7y31d@$sI(yhJ`Rl~!+2+Twj_GrQXMH0^VOLc9AIJs%+tgSOq$QDh(w-E-qyD+1S(bd{uN4IQjK z4XMSt6I1Ki^c$8pL86|1;XE0!$gH`Jz3_DRD9+&$ZNz;jix+7_lmOP#Q>9`nWcma9 zqC{VmW9ZXOVd3fS8Y zUl_&&jCr5-7i&rUWmpdUg3?uhr*-oSVXn?o1E(1Gl^A2 zUa!a;N{IADXN2?EfiYQMcZ#l$fQ%aeF8h10(u=#7)*6kTIHog|O>8IaM7bUEEcI3? zZ&h9rXCj>vlox#=vXWJICNoxA@>G{jnZEDI78G|hlIfxZ(F{HzIncUi2;SO+ZCXKX zWD-%-Nqq{sY&4!-1}f8Rhwahcg=M zP!3r+dyx<MoLe8{L;a==!I}@47RVfdj8TTH=7opoJ*5H(x``A!+N?<<4CGs{s z%Xu4dh7u>?&}+`*z)3V;l3q+ULN`#CzePQFCFXAo(;10}jHUCOsW|<+hpi>Cd%wcp z(`8|ZD8(q>B0k%X&f3TX_-aa@FeY*Z-NX?e{QGI4I6S9)2&5h^qlu7wo5LRqP33;N z&t)lp&TZ5XONWO*@;yd}ogU1CJ8|K&6`M~e@E$)lY)GvV5p?Rc8^@t+j&~X^!i_jk zRPPWd@)1CLF6E@qZlb+L;Vau6s{aOAB?(Lq@m}dscM~E}t7;+QkM%MP!^)6NuQSZx zGEaGegoYg;#vX4@ZE=nD4AhzE|5pFXPvSt5LUr}RXRD0SzoLa_H`fXuAEwL(U+Gb` zM8hM=3R6%|ooj?6iyT5S&R}lgoMZQXAD;q(srD~hM)>_eR|(4T&NFgcumcWhDXe$l z76;h2f_G8EwIIr9G0}g75}H;2O>?^_M(Jdv7+cQKiBPmybV5?>zMCU6P)r`s#Gcc% z`(6;^NO=AsAWUT-d%6VVt)i}l`|9#ur@eS?scj zn>dpe^d%wL$%v9FABs7HTJ1VAQpt_Zi29+DG|dcDskRI<&ZWdVI;W(^Q0Q%Zx6~uLbwWr>^LnD-(_( zIIk9f5Apm5WTrZ1CeW$=4{*IRF^$L2cJD(J;l7vEp-4pzL`Lef+UF&MrRBQkC8^ z+zcF?cE+XWitatMXoEH=DTEqtEjW1^knbR^P6kd64{@m3y9K+_DRtpYLU}FHp-w-T zudTEfz2#>jePI7^G`r+KBnml-4{61th;QH!Dqr+#8RHlx@PGIBPk1t>vvFOOVbnvep41 zhEod|Tuj4i)5{aH2zGoGSdS(;8+;b8e)DD3#ToCtm%GtTWq$l2YR*XAPw*iei=7#A zI+z|~Z8;i$4Iwod$UmU>d)Yd|p=4&oM7aDbQ5p$1&9vAS8rP@HGE>-u{-_Z?HjK9V zV8}1y^`8AH5^m(3&f$-bM}k5$d63FgKIKjUwRdwmtv%Agc_BRCz%?bey%TPB2glA zJqfiIpZQV~pmTHSaAy(#{k>Sibf?uNQi7={I3Da3CgDSPM!e{@X}nPJP$g2A?6lV-!UNqu6;Q^cMOjw(f0|BPa|=*F8zQH!V*(C z=5qULn^`M9*A(TM6y(?ORK7<=AK#>$wcHNc|3geeuC+E^fV)0i04Jas{Jt)T?*kEn~KB< zM8`gj40|$D)*;KZ@>a_D9U>C=K&_7cKK>UCocEi>Mz#uYOVVf@{fPD|EuH>XBD0*v zCclUdQO@7+C23Q+k8X?jk2h!`qoRAAK(5f5(~0P6RUV*T6pnCqss9v&{>PmvcPilM tuLu3Ti@J~+Wy}miePq3;2OL# zSq zgLjO${UPTN+80{8+YdMo=-LaKC<^PgCahbAb6Dt?wMywO#-uG&^eBI3xz_9;bGX^L z;Y#1FUTb#Tb}eYut6n!~wwodH9@VfGy@2!Kp}D!^=R1B-YqgFyTJBo&MpcSy$6L*- z)$TfK?&{VEQa7lgPT=}=`daof@o)-1U(+?sWxN8Xx-f2Qz-dmHxSJv`3b^N0@124u zi87vxVoZ$VZixvoiF--x5>vRB#cnZ;`N!Sfosg7VM2z%&20y=pB+$0>4SmD7V{B+ukR#IE zMh>ZoH~BmCTnMz4BJLJSN?cM7##Y9+CW1+!-!V4LVAs~vhIvOvo!!C^x!Y*9vU_Fv ztagja_6QT{jL^`<{6hFJCjGSUyFohT&Cu&S(W4iCZdHm#9W$C<26EK(%lP?^BU!>H zUVCIy53~(E&{vF=+$}?z8~O@3=$hd_!a4mKm;JhN-NdI2$CB<1ueBPuPN^%KJ`-mp zAC;SalZdcWcO$cTt-BtXt!C$%W4*k1_S{R9E3bIXPEZZjyY4$iWXzp#@?7$cCdbj4 z@1H!r?6uwF>$T2tnt%(K0EPSGi{4caEZ|@9yw>yH8o#FbQPe(%Z*i5k+qzUp@B>rw6{Zz;;5ab#Zb1kll@bd1|wW)_UbxpmBt zk>UA~*{uc3kx3Yd#_Fq5y1&WT69G8IxoBZmSVls!_8`?=b3Wvn%%y^im3#XF??tcoSI(qW0p+-;YEyCiMdT z%DSbOksqh0cP%qKv=g3(=r9LJ%s$^j280dwV+-Mj&ec|1f$i0Ko`mgnY(}l^yAoo1)(+%)v$JFeo?UObHEGYy z&E2JW3?EW0>$OhD3+$_|dhd!^8*)3KHmDlfG5mh8cGFL)htu<&+Eq;ZQv6}GlEmtF zIU!XVu3ZkAt(JYWrrt+-@|{^e^^0Uoqr^faFa2 z7*|L5RT{J_28nr$KT+ioR8Zf47>Q<-LBnPK8`n*JJcp;8$JTTc2@i7sz(l^5Qz1$U~j~UG|$0u~eQetkQl2ZtqTyg_OsuCaa0ARkO z^PmR@xo4H3Ru!)+jX#g-KGC0MghxjWWJH_o2V?_IZ$K-|f&6SLz7+=aL@qpV!Moww zHQ6RMAZgpIwd}6vyK`aTMNr?d%0eX{(*yJvNlLj7prd^Yi&v_b7SCTef93ou^VRuJ z%%6Gr%K3{6(UiO9)}eSVNf<&T>f{rsKZt0%O z5-hK3V$6g|aVw{5ovFt(f9(VW7XAGK39d09bUZvFLETc=#2lE{cGrfEvV)ogaa4iW zNUwcgw^;Kz63{gTY%c9brNA5_j{_r)dHq6djZ)}*7*~D}<-%q+o;0Tfcxt4`Hal^4 zjC-dZ2B&U{2#4QSRfSja72iUl6~`eIF^3P0WSfCzbrvnu3>Qrz5gAY9Zy*D;N+#sF z06cR`hcah5xWWsI&l?9|-JaE6JNzkQ)7Z?RPJY7xPZc(DE9O_Vw@qqs{h`i_fKmjM zFQJCj2gMY*6v~5HD8k%F+}RIlMG(Q1!Z5`!gh?k*lcOe*n2HQ4$Sh0=nYU2ypq-kM z%^9jWEtdvJrc;AfJ?00pa?(C}73%b;jafYkVmq3CFf!>yXg^QdcKc7BTwo>+^JF{Q zp>fi7a)1*aISq-rLo--=dD!KhAS$_?)ix=3S02VZ$RkK1le!p_jC_eo^rkG)YqG&r z!j|7&1`?A8;R6V1~50eXZp~v^k**8g<5vp+OH11Bf#D_~eyY0yOrV8daNK#WgUta%mVmC_a?HBOoM2y;_yiU1=}nzk4o z9)*!!)$P$R!)Vyj*z}FLmS%!ZPJKbRFx*T4%|X z@O5d@reu^YGtraKO|kYmswKqrzU_#MI{->t8$w;mW|M$JY(`ei0Vr6Y%?RJz25nc) zT(XbR!rMLro##sX60PDZN<9_0c|kHN79^-FWQQRnh`L)uH^GLf>|${>5>4ZT zrl8)f63E}qV9DMNOG)KEBoTsABxRWp$bxr-R)|btU(XIQLt%*xpGfB=G8cvim~B0b zq9LmTM0JmV6U+f=#E;C1^5+SQYJC)qE8+m*p=W~ve?K8H{!%RVRmHCnLP@17nIFz> zhtyGxhY*|t92WL8uN#_@aw&qft=t9{|0$~fJBzFlsHXMA5Q>lbwhJZhHX3d{@MK0~ zUs=YYw6WTOqlA?D)t+GnKv+_skkr42FTrR_G(Yk6HUOcNhNqA^b&{pQC)Z}}^;tWd zh3(LyHFh(oFQce-^EI2zKJI(Hn~oCFA)UIY2r9hP!oOL>DnB{dh`mBSV4(&QOSzWM#Nz zvLef)tgupq)n35TNnh3EQ^Hu%HwrV_rm+E^4@zE4OL7}nO`^mRxwwwGk>`?@Fyj(} zsBKxa8i2DE@9*TXSRx+H*M0!QBE@QUC0eunu3K+5nswL#@ZmI4V~Ut!mdtg(iKQr6 z_ub`nA8yAC0S%jcS(eJK7KH0JC43`&!r&OxjjV}gt>ahN;OBLMsuBwd*bzPRpEx>R zfZcFgC+&;a1MoVv)|{PLmhb}5#(S?05{_3AJ-{QO)#~*fq2av?QezfSOOx#YUO4@U z>MQdvjfR#kaqg{i1ncyfGdsh&x2`RG7qrM%ocW*U;>^E4S1GAQ)l+z{Tn^LZa`@QA z^9yi1T)Ygw!^H)~hpe8-q+vN4?Z&!Ke2f$z_>$#CrN~QWNlT}6v5dJ$^XP6=jFF@= z7-5uy6*S1Y``d7supJ3b&yv-*0bBWA$ELuw;|4gFxHtJ3qeb#|;QIKNOn+MOG5>xC z89}_O-LHX5q4nVGfOWNEl9K|$I?tR;Z;C5ehHVzX$p*Oe6mw}Y=42{aOiPH9sYEc} zQXS%CD!Czaa59x(MUO?`^P3ib6W&_+W>6OSTl(jyH~QAthLz>wUj3(G^bpq$v6 z+$c~FuZQe=D(=Mn4fp1chw)>+3}kJE)y(?OxPL%0{r_>l{6$dmf_x3>5`MU>Q{Vp} zJC}Fx+Gkzx_g#hs5DRZ;PUdz0q+Qv?J0=72Lw*`x7$q=d1uvq~{94^*$t`PC@ne+C zQt~JgNTUaFb9N6Al)OrH>XgvBTe?W10^HtBPsj$95Z4ZhkwYopH(+ykH~U1%KcRvD z5E5;ytg|plgh+BXEI7oi+>Ab%8@+Y56Zr4`-dE`Q_H(j|Pt<9#mRw{s(2oVpNo0um zg+by#?ViHwBZZFbAEf3=vUy`{6*lrv?ojb}OsIGA--nfLqte7qQAlv((tMq2lYURaAr9F<4H&%XTH3Z`F)- zSxHS4()8dLTESq$s0s~YA8WL{8n#+!!pO|~H zU*{8CnDVQw_XZqS+XnJub0>O`nY-k3n8}4oG0LxEw+VZYYKTx^grEU7i4KyQJ#%{T z%KYW?rx&UhE|Q|Hp1ZhsWxovL9{H zu7Tl3g`3z*tuIFhM+2y0*LIMH4l|MW)yI%hiueKgqs-_MZ5&~H;qg&s^v*z7M#VPC zL#OIRz)UVu$SJR!#Y4&=h#(l?5HgB&ImhMT6s4$9r?>&-OlC224H?BoZNnI06#uky zjhbC>r?P{|v!IH=Ktw;v*NzP#@><019`%Ae^H`U`qu+qf9)SyRp20ghYk#tVbz1`q zw+2>jZRFd?x5L-$m$8j?(*ESaYxuQa`zdB#5;wdtQ4gvK0u$j9m+yc@a&{%Pgd4#8 z0kdw8wiHhb6-%~3&XQtg284t3Q0DBRoxs}K8QL?c2d^^Y$}ZlZB}oB!osxhONr{id znd~2lv!6C-5RHQH2O@xKu#D$ruO;LvwHa`9e_&@ejnRXMG0o=?HBy{8vckg;|L{y7 zL+jIw#QF4N&A5h4PcsVAg=CDAW*ljZXUf?GW<&b%*my6(kbWgVP(*4-mo^GLT^f{O zShK{qeoEvaAqutTPeiXO%LeAt&ot<04a&MeE@SQ&D&y~AVG^IPT`8NCBj(5$lWI8% z*AYuYzsDttX;!HAD68@P!(0%XkG%!X5#j;zi)yjjv&e)bbJ3VK!l&l?Wnx;#YtwJcjua)1{0 z9L5D{1&cke9ndz6rx6JAj24hp4@tvv$=r%vfw8S(!iC4OOB5i3is65ybIl>HE_+g8 zi}h^mL7%#Pbsd&|?AL?Ojs4bC$kynNCO-^nKiRQ8O?ep5k|Jv|;mQj?UUg z*OAa(Zg{lPn^01t>SM}SA6O(XAgIQ;PS5$*gCL&)YUM{Mc@_zxGW4}bUyt-KJk0%R zulgJ)HrNXU2bH4oD-=60PXSW$Qv_o>?dlxa-t?|MfBN#gQnIo1FbOMBo=4_P(gQYs zg}YX5*Sb!*_w0-(E`PH29joKsRCfP>iruF$TJ1Ne>)lgCv$T)8Ru=XU8Ql@~N z500YYZQ%5Y5b|iH>U0ASYLfvzW`kl3DP~0IOq_Z`oKT|halxMjPZc~;CV8aNT!B4V zREB87_$~RV5R-@$ez2H~^IIjD+j%J1;)cbZXo{E3hu{41kuy65cP3}r+KE?=KLRn* zYEG@|*nv_h%TJ&rGV5Mh9vf9$X`L;nMnvF>(6xd#IBBPp>k+k zB`9@D1SRD07?gzvMiBu+dX>nD^;1z1QWDq41oh~>5){MgBPk+hik`~h=l=~73Yfly zu<2igRd!3imXikqY-x}r402oWXP`X4Qn*DsS;iN&220M{7S*O`Zp671sXbhtGl*C* zSzhv5?Eqxu^%to%WFle(AukI8q4%FRM81Jo88}>%XD;P3D=2N1L2~+LX`{3?1`;oC zzLK%0Y&FpO3$7r7u6x!Rz==%t*FlgH??L`Li$UQfXwIXnR zL03tL*2vD<#~`&>cVcLLn|`C>CP>uN70#0pi^!U**b7g0kK!0E;lb}ik-Z2Tq5!bI znkqb7A=4k)7bW_lxI_OS5)RCt-qb+}%I*gxsQ`RsL1cEtqdbddNo$kL<~YUhNZRNp zPucfy*TRpCVgSavPkW2?#P>2PCVoNbDZtWtv4t>Id*KV8#jD8ZAYM0grRQh1?LUq; z&NOBealRsRKq1l>qcM(a2exE$Eh^T0IAdx7;66wnqCFL7cVo5H>hobbGuc3Q(o7WB zAumvE74BB$>2RjfAwhBR7p!E)o#~8)mb}%aQ>O2`vIC_Zjb*ARA@l_GiZC1P8G$#C z#1GA$HZ~0p>a;!sJvNye&rRtwIohl_v`zHw2v|d=Sn3WUd!O7v*r&m9lP!ZZ8VMbW zkB0$9yDc!%ATT&>7|%ddrDY~Pi~N#~7)tEM-qK0a1<+wDC3>bkR8bJcH%t~hmMBqZ z9YjUB=H8>I7|m%E0SuXuW$_F{4{xd_)0Bz{;<+B^^!1=2*t4sMRWtLq4WMj|VOXhX z94T=I12wXPgG|H$4!?_FbQVD#i-IMDEO#=)=o6gOVY;H++Iq+aVTR|T91eF?%qY(X zKAiDbf3myE*^6LcNztg)4q~W%a`j>-+nLG?u1azF%(!>Sw+OxdYq&*a<~}x(qY{`; zaf$pQz00}ladZ*|YDjA4Ob?w(^CjuTWE1pts`(|VxhpY!W0=lZ++|Fi>&(RA;C*b( z>^`XM_w-m8B1$pJZxWyJQIxSM@YReyWlZG?x{2dH`1?VjI6B6C1f)JLqk)ioJA)qw zr>Ui9?a3_VpQ*L#@S|gOK(akXhn*hM!&2h>$166USm6DAY}SxUB_ilH>(?HIqItC2 zs=?KW14hj*fui*RXs<@`D72GkuafJ^u8q`v!>p1xrAMq>=~Zt7B2lSoBI1wrGYrGZ zkWK$Wn89V<@&E}9+dzz0yEzu-&O~2FovFcZ4bJ>D4k#&9SI>XE${77ins@`#qgmm@ zqmZyC z1-svnmcn`iOX2`qR#;h-a4(25N=(!rqhMxLf5Y4=it!~)O#D8M`(Za7%U&@_rzP*? z$N&_(4`^cg6z#hg#2$n?{}tNLWZLKG*dW>&uOdiIrxQyt`zCu0iUw996a^i|QmPV6v(r#+s(KU8F(xJ|)wN*i(;_w{ec!{?f+@*DN zMd$dKZ~_i_g?K*9^PiBF2r?ub-{o~0<(mJ9Rd>dnag^S^i$uPKB;5CcIw+|~f=Eeq zBg|hSS_${6^J0P0l&bDYdjUtrXxezsJSr#xaSE3?f;%$WZKq5}pCtm_JAq?exOjHH zx_ISfXBVGmsY-VV%M2W(b|$4;6TN3>(iUw{k_$E7T5$5zAv%bOo0G%Kr~qn!ZRwPH zaHgTa7U`g;AI#U6y+wEFLyk3<`o9Kt)Si3sA^0~8oU549r#Z#(Nt8^|-n%i5 z(pdxN0)`JC&t<{&5ez0mBg>yf+MCG4&JKjC%G=@dbf}lkU%Y`ZnkqyTPA!qEBad?t zxwj$o_^4!VtAGQMFX1hpwxAkY*g2q6N!MS8V6oPJ8Da&ZB|L~ad_F^oG!`g@P2)4h z_1AH@5_b|P9SgM(=7+s{^53}M&6Iou+t_`-|)N(TsF|ZVuU@g(keHy2i zntL~!*ilzj6kf8UPmmU>yFEQdN8@P*>6}COKp)*E--v0?j`X>3Z`>zRxe2S{1m^V1 z9d`|Jf^@o(bq`1}oLNBUVk%bKUapvfyW<o2G(&Sd|yBuAHu{rE%V zoUx{#;6peTFKKA$!4w(m%<=eZaH`2b{sFb`XYdF|m6yW4Fwfl@fZGdgrfNGW0$aHzNcDBPnq&||k4c^`=_c(Vj+aAFFf{J`fCz3axu0ApaROkq7CVueov_S1h-IwZ8x|HrpRP;<;>_t9+l~GvqvP>&)m8=PC$|_rv$d#-yYpQ&I`B-_fytlj` z&-<+X)Z&ilPA&e3T9oNOi_EbXMEzroeO$1OUjiSfH&Nfm|JJ~Xzd3AWD;KvU zji!7cdzF?>|GCH<2eR=-T9otGeMufYc|VelP9DpxAt@UIK~y^E@lq+ra9LVaXCqX_3&5-Myn(lL$RADzmjKPo|lCR>i< G#{U8|O80UA literal 0 HcmV?d00001 diff --git a/Robotics_API/__pycache__/Bestman_sim_flexiv.cpython-38.pyc b/Robotics_API/__pycache__/Bestman_sim_flexiv.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..f5ac090faf3b8ec1a2010ed8c2eb4c81e5c6c7bc GIT binary patch literal 20037 zcmeHvTZ|l6dR|v`^>r>B4u@A!WKk=N?A_r|E3G6iSC)8}xGN5sA-U4lO71lK)C_yr z)7?B(L(Wus*2z+vbsWgv7`B6CFPX`k93w$4aO~tIF`PF)2n=`zed<)zslU$UzyJTKyHiu;4F10Tpa1#tAKuMm{)8v#zZ_03 z;OG5s6e^>PX2vn(Xga1GvrblymSf2==j7y=ck*&9#K)pjlJl}t#?fp}tW7$T24QBK zQ)|=CwA8KUzO@-=ztnTh*|h`C0jcLvpL6DJb%hL^-e~WRQcnKDnBuu7nE^3 zQ<->*ac34QMwq*Nzt(7>a-z|ClsqOdbQxDf` z{wwZ!)78$mKETa=of;qR_*D;$T#xOW4bOkY^L221Vd32M7H%|~=a!r9M&nUcs|V+r zjeFJ3W}RJ0|5-S>fS)&GWHPQS3>L*u=EoT*7oS}LdVIObGA6>-c*Lv#viLQUel zsHW63jwQ8E&EQy8`_(Lt6Y7AP!*Nm_REKb!Qis(M9H-S$bqvRS>N#~B#~Jm!I)USU zbyeBw)W?Q1tFEaR)E7`Ypx#k0sne*E)t68^sIIGdbr!WlYC)aDlZVyI>dQDD zQLm_1aXhNNqF%%Cn0j5kf#Y-P=hatnJg&~Gui^N-dQ<%ZjwjUD)!XU zexuBGvnVax$?fnw?`KvEI2LiGBv;D*#Ombsls~PE9dpa__ifK~tsMhx_A66mKgLt5 z`&VbLWS;Qa0cD|_QyFwIw-~&H6~E|tuAi)TBWSn2(nl_DVO^``I#xLOGf`vLZ{z2^ zf?@@)_~?sUhM(ygzOibqW}lea>Kdyc$p@zQMX4DN!aLfdz=>{@hagUK{k{-D$U*m)aY0o9p8>)~XS@FW`OU zH%Slm_FCtypW_J0q4I`lOlQiO^3Q^o$E@l?wc2Pk{AzXK&1Sn^YkF^?(gPCg11!8o zRh#XV1%G|5axg4KuLE&8Iq6lH)9hSWSkaA6$JJ<|-G}RLtG*d#-)pagSv>4mH`)r* zvWC)`kmqg-5r>oVbWGBr+4e%KQ}geK7H1}$sIP17V$M`UYp$L%y{y}7RjzDxxzTjP ziRk${c}$q&gnQvst?oA-xmB554_u0LzL* z#Xj5#i@k|DhBNO7ii|mJk> zlkt8eFbmScP=z-#xM!#$aDFxJLy7kuMrTD;=9v)T#A5L3wc2{qx7&4DCuO%=woI+< zxf;BF-uCroqqSoDZM)udYua8|Sa`~H3|@?$tk+ttwr}5aqx-I!x52!9K7*!#9m5|4 zYd5@J^I-OBt9B2|z7oF}Pid0;Qz=LR!NzUB(QMj}YZ2V#wvOAPSNB|?xNEn8xFUFJ z&3U_no_greeYgI=_V3sH(t!@cR6l1oN9PC`<<_t`V=8edl*+sc~uVDRl~?UA+M;)2o})ZdkICxECYwj@;_-< z#$*;}Sxe4~z;SSrpXGDG$#K}7f;WKRT!aL{IbuOOBA+0!XbNU<>TX9-qRTYL6yL)m z;Svjrm8?vdGri*aGN}>!&?Ll94H@+CAP=ky)M`ZQV9a>K*U{V~{$o*sj8F$02Ljc{<(4NTZ^s8t;JOPE3 zc9jBU5%)Zi-sX%EJU4FGam%_)4Vv?B3cmt=!kpekrE7dx*fO`WTh>-?E5B9Pq9ixE z`EH?G?B-Ui9b>E1Ev(dMIc7mj{h#V zsTcb3igRLYGI_gZQxe(V-k7&H=j~wLe)t$aoIS+p!{8%MXYb}mHpG;DmD(a|D^1s1 zNZ==xTQyx+LogNK~A z&m=w9?IsyZ!>`}RU}}#m_T&4mc9RyuL&#*%9V{r=D2H=q^syQqV!QN6fm61wsYb1p z8g!W5*bJ_HYCvCgSF{UtIU5>F4umtrCsk{wSo5l)URS0freSd|LSrK|HbWx_&4-Wi z+rV!VzaTWgdf1^bC*ugS7{@TmO%X)ZZ%2Sd%_;(OGUZug)|fMbQ{#|t;F-@v!F(D8 zp;8*k=;nxoD_JUypJca+-Qu>nlljovDh(juWVb{lTqy_&mYxL#d+0R}Y_%m*1Y0W_NoPHISEt!xT%$e^{3P_|#w z5M&%@Xw-gc*rKv%N?}dS{VB)slmmE5bLD9sMPS-xh{4c;uF}#ru7tVA8q&RD3Hdsu zd5YJUrDCREcBd{vklc;$hktFWytF2c#i;$e&E{7g33%;SR)mtYJb!UCnNp zzUAj=9zHRj7@T@cjS>8pG~Vil`zY1`L~VygwGIWw22<`UUyz{-<@8g7Y?tyR!uX2` zM;BJ?+1l-B*MlV?+9()k`X%!*H+F0F`?fq_LwoXJQA8@q8ie-RIs_7U@>*>}>W^y8 zb@Z3YWGw>Lr};`=q`9VQLcu_{75(Tkir&0Z@JwEs6tBER{{5ypV>X ztiz>I48PP&=X=waG7GgPi83uH z1@;+ukt*ly)AykIpSH28r-5vzlNW{-Zv^%wS_Zqn^8BI@agd|EZwKai+sOh>aPlIw zzdas< ze;+Nq85EfU75U@FF_b(8m2sFd7RCVGQ>~|}DCjbYN}uk9vL7e&28ooS?CV$l-1Lk& zId9*=I>Vxb-nwfb}=KOmLrpt$OxK%KA4FQHz9m&2LEM>ev%$x z3S$~A)u@Bd#^VqY@~H6sKM92%RC4cxhl~Rop%oCz(s1%a;%XQduCAi4PqG+icTM$K zZ9{GRGc@ZV*H{f^p!LYqC1gfQCz(;@sb4F5j}x&{=3-hRlOabc>$H zO2i^ZaUu<66urP1BN8N3VX~tTdK2}O79+?o;a$S6#!k~D2kzH9Rf7Dz)GXP%kzlX! zz$9^kl1WMvAtnnRU3#+w!~RivkSPr7`5SuNnYLpO-6CHWm_$l`jOf@L`iRy6hV_hq zdozbgBRvW!>c5U!&zCbPJ*!4m?J&bX!byzZ6tn%P;lBq2k0L$d`N8~dlRECi7DJSGwiP*erRI0#DCeW(&R?x$1*;;3xg#_Hze zN>f?!m7Kh*NE1MOLAjkAoX8~1T$Y#4$Yc8Z-6y3LRzg>sjADa(-zU-m%C3elTb&jwuUcS6HtY_=m;-3;b`4?CI*V}RBzrI~5MV8cSxGO%cB7<=mLlzJdWak5kjk(+oKZ4wI^YmwB^-LMc( zQdGeR!z@&vVbVRa29K^Nk8oC`(dv%@I|>d>%z$bqO;9eWxA!y0^WojV4bX21u;~vG zKKuu$!0`nNel7zlg{Xr!11i-jys!-WFy;!Yjdu#Gu)Velpkx!2d_kzR5K}U*EG1V+ z$-JV3ZrO&E%qx$S0ZQf-nDMauF2TvhcOsmmf8du@?uqeT_Qu$rfG;>jY==oAj&{t<-IZMNyKpgT>|-5BN(mus|@_IPE9#gMgrJfZt#c$eFc2UyAf z!2{llC^8dect62RdQ7BuG>eGX6|-~3bas4fe=qQV`VapB|9<~%-NY;EV%PCy_@mOm zpD-F~E+l?s3Oj&-E20Oy9x-j+^ZQ`O6D0_aO^)>6=)|MYtxf<2XbW;W0|p%NLjKT)ln$;$rp2P0H2kwVO+q zuZBk`NiWwl!lwu^-9R2nyLGLt>n>mkIUE|l7~%BbEQR}qMi=HEBe$b|KRh-b074EU zj_VVG(*x}xf)fNd31o}3<`M(h2pYUHPHWy9h_Iws1$ps8y@2`Z{ZalVq;Ul&w1tEq zkl8du5Y&N7dO@y(Fe15(HjI~wgcKs74s6!CuwWAeyAg+r|FRQFw7MWh33k}@9z>Tw zFT|h7pN;@EJrl7@C%QrJJHlP?+AkwYii9dS$lw8;x4*rNkm@qRzsm>*uc5w%`WiyP zcM(=cplk6X{MJ57rt&Wzt+K}we5m`?-URk|Nql{eS)@a&*GeJMEhh8s)0WKBB9!AX zd_n&Wpty5@NeQ)$%M6Ls{r81tbr*Lk#Ym&xLS6q&7T;mWXXY%aohd#dBD2)DgRlPi={<%vAOlJC{D}+}Sl_Cc8I@W%%GWi+0BFWiF8h==RLS60b;Qk&2-?YX)yT8#RnT9S9CWHo=yRX@QDjoVN8J zK7XVf5bQmvF4p_!fTw9~uCE*|3h#*>b&Q++=IU@?kgZq@%im;z zz*S~!>INZ=5wFh4-PP{urHi+(Mp8Cr9~P%2%t^PMX`UdstK5z1TCL-h`{&MN&!tbx z$0@el$6_1}N@pK$P3!Y`4h zWRFywcj!(wneWV;5M>B<8WDCz5yDkS$t^gg;C!;^d`fD0aa!>jGN0qSbV=o)UW>~s zR=0E8B{-ZVKdf66r!#Z-bHQ(ae)uf@zrFF935l_h_!q!N8g7vZC%%$OS+C$qXw}=D z%_tO^xIKsLy2#r>Pe$yz(7%kjGuK0qwCmD25PLP!7bD-r80W??G94X<1Oh2Y1XLkH zq+X7(Ak^7sMJRzaAeJUElpKCp{JcLz!A%@bu$AMNVTnC49%S_~AJGf4LLcl+u7kln zVWi6Zer8!%=f_31<_;6&u@v|`yqj~#NI{M(&_?XEqhOW~-$lFJHhi1HF!KuAgT7~~ z+%j@u;9KpT%T>8n^h?`ipq%8yZchNk%U$?M!G6mM`D7@OONJ78WGmLz1Z={IWyW+% zwU;qr33lE=z}bVwycQ+9Bmqg-hB6}&7ZVQIKtfDnro#$A)>)&)q~|?xGrijO8OAYz zq5as%z)NUdFszpJA_aMDOfAlXy*QXu)l0z|+gbYzxRwY{3=JC!`a!*Mc@gCG{0pv9 zi$&(lJ;b_`KvA5drP^@uaF+qnf|%en&@$R85>2Kz689v2F^AAQhC((A&2AaMgvizh zCPkZqA{8PP^$v5wnl*7W75v`EQzqj4V-f`=c^U(@8FIzi&e$G1C`o1B?+L;gd~7s&U9l#zl)}T zdWx^1;^FLHUT-!B9G1>pdfN?U6EipThin}sv{hw6oS9@m5M6{G7JF;%%%&DRzAO2o zPMP_qu5LkchZCtr5hIc`49;$NV9dPzI%k_Jn3pvooH1k0Wb=lF zy@&Y!vwUIvW}7jP2FQy80p!Ts=^LK4*U~6|xz?=1Uk=-Vj6I$XJK16kL*n{3Dzi|esW3mY&hW0ugSFmQr3BFMp*~4z- zt$v`PCr_h=h+j5HDFQ2nFN0>VfJ@z#0dg94ff(DmS>fZ()Idj_nc;U0uly`_N=8#% zz5b1=%;#h=CNMJ&np91v`2jIX?( zd3eUk_~i%X9~c^tw+cokID)4SN~ZLoZlRYa%^d^k@K)hFg%2{{!c!mMrw)U)9|`^_ z;d8eUX~KTQD8GXX9B;PehRfQcTBFImfN_c}@JoZGQ+v|U#w4K(UbP!YHKhTa;_+iy zXp-_5<2gMEhj9NayWF<`0aFxF0KKSiDzT zSIh@TW*PR7wAcNoG^Q(tJ)$R)H3v9m&r?p#CsqSPre0ZWUc7x*( zW^psj0~4T9hNV87SqQHs?l$-QtM&WsC3of3&=?6CPNtXqPlzEeVi1|ai*RgUcRZXM zvncta!(*E9b#oHuXG|pgkA4YZAq{Q-Q7Et|mW7>B^u}HePh2E&v6+S%X3svNC(j1qihQ z1&OiwBoi;u@E$i1GmjJ}+!P0&qWr15eF?@-ba4H+eIa;$08@MKh%wI&jfLP~+$Rd# z9)V&L`0U-5yMatVZm<@q14Inw7SXvFhoeZBs}|tp_$Qe4aHg}lBwtK;_)h#$g6Y9? zO`|T#?)V)(FwyXO^P$3`*fF#==*)-r?-UNsWc)Vx(zLICo6irDb%?Je2r2fMivcbt z%q?RTMHiYj=B%<{*o^Ul8GKx`LxriD(xjcP$rrYv$S=uo)Tv*}}9zPGyeb!4X_ zp&o8j|FH`l{AIz$S6&;+i|7dv9$)Tn=86Wu4RJ|#=X(H#TM38h537%)gc)IQ+}YcL z&VxP0e*Q-;x!U|^byY}!xg{L;wJ6Tr=`y{p5lF5bCGD*ZI8e;0@TI2Z9@f`82`ud)~(G#8{dixmzUKYo^Ati(w-ex?CK z$|#duAj-lD-n~p586kMu{=MCe&o*FTNc#8C)kXdL9M`zPO$kQ!Mt3$fy0JT823l9F zMf}C3sRO>Fi2~TfL!2-8gjdY|71X%~QdC~x*IFoZs@1SurHPI20-!!wO^#K4$Sf7Jr+?-(m50 zS^R*-Z?O1H7L--`U$77~`8TZnTNZs>k(#&-X(}DX&wC36!ZLHk<3+2OEAGR8sW?@f zDVB@VsFjKn#hLPREoF6V8DZW@diMuC?2g?VFmKImT7s4DIrU)U>xd5T3 zSL6d*61T?JtswuXZ))l@`bTVXIMVkPeC#Xo7U*RWS%~lL2nyAjnCN@!p8g4+_@^kG zGCzZ~Ag+<<>ph3%!Y)1w=df3ApuL9=0AZ`&0)pI;?^{#mMNY}WI=PeSPc literal 0 HcmV?d00001 diff --git a/Robotics_API/__pycache__/Pose.cpython-312.pyc b/Robotics_API/__pycache__/Pose.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..d1a5f1cc41b9309719cc4a58074ce42b31868b4c GIT binary patch literal 5528 zcmdTIU2Id=`P?5nj$<1q4lve0E=x-=3QoXufMJwCNXdZk6IfY7=HB?;B-gR8y>qWi zY{G_G9@tc=AZ-c~tJZ3jx+*+$(xmlmowf(2J%%gJKs6y`_A`!{HI{hPvE=wkC(@O8zSVdIB-6((pl<)&NNZTAW=Al&v1i0 zKp`V!#X%28c=9Sy#H&Q{%<%S#L2r?ddP_g|w@C+kwT#*?XI07m64I%hp~mt>oI9(z zc$`g0JG*x7+TOKidskfA@p7VTPa?ht>ADN%c#c+fK)T`tqK{-{E#qPlE$d?w1#QQU zox2Z?;bbh8%OYiuYNV)^H?^FeUL!QVDpbTNUh&-a&Qx^#+;_X#*;}b|ZvRa# zS}*?;Ow8}NVV@4>D`&KlD#?si8NvDEt! zXVod{7+j%Ix-f*%YFK(4^+y{m5tV2892HyKq{S62u4MT&L(>gY)>Ep*>v^lLqH)V(jLUh|;?7tNqcoRI z+M-!?vXR6Ku^MV_+ww5ISwcqDEk7ey@kTBGcjQb#J%R#7MN2f)%qR`wHGB;ki=zhK z32NlY_RjHKR_%oR(RoNU%oDPn?9Hf?+8do`aw9oYOBu=TQ+=IyO^~`QUoxqIohOr} zAVWFBwXmOoS>+0O+)OrXym|gs=$iO;aA9+F*0%|I-s|E|yf?+4c^`X8VBPhJ4|aZ- zct3HkA@=dU`FPK4yyuShZoL0~xN$moBY1OSF1+np_x;wk_qspqd%y1&$L3nQuJwEt zXo1zi_174gkva|gbB2yM3oq|hhLOqngxJy+dw8H+6t`|fX$PR<&_$lzG_k5 zM0!kZSIEN<2{*rU`A3(_z81O}P$@T6!T^nh3Fd>{K`^e8+yYM%5h(10(PiQ~_N8%` z72oRXXHW+C(&)V_K1EQ(+itZrfV_BFO4$BfC)KYw28K?$9jGY^mW6*`>GU!+*7)T! zX|k5yR;46a{%X*(Jj|CV^43eSR<+hR0k0Ajo(w%h{4$Gq)eT?m)&1K$q;_@!bReR( zuh_di>SfV_qHA0AEV!@$b>-+CEv@S%}&qvy3BW>kC+d{DM`fEQ<-j6g-@4K<@ra9l*IosMf z7wMXh?46D5{p6)PYs-}OtWRK40Ze%EM$ znI`x#QWU?F~rxB*+wEm844g`)1?Quthodc~Xe zu#J~Bu=8r};?~1G%;dOGrZ$UFnGsi;tH04(S2rga?Q!H4bCEa-WAl`h**%TVRT| zV}ovRHp*M|@a9ZGm6g*ipHax;3)CouS1yLK3G3+Ogr+OYvN@iGZ?`2>JZ5u+yCp*Q z=KE z?K$@1H>?1AH0(5>Jl4nTC@tDiIt-P8npCQP4^VvU+ah7AMlG^Bf+k>8@>c~q4P&DX zq9L&FBb7LPKhU_)6f1YWQjQ(`G;rtapNIcATpqeGdt$i!J-K{*q&%XORc&6Svnnkc z=B!#MzfmkJrE+Nsr2YYSi2nk?G5(Pa0C}80VUPNG7@g$LAh^s8AQ<2k1S+rLI+fRv z5&AQ1rayMih}$<`x`3V$ag$B!ZMhx|bV25;iJ0zZj>@jtSou*=EOZl;V#gXgnb@#Xt6u$i{_mP5toY;s$11 zUoQFpjtB+@aKHq7tQIq7a}uxz&-7Rv*QO&ER#mS$JUBcAv87JaRr(BdJk z&a-i;DeMV7ezDmTvKJzGw2mGIV0rXHHea+u%6cQE<%==+7Xkm=h?!K@4Gc;Y{GYJe yke#%G1&D;U90zDG&|&a4@Q<2@0>^QWyk1URY9d_ZFJ#9T-g6umS|r#qPW}n;w&lkF literal 0 HcmV?d00001 diff --git a/Robotics_API/__pycache__/Pose.cpython-38.pyc b/Robotics_API/__pycache__/Pose.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..d992812f90ae2eb3a2ebbebc590de7f3a3869370 GIT binary patch literal 3494 zcmbVP&2JmW72nxilFJn((XQjfwVO;@v@%hn|F>($Uxa0d4+_V6HjooSS=a-y4!kN>PEN3(Rmn-n@D5_r8W7 z&d$09e4qX2-Oj(~4C5P|On(+kZbOs5gpL_)gAry%q;0~l6;{(avV^GLgZxH1?Ofnxg16-N@3gvIWDmU5$0070?7BiXk%xK#~wc8~&%PP!y zMnL&F=CT}93PdzWVi`)km~7!v zh*D8MW;78YkZUQMnpJ?ZjXxU(C1~$MlmCJ)GX`Xz3=EG9$j27^n$Ymq&PoF_D|ajV zruf6a>^h9>z$mL8x&!lwd~U%>>u~1C(5Gv0s_ADl1J7((a4O5@2If9E0i4T#8pgo4 z7O#DW^2-sW1(aWA)$TmA2PP}A@^kAu_c&RdRrbkW33C9os{apS7+Oz+?S2eE_)(a1 z04wI7X(yt|(=HFPrdyER5gpkoW_YlG@M;9O(j^+F&6xQ@_5;N__wuzDL{%8?0E9CCcoY5p3BA0za;?XONW|sM zmtFv{>mioE7PhKqdKXZCNW>-vzo*B zbO4c4d^pDTJ~UZ|ZeVl`2r!cWu?Un7&kE0;1k*#F^+kLto)GfZgcuzUzXs6|z|SSP zLhC>5uE12)-~w-KreC8VCUiz6Kd0rM-c|0zGXqV9l046b7C; zpo|lx3zJD5nzrmJ_$rhLEJ>ea1~Uibz+h0ex`f%6;B|3ev$CF5PRyLqtjfxVrY^tW z=Z6*}N8~OPvLobD`(kFF5F`E?_|F3Wul{fTb5P!g{AbVLzYF~5cg(*Lv7Fi6605Uw z?EG_+KuP|=`uqqufWDx~{1IUn*lXuM}RK ziZIrZ&5I@Nkom=lx83O7kGGOxX3lFxWVh(1z_jtwYOZYM<{~}D*)c|8e(nkSS@9YH z(i0hH9I20hR$hRv16Vz&eGU&1W*pdmH$Sx>883*q3R~K@UYN>kE{mD*d@6IetC(=$ z5trWjGqS@BP=;KPzo{KdE7)9(lE9DTEf|c6Uy>(+0__hMO#BLt4JBACMwrh9rZUll zZWCG|iFg~AQLe`gQ&r(r7eMt%V&%wwif=rb*H7c6T>ZZAcEXsQkPtCO_%w753yTjO7s%nes;UFO`0I`C@Ot_+!K0ZtCb4pz_@{%d%iay_q? zFeunCu6bDAqtQ;q^H>sip7;qKd=opZBFZu)-$cl9f|6L}9b((g+fLOzFL1q45x<2W zRf_w)bZ-dL>oN$_y(T;k@y*)Igda-;TA*ol64po0x=T5$L#b~eb6v0PszYkle*qMV BagYE2 literal 0 HcmV?d00001 diff --git a/Robotics_API/__pycache__/Utils.cpython-38.pyc b/Robotics_API/__pycache__/Utils.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..455f025e10f3ba6717a07f91fc8916af32b4036f GIT binary patch literal 4765 zcmb_fTXWmS6~+P}2~m46NBME^;||{B1&FX(mSDmTSR;>mK{)P4|A7di?#lajp5wFa_tH3ON-?0RpUR}8f_@srkvZxK@j)X` z62CiQPc!i_j%586q-8Z&?mdcApH@9w@7UB=JS5f{-urly+aRfR#4-r-$>S*d- zx4P~zdtr-8#zgfSduV6&k#lT8NHu?rSyr7mx*tcoQl;GII|DzJDk4tPn6C{&skk3? zLz(b})>mKhSP7{b&AMK;;VVcN^Ar7ZY5eG)v=zh>21&}}Exr}TeyRuP#NVBq6GZ%v zdy9O3ksmDb!)NA*rk*KeS4oLYp2u-GUipq|gtmMeNED3G>Dw9#IAhuN0>UDZ*S(AU8` z@z7;7zJ82G3T(Y(jJ~${zJ$?gQ*#gB*V5ZiL6mZc9ZEPsJM?b4dU2GbN`u*`_Hb=; zJ4m>hiZ!DN09q>j&NhsQKR+l95G1!?h(1^5p4?Zx=Hb0{AY@6XU{~s+^dMnZ$qvBE z@B1p13DnQ~ae~eLNMNY1Vln9G6}{#|m|CSz{A&r9$Q^kF6coVAkJ&hV;GG_JPWd0q(9J4kYnU;+LIZ zE9h|Oz89nft_1g!oXi*FT2Yv(J4cS?``w|LT5^bG(T)^!`Z&yi+HnB+%+yz1nYIlh zs|1O$YBnKp(;rd1gLN97VJs07UVK-d%`so23OSTzo@hDDVk%LGW z_O#b~^&Otvg5Rqo{V+)3^{VFPt(h|a7$$ku&z5`7g-!(^O zH^VM4^;7iypRw`x5jLRD#5k3+$bdGqN(apq2Ig7&%#~)vIAP z*73$96eo5sLLs=)T*V&cdJX=o~n zWaTR0DiBP;WM8|@QX4-h&A23{mQ_uEnK+;z4pzrm?#NU;o ztz%AezZXt~2M;r*@oN8dgoerAj@gi9?8t@(v%BQQkf{lF%;4J&hi>K`xdOpsSkWy9 zt!i4yDo0g;4DTf?+LcnfTC}T}Q!BN-QhTb@u2Z{P8`d)CsFq;`vvzn!3H3A*YLfbc z*az@+AT^#a;$MF8fNvquF7R+yb(7UGW@$isqy?z8vC7Nf8Z9Jmh1^i+uwevd(#s-I zJK2rwXqAtBrDkI_Cbv9LJ+C$W+mkst+t1lS&JJ^X zXCLnYUTW_(yxd;fL~1~OAg{riR2-*yZ7Ya`ABMS0jl7~0+uXr$UiJHZ83`=(MmNvh z6bVZ1($ZE%UoUs*lY5|=n|!*&F+hpIw(wz`r@(iGq+}q~Ch50EnP6bJ%CI`DXtxGw7C8UBOf}-ZA@B5SDOf-qilMS@1O+O zL2kW+B47tq06;Mse5SQpPQ(kWZH(&=gAv;Pq~Q3_lpG0P8t-)Gm~()Z4*DT>St#;+ zR2gv?F7kemcD7NX_$m)T`@m8svL{ey_9mJyR$FOb}3ZU)SBk#_+l4!lW?eA-iY=uO~dI`arK z-5Me0@E_V|ya_HND4f;C1X9lE_su}jmS=8$0+|oagUo3~K060A#xtF8c?!+?Zf7-{ z*R>hz+AOc39HdgMG1IyL#gzCvt?!Sa7xil*pAh+o$fWuWBu=@HS$$3$`X;>}V^yK< zIrbKtW9lAy8f?A6)VtK0%WLhnh&%0eYd+}&{evcIk~HwcCNkzIq4=yQLDv|~ufZ2J z;UJ}Ku9+fZliXfKcjeQiKD9`TsOHpllg{pS8m9xNc2gR8nx7J6^=)(z_CD^RdJE+z c4mL@syn=&cm+ktzH|1UO=Di#Bx4pUl0B(T#R)Wn>4x17|9%rc-bOF({cs-GrP5i?L(5etx5$?zFS zGW;^s&&bbB)vrv<(+6qw1!{%p(htf{$}h=GE{=B$@YIiw&&u;SP>hL4*)M%G{gV^ literal 0 HcmV?d00001 diff --git a/Robotics_API/__pycache__/__init__.cpython-38.pyc b/Robotics_API/__pycache__/__init__.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..99e2730476a785a20f083224c64ce8642e95d043 GIT binary patch literal 212 zcmWIL<>g`k0*9GC>E1y4F^GcAU0DDV=iM9BaqD; z%%I8gk`bsxlkt|YQ)+QZZem`1P-;IMq*+sR(3b5i^ij$xy@s zVuOfZCi)rqxvBb29uxU<(F!fIH8HJGp)ws3?SI}Hc_rP%G38f9jgu}asoEoOC> z8a_s@h>GyOWsRy>68^VVV_8(iGX7V@M`Bg1;mxX87nd-yCN{)n{I3i5sa4w?e%x14 z9H~LQH}L)E1<$*oI!r}Y_lIBHPkRFybq*EZx?khFH*ej(v3qm(7JnFrr_nKgq(q(H zi(|e|b27dkW}!R|MZHljW}L$y>#G{m-hQeA6{X3;mQ7>hi!86;zK=WmEr_ztSPuKI zS)jI|j-l=~tK+_5uWeDvnJ9l}pE+-=k)7MHqY@l3%knv{`)P6;(p175G!iAxUn^=%&2!#WRx)?ooi zK3%7s#&LQAC;%p~s~5CF%uZTyU%)~RsPZ_-6i*W)mpUj$6e|;_Iv5k9PbnpwSx5~|?wB;f z{$WcOJdz2e%%|3oD;&hk4gNie8O}V87b$PfA161@z`iG=Zlt2)5Kcr^;RI$gyGM3l z1ZTuV*aYb(h515qk7VZ~{$zf?xz#zGheUx07%j{J*xT!bYAW>9XtY%;{NE$fnktZ( z0%T#YX)M1&o+DGq?nM&;TO-KnwfHb$+JCsnXTILml%7ymIbJM&0d;?S^cyd&m|Rtauv4$>lv7$ z&b^`3!p>}}WF3Mms+wSL7?RC!~D{Rau8O{M$R;9djRu-s)->@-Xn&Hc1zKl7Q z8Qz=Wmu7gM_)cY1$(^%G4hd!*@tlrFb@(q6fk$Z|xG}0gT#)b=k00@N6i-snJ=w|j zrqQAo2_b6>p{(ukS(Hr#DJ!CaiUNAZVMXyBXS0jdC?D+cN1z0$LJd>D!Qx1!Nrr@o z!X%?GK@e%6z(K=(f^{1nsF~NO{^eqhW~Z7BG#hIB=oI$=H}NMmPumZk>5_@LuHekb zG*!CNjuH{Xv37~kB~x6rgV(wo^m<_;Am~jj*RDc=*Di@RN=9ex(x3J~jW)VEqcMPq zmrW7ilv16V$%mK8O9iBwri21lFEAw|Y+Jt1Jlkcf%*XEqTejur7@fo2G6tso)t~W@ zTQ4gkHnK;~$Q_kNQ}XXYj!`A8zxb6Wn+@!s8LTBEVqs^1#G#wnZu8XmIsp z+LIN?Ab%BAl2CD{XvZ>sW4hXVTbX4HnS+?>_hPKHHPuj}zHRefq*{kiIykBE6Leny z^LR#$BQu$VIgX@Jd+U9%075ix)}pxvj*6o!BO(ta1*qo8CHzUsERr%sZCTzV{#QhP zO@tHql*lJUs5Ozl0ntnn*Z|LxQCvGvd=W`224gBMdandxMS1l%P+YGt`2|M)FLKJW z$Whj7CRpy~z)lC{6=-pgM=F%fUb-W14mZ`@BkXi4gq-2IiNp`VW&#PcqzloQL(F?I z{&B#Vb5~e@0iD|)g3f!p{s4H2+;pMs0=kXV`w`Z@Imo6l)fF_u)Th>#8>^GtQ0TZA z_uo;pZxgvggg%uPU{F9~A%U!zeVT|n#eW2;aoE?{CR=Cn_ZX?MhczZYChn51G@BxA zHJf^+*=$Wd{_yBGn?WC+g%Z4r=J8Np&00}!P)84_q9Cp-iSH7s<&r)K44CytNLO{_ z&@ik&h{G;Y^fStgB{QU}Gh^~I5~5z*GTkIuOk^ YLEMtdT-#&IY{T}id7IuZ{7rA|f8Pf)Q2+n{ literal 0 HcmV?d00001 diff --git a/Robotics_API/__pycache__/utils.cpython-38.pyc b/Robotics_API/__pycache__/utils.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..4681ae70b207ed1cf988193eb7986341a566c9da GIT binary patch literal 3969 zcmb7H&2rnw5yk)@0g?K%?3K%@Y%*~+RbrJ)*-Bz>DJfUHj$Jv}8%MTN4%aFc%mFDl zAV3X28DxU;t)!fIH8HJGp)ws3?SI}Hc_rP%G38f9jgF|X^}7PGob zjS5Dt2~YUnutrrZiOM%tV_8(iGX7V@Ct_8s;mxX87gsQ{CN{)X{I3i5xmDX7e%e=2 z9H~LQH>gxz7Ci5U>M#{qy)yj#VcHwWsB@_J*25a#y?y8It=-$ZclhHtJdKX|6D8{W zK^*ganv?PUFbm~zDC&)JG2|4Uu3z5`yuY^Hz3M7V>#@< zVS(C)I)=J8td9GRy|qOtXQKSAedfHgMs{w)4lg)jmQ~@n9;V50C>1~I2P%{aHsmVh zkNR;ad60DCFykkYI^>PNz2d=XlyN!M3+9>yo&_?1145aZ$6M3OJkqj!kGJC#|8bNl zGfEr(Gc(l68_c6_UZOudw9T8DMDE{lSSB(VTeWEq-i`A-w#z^Cj2;v`%n?G z#k6djr|K}2Cs7vCs0>6DBokc=p^qy15e?^@K-$gfwmn{&zZqcbcF>O%?tNjIfv7&a|-7<#Zl6uc1{Pw@0j++`qx~|_VrR*rrjnvPy0dE3@IGCtWxq| z&C#xTQ!YdrzIlxReP>CW7VG_;ox`*n?hJxt=Zi2?PlKdcplfG8eUqxFmA!x~zerDa z2+bL=tXF8o-N{0l9oM#yS+xZ>O>g^xllKuWlc3&^rgbUau3E9PpdlJgSFv9tN$$ z0*-vTPCJd`^aMZvNMKhlXoZ-awBo*ig&aWTagZsVCQy(Lg|^-^j??cz8^qa;vxX-_ z)x@J_k%?|WC^*_D%Xjf^3s0F3Vy(N(XB$jj2S1lMC`J@36Q?>D6Qj>4A)HxA3{LKt zB*Ok~OBXy+2_?+u){!e5#LNx;HHjHcJPsErZ%!X4H_w2+C!=npqT>)wL{8xZWiz`+ zc3=c2#6;Kx=|_e6LUNB}=Og}Xe!scZIh={e@&LXNF0kBAp4AcR)De2A9Z^Q&@LQ_ zv^>>wYH`#ql$w|$Qg}W|6-On?RAH}4LFXTE$qirv$SY+%)VG@3SeiP$u{`S*%pjNP z(g|{Owg}0!V(L`(K32_4F~8#5zP-#=ZTVA-yZd56XB<|P-f=d&SdH?*9)AK#kSf$L)f+61WSV41 zh$u@k3KIm81_~TB+-F#~;e(oajq0B-=4f`R*+8?QwvSG64{#HIQuDR_=!Gtsm}?Jb zMy9FKUOP%e5XagjMwd)!)ec_ka?tCAiGZL#v0S?fWsz! zCSEp0fKy6!Y9=3EB`+0_YMKHHT)o1SjIeE0D$KWCw#q8_y<*F@{53}BaJP(sX?OJ} zJml7EZ^TCS$Qik#(x^Q03=&IuIrnmx`m2$j`{=Py;)L_9{n{BVjVhyR&T>0<@}<0z zS36~sKJQP`=cMD3REtfJ#!xAW7P<^R*yh7+esqExPe*t>LQe!3OkN(CIL89EGa-WM=s${Qf85qDQe5|Ht|0r z@^d1b$Y(@;L4;Zp`AZPZB!La^EE&bM1H~7Sv|=!((xUT9AXb#u$_9$-H6}mD$p1u6 zc@{a!ddmdM-5l8Ipu7St4)Tac+3dAD^5<|<&3(d7$0Otn&rKx04>l7>pdnp|#vEcU z#rTH-W6oV+{RMRHehfO7cKsgk6uId_+XZwRsml@86?2eHW2!y0!qlhMmK&>++)(Ja z828^&wC@tRM}$6<7GO|7Vwlsd3oX*(O_O@^=`ivBxzgKPB#p_L@zR zwwg`7(rmUSpMH3(G@C&mAB7UUi{|lAU(Z@mZ%{`MsG=aQD~YcXs^yYC2Mn0?CrDRy zBgJ@%9(J=nLn+kkO+;LcPDR$Ljeh?i?GI(tJyv|@L4)t#-TU;;{@wjO{wNO5qBs1p5-ol| zj`<<=$@pQIh4M`(T1B<&aRG;nFB+_H4^tJWC`}%?oef9dGtZgW->?gISgdqp+Vg4i3OC67kit<^RZ2V9 zusmCTsg^@LSARx;zBlALrhK=zdz|*d-C>aIejaA(Ns#y@x^@rKSE-8H*>h~==jqul zp*aJVi#3{Ye>xG)oRQTbjO&}M{=UlW_ccpy;=N5SOfPc6#;w748;W3Kfbmbp(G38N zDI+((!Lj_v_{;A-s684=`_JG{marX7yBM!ND#9OTvX~_}4-Q-a#Z1FH?AD2+% zvw1pc9H*xM0RRHO`awHH@1z|M1U%#bDUX9p@ic*fbRY`j9pgCp7OX*x%{cQM87d#I z1&efKOF_{oT=IM$-*)hpxe&&t&0My{BimzAvL*q@p(=HW7Tn3C6m2pZq`z zPKS=L4$@Bw^X13`8oL|310qt3aX) zkcG8ovHSuIN3N3J%Ps=8dL#!_{y(JsmAGp{$uM7Ju05D?q#4NKo?3 z&Y8zir!;Enj!3cdNvb%qP^Lm#={Y;R?@>0W()ThN2Htf-uV zpR;91Zj@bTWA9#P?qkP1P06}JZKIg~QP zy(Ma4=O*Q_Ex`Y;I(Kr6V8#j0>3FP;{;4DII1L2XPL+rY68_@JW8R74DFxk^-Rxi% zE&7lUvUU*4#sQy4*>oVWB66oFp{F;jOuG{{yPS>q;DA4dBuJHJ=*$fjM>0(^5F%2N zjKTy#q=pg)p8XW_dM>2y=dFHu*{5J<1sfJ@RG24cxQDn&KW(^$`S5vB(J}WNoEe#> zs&G0{B7!(BY*LDfPOF85&qX!p_rpYBpgZ;4vlUW%Vbf5rqHQj0dU!76#F{ya0Ze@K zMSxREnQo2`uhTA-kn(l%1g>77OST1J)N0H%ZMM#8_`P6tQ{F-A0`4f?%_^%u<0Us< zI%76A$JW>$SH{(`qmfw2tGScgR9lVR+(mtb6eq0j%$L@9Wn3Gt<}5dJD__ZL`D(YS z>GR%{KBv`|q&%#HG=WN)v`}I2VUv%V{Nxlj-cIm(f_exrn0`Fcahxws$~bCVJ)Ko! zB{FEg$}CB!_^d3%GJa<^+m)sCG+Hb`Obz-mX4;uqD3RZqydSCdF^mpQ8~hY?7r;E} z(coYvO_-BNYO{B)4ht}d`ptW^P`XialoXmrlE|T9$rU^#F^eRoXw+qs^q-UXgv2jM zP(dPpNrIF36-2=#ferAiXu}H&hA$&&O=C=@W!05nSeagHTS%@qnA}Ip|3pr89y!W* z%>>Ks9N6h1y#g&3c*G$#dufl|1>CIWE@7wZ5OPKrIubtwn<*qvj4nlE0Wnu%{NsSp z=dSer5;~uL2s&3*{Q>Zl+;nN}61w%Ms}c5U`XHA@se>|@YSc#ETc2`6Y2$L-cPZL; zN$ioJdS?j+B{Y@@WKFN*BkmOcF-A?ozR9-PCX>HKOM^XXF!?jmt`&~&i?r?g#hUN8 zr+@qKTJ!y2fIo!N_dOfM bool: + """ + Checks if any RealSense device is connected. + + Returns: + bool: True if a device is detected, False otherwise. + """ + context = rs.context() + if len(context.devices) == 0: + print("No RealSense devices connected.") + return False + else: + print("RealSense device detected.") + return True + + def update(self): + """ + Captures the latest color frame from the camera. + + Returns: + np.ndarray: The color frame as a NumPy array, or None if not available. + """ + frames = self.pipeline.wait_for_frames() + color_frame = frames.get_color_frame() + if not color_frame: + return None + + color_image = np.asanyarray(color_frame.get_data()) + return color_image + + def get_marker_positions(self, debug: bool = False) -> np.ndarray: + """ + Detects ArUco markers in the camera's field of view and returns their positions. + + Args: + debug (bool): If True, displays the frame with markers for debugging. + + Returns: + np.ndarray: An array of marker positions in the format [y, x, -z]. + """ + try: + while True: + color_image = self.update() + if color_image is None: + continue + + # Convert to grayscale + gray = cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY) + + # Detect ArUco markers in the image + corners, ids, _ = cv2.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.parameters) + + marker_positions: Dict[int, np.ndarray] = {} + + # If markers are detected + if ids is not None: + # Draw the detected markers + cv2.aruco.drawDetectedMarkers(color_image, corners, ids) + + for i in range(len(ids)): + rvec, tvec, _ = cv2.aruco.estimatePoseSingleMarkers(corners[i], 0.035, self.camera_matrix, self.dist_coeffs) + # Draw the axis for each marker + if hasattr(cv2.aruco, 'drawAxis'): + cv2.aruco.drawAxis(color_image, self.camera_matrix, self.dist_coeffs, rvec, tvec, 0.1) + # Store the position in a dictionary + marker_positions[ids[i][0]] = tvec[0][0] + + # Convert the positions dictionary to a NumPy array + positions_array = np.array([[pos[1], pos[0], -pos[2]] for pos in marker_positions.values()]) + + # Debug mode: Show the frame with markers + if debug: + cv2.imshow('RealSense Debug', color_image) + if cv2.waitKey(1000) & 0xFF == ord('q'): + break + + return positions_array + + # In debug mode, show the frame even if no markers are detected + if debug: + cv2.imshow('RealSense Debug', color_image) + if cv2.waitKey(1000) & 0xFF == ord('q'): + break + + return None + finally: + # Stop streaming + self.pipeline.stop() + if debug: + cv2.destroyAllWindows() + + + def display(self, option="rgb"): + """ + Displays frames from the RealSense camera in a separate thread. + Supports RGB, depth, or both. + + Parameters: + option (str): Determines the display mode. + "rgb" - display only RGB frames. + "d" - display only depth frames. + "rgbd" - display both RGB and depth frames. + + This function listens for the Ctrl+C signal to stop gracefully. + """ + stop_flag = [False] # Mutable flag to control the thread termination + + def signal_handler(sig, frame): + """ + Signal handler for gracefully exiting the program on Ctrl+C. + + Args: + sig: Signal number. + frame: Current stack frame. + """ + stop_flag[0] = True + print("Ctrl+C detected. Exiting...") + + # Bind the SIGINT signal (Ctrl+C) to the signal handler + signal.signal(signal.SIGINT, signal_handler) + + def process_frames(): + """ + Fetches frames from the RealSense camera and displays them + in a loop until the stop_flag is set to True. + """ + try: + while not stop_flag[0]: + # Fetch frames non-blocking + frames = self.pipeline.poll_for_frames() + if not frames: + continue + + # Retrieve color and depth frames based on the selected option + color_frame = frames.get_color_frame() if option in ["rgb", "rgbd"] else None + depth_frame = frames.get_depth_frame() if option in ["d", "rgbd"] else None + + # Skip iteration if frames are not available for the selected mode + if option in ["rgb", "rgbd"] and not color_frame: + continue + if option in ["d", "rgbd"] and not depth_frame: + continue + + # Display RGB image if available + if color_frame: + color_image = np.asanyarray(color_frame.get_data()) + cv2.imshow('RGB Frame', color_image) + + # Display depth image if available + if depth_frame: + depth_image = np.asanyarray(depth_frame.get_data()) + # Normalize depth data and apply color mapping for visualization + depth_colormap = cv2.convertScaleAbs(depth_image, alpha=255.0 / depth_image.max()) + cv2.imshow('Depth Frame', depth_colormap) + + # Exit loop if 'q' key is pressed + if cv2.waitKey(1) & 0xFF == ord('q'): + break + finally: + # Ensure resources are properly released + self.pipeline.stop() # Stop the RealSense pipeline + cv2.destroyAllWindows() # Close OpenCV display windows + + # Start the frame processing in a separate thread + thread = threading.Thread(target=process_frames) + thread.start() + + # Wait for the thread to finish (main thread blocks here) + thread.join() diff --git a/Sensor/__init__.py b/Sensor/__init__.py new file mode 100644 index 0000000..67815df --- /dev/null +++ b/Sensor/__init__.py @@ -0,0 +1,3 @@ +from .Camera import Camera + +__all__ = ["Camera"] \ No newline at end of file diff --git a/Sensor/__pycache__/Camera.cpython-38.pyc b/Sensor/__pycache__/Camera.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..735cccbdfaf6efec564190ea21c2145cdd27d969 GIT binary patch literal 6543 zcmai2O^h7Jb?(3E>7AXOT~gHYH&Tm|C5}Wc$rZ^c1Vb&g%cTiRE1FALQaf^^cdBN# zhdtf1)jdn@U=|5P8j^tsk&j6fU?#~Sz6FR6_R;VqFmm&)PX-JaCX#y)ADn!zy6106 zMLn46?)rIEUGKf`eXl;4n=2`}{_LOLZQT5pqWqR#CNCW?EBMEEQ82|(n98(3byRuR z99`ZGr+~X2n608yRI@&oW68D|XGY#jPU)7y3|4rou)-tFnPu9hQZ>8(UV7PHS@#3J z;kCG(U03Z@ya;cpy+^J0{80OBG-RLCSk;r9G zzmL6;>Tk9@KNwY`evkQKWBKyR7H->KD~ZA-&N|d}jmI_Nw-Y}Kv#GL$*!50Wqso{% zeGq$%;Zo&#HlmQ1o?wTRjjEa&Ykn=M(c9!@;$;Q@_Xg}Y*v+G< znXUK9UulZ+*G zxD6RSwWqjBPeZBpbd*}pK#8ZmF;L<&3AF!6{ZviNesQ3nrHCg>K3P4bY4vANDg#wT z8{SHZ*_>^b@eH|54R3SJ3ZC>qVY&y?n4YyV<3QW|sBK}?R_q@l`2$+*;pDmg5mw0O zJluain{%M<7kY*M;y~#i9Vq)|a;$m0XEskT6XnTP`ma*id`;Gzku_Dh zU$3(Y^f>SRCAPf;+mhGq&`*3X@Vh*=lWm?mhg}Q2IJO`7$+mr$d%-3TV{T_^w%eY7 z2PIs@OQn&V8=?_k9leq0kx|%Z;zU%h+V>#-_L?2l)VW@Ml7!z3S=Rpev+ z7C8x|a<65dVZ82jg2eV?`^u#Y)oDSQAx=ulYraX;rMC`>f-Mi_tkVv}cAE=3=Cvqf zb{*hA`}eg=dvj7;_l1q<>5YY;kk=B@`MWDK!GKDZQeZ= z^Ps*|lYr*7yhQj9-Gs-<)yA*i+gkYOe?DDlyh+!`$x7qDzWcKme)!*?u2i)rQhFJZ zHmYw3az|lNF+&Amc6y?=&1?5v>~9d0>fL;4K&iBRw-Y8n&6`3*BGp8kT5Z4013%=c ziGFpz;mq;S+X}dw*}&*E(^zWe_d+cSAi~7nVeO8w)evvt%XmB4cJ%JWD`_E4JdrrX zwutIL@zkJsQ5$gHc1jJNxS1s9s297tKI2hd;pQWp3Uz`N2|tYeTAXTOJ1uxZczdZ< zf9UA#_Cq}E(ZgO^s0R_&aro)|ObcyDNYc5frnJ!IB8t;l=En)Ph}Y|Js@HaxX^KuQ za*ks5JA%VNu{n&O8yo0}J*T{Od-a}s<)bU^^5qL@N%nRb+MUB|@T^)Ai8~`NOG{&q zs}@s(bp9-@i6c;*QyPEmPZUu_)gP+ox1$z6zvqSL-+^(w=Y{Tiz#sa%=gF@laXtff zM8wo`v0HxPx{DYXUqPXiOjZ7LOSSZpT2d?OY4wzPN?%Y*`hs3o7u2$9{ZZ4jKU#P{ zqn7o^8Rjy6uH~PkR_3V(esN@VYA(b^yP&q9_1pi4L3^ z;)du45lpIxv$OU(`G+0B@x=o_=40rQ&f0zP8D2iQg7xl+gD^M}zI1P+S`de@C20|B zPWVHF(}0I*F>|U|*l4S2;tX!$92KPOtazQC=23hb|Cl^oDOymRr5c*0c3;Ys^Yvan zr6ht2`b#C9#RI}ruBM*a*Pt$~r^P3j3gzg1qo?%?1BB##y$5wwweG)+)#J5c0(qzj zJc24H1R!A72~kl-EeWHUpH0MZSGDX@9 zqr~=h!Q2R?##-B`n&L3N&zXPV!3ls%u3T4~&J&JE@mY;HiEeP*7-p8iiA6n_mv~h} zssykmgG4$zF>LrEqr?GyrDms%@%%$b&Q(?-g+XN%wL6!qEbFsg)d(oJIt@YksF4c| zFVZ$4xztt&XZup^zMIi0(i0rY5{o-V0xk8lL~EJ@ZC}T|*tCd%HHn0jnD&MK7y7=j zU)VQ&Wxv=14ZA<{r7BKQ&;Be3o6#!_bkLM?GS@3S0!dp_KH1v0WP1gz)^UZAym|Z= z1_q?f?3X}`4p9%duqJ*g`yM9pm-Xt+^hzLBCa4;S560u)!`xC9p06o8O-a6r%zBKL z{__K6O?jlk)@FMuR(AwnwFnxe4K%Dw>mOyMfy!oAK&Dvv5uk+ne5vkyyEprx@{dF5OJ2vEn0XcY`oCnCuYIj{{G%s z`{7ypT=$bncT>e{u+)s2{O929)MCPWaO0sLH~#K#>H7KG5=f`eh-{rm(I)=!D>NJ$ z`Q+5T+C$m{dqK#MrFLAIRT}z_wH*Um(FOU1lqP2BiNF$Gq%B=Yb?ojzXDnR{5Y%f? zt9|{i(K9CeGZCt~!(d77o_(`BR?f8`sv#s?UmB0@&W|B}jXc)>y$&AtzmS)zJJsV7 za1hj!s4eJg(nAcWD5AX^LGYm;vgko9C=NSTZ8upZtaK`?@4x&0UH6?^cb9M7z45U? za;-RXSyXy2GcfTg>g47+&(WS{R>)c2oEC%^Hn>=#k;gF~04d>jqL^>O1Oh&k@)AjT z8AV#$2G%8s5N}Zxp`SQK_2#Y@ba1Int4H$yD`IL+tXJVZ_Q8)0ksG4{xf%SAx=BWED>8UOnTT{qOe4XqB z`c>jPD45z%f24h;)p4-F^vBk|26_XEgXB~q)Mv_PDq3}UbYV#pM$Ba97wW#zGbjND z#uyY$L3fING2T617^>b>=8(9cG~SoB(Z(> z);qupV&F1fZh_iN0K%A)PN#ZNcBql(d1Py%1;(2JPL4iyD`FF&mNLZ2>ohj=w0&-f zRdy5x;BU9y$%p}9Z?f~`^J(_fFuDG39*7yI#?r^iDG7e3WsGLOw+;R`Jg~6|BEXj=Ta1mEk`?q`+LeeY$2_+-9A8CD_DwQpw2Tv}Sf@m5Z0 zQyo(y8zcUNykUlR(X9twBlCJ9w`nEYWH@j1xSaV7rM=PCle0h)_I6Q9`8NH_OQ@j6HqTr-4g?~}d^)LloN znb!^b-RY_E9XX16`~Xz>2Rq1aL@z1!EfDCLj4Xpjfc*4yqAop6{Htu$@>YcqkX7cR^&Gzvy!xENLPrS zm%D#6rPXzwAT5AROZJLe&d8sF6zL^H073@T8p8=$hD4kog8kER0lWc+BO^u9Sxw9n zauG_frfo%<3KE-lp#%g0co13hT7E23{5AH_-~6hj>AI+w0D*5p-qE zS-`)BF5=HoROe-+71ybP0JZR7NS|pdnneZ{U!?J5^vJ;CF(mX@gx@}BVk@6GXbZPHOow)<+XpJr)Gw!S^RvT zuu#VJdMt+F&RpzF2C@m-) z3QBDtF6$`2=L4#iA63kq^`B7_S5PQ(rdBRjw1qctyr6e0(>}ccMo0cWJ|wHE>p&1P zn(7>#V}vjf*G)^Vi^$W#VU(MeU3aJB1$m1Ongs@e!=X1YC^d*S%cNQItt`VU=J}=g z4$b9KA=f=FWU5Wz{XG7035BIDE?4HQ`6Jf_)mO`D(REo=bKQ*K6#%B~Jt^NQ<6(Hn xwwz`ACQkOSp@M>fI8Md)srZ-*lJ~3yq}H|UBd=4Bcmah`BF1CU*$n-({{tmh%ohLv literal 0 HcmV?d00001 diff --git a/Sensor/__pycache__/__init__.cpython-38.pyc b/Sensor/__pycache__/__init__.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..195bd75eed88ebdb0bb1cfcd858f73e2cea9a135 GIT binary patch literal 185 zcmWIL<>g`kg523Y=>|ahF^Gc3@J?Mj8Tj!%)tzr zEH4>>lA4US*qjq{Q;QOdK$`qCnZWcd_W1b3oSgXhl?+8JKyfhfOItr9KQ~psGBHoz zDYdx7H!&~XEhn`ivrIoYHLp0oNIyP4GcU6wK3=b&@)n0pZhlH>PO2TqnqrXQJWK%N CA1H_b literal 0 HcmV?d00001 diff --git a/Sensor/__pycache__/camera.cpython-38.pyc b/Sensor/__pycache__/camera.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..7624df0e6946fe7e2c3f4d2ea2690f0701e51f70 GIT binary patch literal 4309 zcmai1-ESOM6~A}BcJ|Yb<2a6+CT`l&YzuapHldWzI*FS!(7LG|nsh`NjrU%!XS2Jr zzB6|0)fk~VNPVdk9)Tk5_KkNQK;nPk0STdbs>BNsyd!uh{LY+mbrntz;O?636k=w;$#0Z;s62*CtzGcSL0&(PndXW?zM?T+I)oq|{B zxSq?YjoB`CN?u9pR=eD(conVlcGat1Wx^KDT_&78!>b9t!s>qnCq^1w8Qw5R!RZFf4OYn$n_MSf5#X zTHCXwBMP5!;fms2%PWYIDBoqCD~?D<7F#7z{mc+GF?QGX%3?|!5)*g1R}qJ?%%hoe zIp|0g-2bV;*!?d!#0gASx3EOH6*ihM)c zoL0G2fsS!o2EMAT8Z>igKi)zZ!pv)hb)atSLEZR+y29WPjX$K-P9%p1lfoX&IWahr z&pG6~c4`l%hHP+j$abA%x^*mdS~J3de5z^c_i^Y?la9O4G@E8}Lb#vO?oTGOg7$Lq zSM1lu9`-~1aXyYH>-Lq=`(VFklIg)IQPnGZHhHe~{NNiTTQBI6)4HUt?e(HyILF#j z|HM)>yDWqDitNU6Hb1=CM%3-fMk2+Wa&b7SNP$exJ5B2@nu&N*@(QO znEkqvLaxC$g2)604AhQSS-f`nhJXIndH?LWGg(Qu_66#_iA9;f1tN7lP&m~@!oH>6 z1HE3zEIRThotm1)(RrnZyZsGQ-$K#9&&;n!9XY=fbmuR@H{K1p{*|`e4!7n%2;S z&}UnAvqJ93v3d@5bwhmDNIMMi3Z9q%#kdPAx!f{b-hXCfiT{nP$1Fk! zrAJ$YrUNaFY?AVU0oxd<5zo-h6LVmt#=stef_KdnwyE=_x}&DhD;v8L;2eQ4e~tVp z&c+B48hCk$I!Vn+K^$~a>D`d`Q5B=o@j65<# ziOc{))Of!-GDP0yO5GsHuWmNg88mN@7wlhTL!v-!gD(8#9`C=C2PN9QV7Fa0wP7XL zduk*`%NZKGCfO^|&h zsiyWGLfsU} z2LZ1nZ1l&IVs1Sa^b2gGr9&7($z9Y8jtto%+v7OFa>}u~N$hG$xT!HTurgzCR1}9? zlok+@M0pQrpf>mT#@lK6Cj0#S3WB&IOMGn~Wl@g}3q6oIq0h*tuu7V99sSKAD!6lTJB*vZ`~_XVe^Z0*G1sLdq-Ur$kN}CQXy;W zs;=fF^R8Y}tX@Q!Hm-M=Y?{-)6r<|QRzVlQA};ifVLsq2k=LVGu7urYTkenYJdN@K zL{?Y_gp)+6?@9t zmRT~!jd4C}RQQZBV@{h6V#_qOF)e6Q+<{g?JnBF7HHV#@===ro?7Y;y4D)iIv|uv3UWxDL3PNUVw9oo7IdFt zS{RW+O_V?iWst&{s1PZrzuW{#ZcNjR7E8u&mxv}f+r@8De+KKRrtW@jdy{?0K6i)= z&H?Vb?oZrXDA}-X;i0mJGj$AAq{6m9-z^z7D;=R#5o90hew@)j>01mSPmyjgVeW4 zyscXzL)elkSplKQi>ts&C)ifksQ&vTCL!v@Q3Q>8#6OCBU3~`vzjkDR(Ht6iqY@oZ z@ms2^b-?Wey^Mb)#ukut_S(TtAEU)Y6NDkdsF;&{64@AqTP=Z@8gem`Q>dNdC1V2m zEb!B>jnFe13jeP%6TK)$~JJ1=8Jf5x79{n|T Date: Tue, 17 Dec 2024 23:19:06 +0800 Subject: [PATCH 02/24] update --- RoboticsToolBox/Bestman_real_xarm6 copy.py | 574 ----------- .../Bestman_real_xarm6.cpython-38.pyc | Bin 19177 -> 0 bytes .../Bestman_sim_flexiv.cpython-38.pyc | Bin 20028 -> 0 bytes .../__pycache__/utility.cpython-38.pyc | Bin 2142 -> 0 bytes RoboticsToolBox/utility.py | 87 -- Robotics_API/Bestman_Real_Flexiv.py | 898 ------------------ .../Bestman_Real_Xarm.py | 139 ++- Robotics_API/__init__.py | 2 +- .../Bestman_Real_Flexiv.cpython-312.pyc | Bin 43835 -> 0 bytes .../Bestman_Real_Flexiv.cpython-38.pyc | Bin 28893 -> 0 bytes .../__pycache__/Bestman_flexiv.cpython-38.pyc | Bin 18731 -> 0 bytes .../__pycache__/Bestman_flexiv.cpython-39.pyc | Bin 18749 -> 0 bytes .../Bestman_sim_flexiv.cpython-38.pyc | Bin 20037 -> 0 bytes Robotics_API/__pycache__/Pose.cpython-312.pyc | Bin 5528 -> 0 bytes Robotics_API/__pycache__/Pose.cpython-38.pyc | Bin 3494 -> 0 bytes Robotics_API/__pycache__/Utils.cpython-38.pyc | Bin 4765 -> 0 bytes .../__pycache__/__init__.cpython-312.pyc | Bin 219 -> 0 bytes .../__pycache__/__init__.cpython-38.pyc | Bin 212 -> 0 bytes .../__pycache__/utility.cpython-38.pyc | Bin 3971 -> 0 bytes Robotics_API/__pycache__/utils.cpython-38.pyc | Bin 3969 -> 0 bytes Robotics_API/__pycache__/utils.cpython-39.pyc | Bin 3965 -> 0 bytes 21 files changed, 102 insertions(+), 1598 deletions(-) delete mode 100644 RoboticsToolBox/Bestman_real_xarm6 copy.py delete mode 100644 RoboticsToolBox/__pycache__/Bestman_real_xarm6.cpython-38.pyc delete mode 100644 RoboticsToolBox/__pycache__/Bestman_sim_flexiv.cpython-38.pyc delete mode 100644 RoboticsToolBox/__pycache__/utility.cpython-38.pyc delete mode 100644 RoboticsToolBox/utility.py delete mode 100644 Robotics_API/Bestman_Real_Flexiv.py rename RoboticsToolBox/Bestman_real_xarm6.py => Robotics_API/Bestman_Real_Xarm.py (84%) delete mode 100644 Robotics_API/__pycache__/Bestman_Real_Flexiv.cpython-312.pyc delete mode 100644 Robotics_API/__pycache__/Bestman_Real_Flexiv.cpython-38.pyc delete mode 100644 Robotics_API/__pycache__/Bestman_flexiv.cpython-38.pyc delete mode 100644 Robotics_API/__pycache__/Bestman_flexiv.cpython-39.pyc delete mode 100644 Robotics_API/__pycache__/Bestman_sim_flexiv.cpython-38.pyc delete mode 100644 Robotics_API/__pycache__/Pose.cpython-312.pyc delete mode 100644 Robotics_API/__pycache__/Pose.cpython-38.pyc delete mode 100644 Robotics_API/__pycache__/Utils.cpython-38.pyc delete mode 100644 Robotics_API/__pycache__/__init__.cpython-312.pyc delete mode 100644 Robotics_API/__pycache__/__init__.cpython-38.pyc delete mode 100644 Robotics_API/__pycache__/utility.cpython-38.pyc delete mode 100644 Robotics_API/__pycache__/utils.cpython-38.pyc delete mode 100644 Robotics_API/__pycache__/utils.cpython-39.pyc diff --git a/RoboticsToolBox/Bestman_real_xarm6 copy.py b/RoboticsToolBox/Bestman_real_xarm6 copy.py deleted file mode 100644 index a7cd07b..0000000 --- a/RoboticsToolBox/Bestman_real_xarm6 copy.py +++ /dev/null @@ -1,574 +0,0 @@ -# !/usr/bin/env python -# -*- encoding: utf-8 -*- -""" -# @FileName : Bestman_real_xarm6.py -# @Time : 2024-12-17 22:22:23 -# @Author : Zhaxi & Yan -# @Email : zhaxizhuoma.ayang@gmail.com & yding25@binghamton.edu -# @Description : XXX -# @Usage : XXX -""" - - -import numpy as np -import argparse -import time -import datetime -import ikpy -import serial -import serial.tools.list_ports -from ikpy.chain import Chain -from ikpy.inverse_kinematics import inverse_kinematic_optimization -from scipy.spatial.transform import Rotation as R -import sys -import os -import minimalmodbus as mm -import pyRobotiqGripper - -from xarm.wrapper import XArmAPI -current_dir = os.path.dirname(os.path.abspath(__file__)) - - -class Bestman_Real_Xarm6: - ''' - Functions for initalization - ''' - - def __init__(self, robot_ip, local_ip, frequency): - # Initialize the robot and gripper with the provided IPs and frequency - self.robot = XArmAPI(robot_ip) - local_ip = None - self.mode = self.robot.set_mode(0) # 0: default - self.robot_states = self.robot.set_state(0) - self.first_init_flag = True - self.gripper = True # have gripper by default - self.frequency = frequency - - # # URDF - # urdf_file = os.path.join(current_dir, "../Asset/xarm6_robot.urdf") - # self.robot_chain = Chain.from_urdf_file(urdf_file) - # self.active_joints = [ - # joint for joint in self.robot_chain.links - # if isinstance(joint, ikpy.link.URDFLink) and (joint.joint_type == 'revolute' or joint.joint_type == 'prismatic') - # ] - - ''' - Functions for xarm itself - ''' - def clear_fault(self): - '''Clear fault, and updates the current robot states.''' - self.robot.set_state(0) - - def update_robot_states(self): - '''Updates the current robot states.''' - self.robot.getRobotStates(self.robot_states) - - def set_mode(self, _mode): - ''' - Parameters: - _mode= - 0: position controlmode - 1: servo motionmode - 2: joint teachingmodemode (invalid)3: cartesian teaching - 4: joint velocity control mode - 5: cartesian velocity control mode - 6: joint online trajectory planningmode - 7: cartesian online trajectory planning - - Notes: - https://github.com/xArm-Developer/xArm-Python-SDK/blob/master/doc/api/xarm_api.md#mode - ''' - print("set mode") - self.robot.set_mode(_mode) - - def go_home(self,dist=0): - '''Move arm to initial pose.''' - - self.robot.set_position(x=396.4+dist, y=-5.5,z=360,roll=-90,pitch=-90,yaw=-90,wait=False) - time.sleep(3) - - def pose_to_euler(self, pose): - ''' - Convert robot pose from a list [x, y, z, qw, qx, qy, qz] to [x, y, z] and Euler angles. - - Parameters: - pose: list of 7 floats - [x, y, z, qw, qx, qy, qz] - - Returns: - tuple: (x, y, z, roll, pitch, yaw) where (x, y, z) is the position and (roll, pitch, yaw) are the Euler angles in radians. - ''' - x, y, z, qw, qx, qy, qz = pose - r = R.from_quat([qx, qy, qz, qw]) # Reordering to match scipy's [qx, qy, qz, qw] - roll, pitch, yaw = r.as_euler('xyz', degrees=False) - return [x, y, z, roll, pitch, yaw] - - def euler_to_pose(self, position_euler): - ''' - Convert robot pose from [x, y, z, roll, pitch, yaw] to [x, y, z, qw, qx, qy, qz]. - - Parameters: - position_euler: list of 6 floats - [x, y, z, roll, pitch, yaw] - - Returns: - list: [x, y, z, qw, qx, qy, qz] - ''' - x, y, z, roll, pitch, yaw = position_euler - r = R.from_euler('xyz', [roll, pitch, yaw], degrees=False) - qx, qy, qz, qw = r.as_quat() # Getting [qx, qy, qz, qw] from scipy - return [x, y, z, qw, qx, qy, qz] # Reordering to match [qw, qx, qy, qz] - - def set_pay_load(self,payload_weight, payload_cog): - - self.robot.set_tcp_load(payload_weight, payload_cog) - - - # ---------------------------------------------------------------- - # Functions for others - # ---------------------------------------------------------------- - - def get_joint_bounds(self): - ''' - Retrieves the joint bounds of the robot arm. - - Returns: - list: A list of tuples representing the joint bounds, where each tuple contains the minimum and maximum values for a joint. - ''' - maxbounds = self.robot.info().qMax - minbounds = self.robot.info().qMin - jointbounds = list(zip(maxbounds,minbounds)) - return jointbounds - - - def get_DOF(self): - ''' - Retrieves the degree of freedom (DOF) of the robot arm. - - Returns: - int: The degree of freedom of the robot arm. - ''' - return 6 - - # ---------------------------------------------------------------- - # Functions for joint control - # ---------------------------------------------------------------- - - def print_joint_link_info(self, name): - ''' - Prints the joint and link information of a robot. - - Args: - name (str): 'base' or 'arm' - ''' - if name == 'base': - print("Base joint and link information:") - for i, link in enumerate(self.robot_chain.links[:1]): # Assuming the base is the first link - print(f"Link {i}: {link.name}") - elif name == 'arm': - print("Arm joint and link information:") - for i, link in enumerate(self.robot_chain.links[1:]): # Assuming the arm starts from the second link - print(f"Link {i + 1}: {link.name}") - - def get_joint_idx(self): - ''' - Retrieves the indices of the joints in the robot arm. - - Returns: - list: A list of indices for the joints in the robot arm. - ''' - return list(range(len(self.active_joints))) - - def get_tcp_link(self): - ''' - Retrieves the TCP (Tool Center Point) link of the robot arm. - - Returns: - str: The TCP link of the robot arm. - ''' - return self.robot_chain.links[6].name - - def get_current_joint_angles(self): - ''' - Retrieves the current joint angles of the robot arm. - - Returns: - list: A list of the current joint angles of the robot arm. - ''' - _joint_states = self.robot.get_joint_states(is_radian=True) - _joint_angles = _joint_states[1][0][0:6] - - return _joint_angles - - def get_current_joint_velocities(self): - ''' - Retrieves the current joint velocities of the robot arm. - - Returns: - list: A list of the current joint velocities of the robot arm. - ''' - - _joint_states = self.robot.get_joint_states(is_radian=True) - _joint_velocities = _joint_states[1][1][0:6] - - return _joint_velocities - - - - def move_arm_to_joint_angles(self, joint_angles, target_vel=None, target_acc=None, MAX_VEL=None, MAX_ACC=None, wait_for_finish=None): - ''' - Move arm to a specific set of joint angles, considering physics. - - Args: - Set the servo angle, the API will modify self.last_used_angles value - Note: -     https://github.com/xArm-Developer/xArm-Python-SDK/blob/master/doc/api/xarm_api.md#def-set_servo_angleself-servo_idnone-anglenone-speednone-mvaccnone-mvtimenone-relativefalse-is_radiannone-waitfalse-timeoutnone-radiusnone-kwargs - ''' - - #! Force mode switch - self.robot.set_mode(6) # 6: online joint - self.robot.set_state(0) - - self.robot.set_servo_angle(angle=joint_angles, is_radian=True, speed=0.7, wait=wait_for_finish) # speed in rad/s - - def move_arm_follow_joint_angles(self, target_trajectory, target_vel=None, target_acc=None, MAX_VEL=None, MAX_ACC=None): - ''' - Move arm to a few set of joint angles, considering physics. - - Args: - target_trajectory: A list of desired joint angles (in radians) for each joint of the arm. - target_vel: Optional. A list of target velocities for each joint. - target_acc: Optional. A list of target accelerations for each joint. - MAX_VEL: Optional. A list of maximum velocities for each joint. - MAX_ACC: Optional. A list of maximum accelerations for each joint. - ''' - period = 1.0 / self.frequency - self.update_robot_states() - DOF = len(self.robot_states.q) - - for target_pos in target_trajectory: - # Monitor fault on robot server - if self.robot.isFault(): - raise Exception("Fault occurred on robot server, exiting ...") - - # Send command - self.robot.set_servo_angle(angle=target_pos, is_radian=True, speed=target_vel, wait=True) # speed in rad/s - print(f"Sent joint positions: {target_pos}") - - # Use sleep to control loop period - time.sleep(period) - - # ---------------------------------------------------------------- - # Functions for end effector - # ---------------------------------------------------------------- - - # TODO - def get_current_tcp_speed(self): - ''' - Retrieves the current tcp velocities of the robot arm. - - Returns: - list: A list of the current tcp velocities of the robot arm. - ''' - speed = self.robot.realtime_tcp_speed - return speed - - - def get_current_end_effector_pose(self): - ''' - Retrieves the current pose of the robot arm's end effector. - - This function obtains the position and orientation of the end effector. - - Returns: - pose: the [x, y, z, roll, pitch, yaw] value of tcp in meter and radian - ''' - - _pose = self.robot.get_position(is_radian=True) - pose = _pose[1] - pose[0] = pose[0] / 1000 - pose[1] = pose[1] / 1000 - pose[2] = pose[2] / 1000 - return pose - - - - def move_end_effector_to_tcp_velocity(self, _velocity_setpoint, _duration): - ''' - Move arm's end effector to a target tcp velocity. - - Args: - _velocity_setpoint: mm/s in transition, rad/s in orientation - _duration: function calling loop time - Notes: - https://github.com/xArm-Developer/xArm-Python-SDK/blob/master/doc/api/xarm_api.md#def-vc_set_cartesian_velocityself-speeds-is_radiannone-is_tool_coordfalse-duration-1-kwargs - ''' - - self.robot.set_mode(5) # 5 for cartesian vel - self.robot.set_state(0) - self.robot.vc_set_cartesian_velocity(speeds=[_velocity_setpoint[0], - _velocity_setpoint[1], - _velocity_setpoint[2], - _velocity_setpoint[3], - _velocity_setpoint[4], - _velocity_setpoint[5]], - duration=_duration) - - def move_end_effector_to_goal_pose(self, end_effector_goal_pose, speed=1000, mvacc=50000, wait=False): - ''' - Move arm's end effector to a target position. - - Args: - end_effector_goal_pose (Pose): The desired pose of the end effector (includes both position in mm and euler_orientation), please use radian for orientaiton. - ''' - - self.robot.set_position(x=end_effector_goal_pose[0]*1000 , y=end_effector_goal_pose[1]*1000,z=end_effector_goal_pose[2]*1000, - roll=end_effector_goal_pose[3],pitch=end_effector_goal_pose[4],yaw=end_effector_goal_pose[5], speed=speed, mvacc=mvacc, is_radian=True, wait=wait) - - - - def rotate_end_effector_joint(self, angle): - ''' - Rotate the end effector of the robot arm by a specified angle by joint. - - Args: - angle (float): The desired rotation angle in radians. - ''' - current_joint_angles = self.get_current_joint_angles() - - target_joint_angles = current_joint_angles.copy() - target_joint_angles[6] += angle - target_vel = 1 - self.robot.set_mode(6) # 0: joint control mode; 6: online joint - self.robot.set_state(0) - - self.robot.set_servo_angle(angle=target_joint_angles, is_radian=True, speed=target_vel, wait=False) # speed in rad/s - - # ---------------------------------------------------------------- - # Functions for IK - # ---------------------------------------------------------------- - - def joints_to_cartesian(self, joint_angles): - ''' - Transforms the robot arm's joint angles to its Cartesian coordinates. - - Args: - joint_angles (list): A list of joint angles for the robot arm. - - Returns: - tuple: A tuple containing the Cartesian coordinates (position and orientation) of the robot arm. - ''' - # Validate the number of joint values matches the number of active joints - if len(joint_angles) != len(self.active_joints): - raise ValueError("The number of joint values does not match the number of active joints") - - # Map joint values to the full joint chain - full_joint_angles = np.zeros(len(self.robot_chain.links)) - active_joint_indices = [self.robot_chain.links.index(joint) for joint in self.active_joints] - - for i, joint_value in enumerate(joint_angles): - full_joint_angles[active_joint_indices[i]] = joint_value - - # Calculate the end effector position and orientation - cartesian_matrix = self.robot_chain.forward_kinematics(full_joint_angles) - - # Extract position and orientation - position = cartesian_matrix[:3, 3] - orientation_matrix = cartesian_matrix[:3, :3] - - # Convert rotation matrix to quaternion - r = R.from_matrix(orientation_matrix) - quaternion = r.as_quat() - orientation = [quaternion[3], quaternion[0], quaternion[1], quaternion[2]] - - return position, orientation - - def cartesian_to_joints(self, position, orientation): - ''' - Transforms the robot arm's Cartesian coordinates to its joint angles. - - Args: - position (list): The Cartesian position of the robot arm. - orientation (list): The Cartesian orientation of the robot arm. - - Returns: - list: A list of joint angles corresponding to the given Cartesian coordinates. - ''' - rotation_matrix = R.from_euler('xyz', orientation).as_matrix() - - # Combine rotation matrix and position into a list - target_pose = np.eye(4) - target_pose[:3, :3] = rotation_matrix - target_pose[:3, 3] = position - - initial_joint_angles = [0] * len(self.robot_chain) - - # inverse kinematics calculations and return joint angles - joint_angles = ikpy.inverse_kinematics.inverse_kinematic_optimization( - chain=self.robot_chain, - target_frame=target_pose, - starting_nodes_angles=initial_joint_angles, - orientation_mode='all', - ) - - return joint_angles[1:8] - - - def calculate_IK_error(self, goal_position, goal_orientation): - ''' - Calculate the inverse kinematics (IK) error for performing pick-and-place manipulation of an object using a robot arm. - - Args: - goal_position: The desired goal position for the target object. - goal_orientation: The desired goal orientation for the target object. - ''' - pass - - # ---------------------------------------------------------------- - # Functions for gripper - # ---------------------------------------------------------------- - - def find_gripper_xarm(self): - ''' - Searches for the gripper on available serial ports and returns the port if found. - - Returns: - str: The serial port where the gripper is connected, or None if not found. - ''' - _pos = self.robot.get_gripper_position() - _ver = self.robot.get_gripper_version() - - if _ver is not None and _pos is not None: - print("Have Xarm gripper", _ver) - return True - else: - print("Not found Xarm gripper") - return None - - - def find_gripper_robotiq(self): - """ - Config the parameter via Python SDK - """ - # Baud rate - # Modify the baud rate to 115200, the default is 2000000. - self.robot.set_tgpio_modbus_baudrate(115200) - - # TCP Payload and offset - # Robotiq 2F/85 Gripper - self.robot.set_tcp_load(0.925, [0, 0, 58]) - self.robot.set_tcp_offset([0, 0, 174, 0, 0, 0]) - self.robot.save_conf() - - # Self-Collision Prevention Model - # Robotiq 2F/85 Gripper - self.robot.set_collision_tool_model(4) - - self.robot.robotiq_reset() - self.robot.robotiq_set_activate() #enable the robotiq gripper - - - def get_gripper_position_xarm(self): - ''' - Get the position of the XArm gripper. - ''' - gripper_pos = self.robot.get_gripper_position() - - return gripper_pos[1] - - - def get_gripper_position_robotiq(self, number_of_registers=3): - """ - Reading the status of robotiq gripper - - :param number_of_registers: number of registers, 1/2/3, default is 3 - number_of_registers=1: reading the content of register 0x07D0 - number_of_registers=2: reading the content of register 0x07D0/0x07D1 - number_of_registers=3: reading the content of register 0x07D0/0x07D1/0x07D2 - - Note: - register 0x07D0: Register GRIPPER STATUS - register 0x07D1: Register FAULT STATUS and register POSITION REQUEST ECHO - register 0x07D2: Register POSITION and register CURRENT - :return: tuple((code, robotiq_response)) - code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details. - robotiq_response: See the robotiq documentation - """ - status = self.robot.robotiq_get_status(number_of_registers=number_of_registers) - gripper_width = status[1][-2] - return gripper_width - - - - def gripper_goto_xarm(self, value, speed=5000, force=None): - ''' - Moves the gripper to a specified position with given speed. - - Args: - value (int): Position of the gripper. Integer between 0 and 800. - 0 represents the open position, and 255 represents the closed position. - speed (int): Speed of the gripper movement, between 0 and 8000. - force (int): Not applicable for xarm gripper - - Note: - - 0 means fully closed. - - 800 means fully open. - ''' - self.robot.set_gripper_position(pos=value, speed=speed, wait=False, timeout=1, auto_enable=True) - - - def gripper_goto_robotiq(self, pos, speed=0xFF, force=0xFF, wait=False, timeout=5, **kwargs): - """ - Go to the position with determined speed and force. - - :param pos: position of the gripper. Integer between 0 and 255. 0 being the open position and 255 being the close position. - :param speed: gripper speed between 0 and 255 - :param force: gripper force between 0 and 255 - :param wait: whether to wait for the robotion motion complete, default is True - :param timeout: maximum waiting time(unit: second), default is 5, only available if wait=True - - :return: tuple((code, robotiq_response)) - code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details. - robotiq_response: See the robotiq documentation - """ - return self.robot.robotiq_set_position(pos, speed=speed, force=force, wait=wait, timeout=timeout, **kwargs) - - - def open_gripper_xarm(self): - ''' Opens the gripper to its maximum position with maximum speed and force. ''' - self.gripper_goto(value=850, speed=5000, force=None) - - - def open_gripper_robotiq(self, speed=0xFF, force=0xFF, wait=False, timeout=5, **kwargs): - """ - Open the robotiq gripper - - :param speed: gripper speed between 0 and 255 - :param force: gripper force between 0 and 255 - :param wait: whether to wait for the robotiq motion to complete, default is True - :param timeout: maximum waiting time(unit: second), default is 5, only available if wait=True - - :return: tuple((code, robotiq_response)) - code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details. - robotiq_response: See the robotiq documentation - """ - return self.robot.robotiq_open(speed=speed, force=force, wait=wait, timeout=timeout, **kwargs) - - - def close_gripper_xarm(self): - '''Closes the gripper to its minimum position with maximum speed and force.''' - self.gripper_goto(value=0, speed=5000, force=None) - - - def close_gripper_robotiq(self, speed=0xFF, force=0xFF, wait=False, timeout=5, **kwargs): - """ - Close the robotiq gripper - - :param speed: gripper speed between 0 and 255 - :param force: gripper force between 0 and 255 - :param wait: whether to wait for the robotiq motion to complete, default is True - :param timeout: maximum waiting time(unit: second), default is 3, only available if wait=True - - :return: tuple((code, robotiq_response)) - code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details. - robotiq_response: See the robotiq documentation - """ - return self.robot.robotiq_close(speed=speed, force=force, wait=wait, timeout=timeout, **kwargs) \ No newline at end of file diff --git a/RoboticsToolBox/__pycache__/Bestman_real_xarm6.cpython-38.pyc b/RoboticsToolBox/__pycache__/Bestman_real_xarm6.cpython-38.pyc deleted file mode 100644 index 390b55aba469c3b5d05b47d65387d5015261ffc5..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 19177 zcmeHPTWlQHdES}5FE64f>gtQeS7~FKmgUHHST`1Ri)CAsBFT6M}0)1^?ieB;*ps&F^6)2EGKwI>oPeB0p`_7q}oxPA& z(ga0-EHP)!%$zxM&VT;<{~x|FF;Pn4_pQJAe(lvaQmKFAMe>)%#c3SgU!xEyVKh>< zq0Xjls&m>-t8>QAsB_lNs&mfHsdHYP%|>ChXcrCkk#3Y$$Lujx&ostYC+ta8&o=g~ zPT5nco#=>K?)fJ@U!tCWYeB9qT)9}~cay&?E>7d{jvJ|z zs~`yI8Nz%w1?Z(k2Iq{(iX6^akrxG=b9z2@UW|!xTo=TIn8dj#_J}E*OJc8>#(7Na z6EirEi~ZsN&J*IGIE3@0I4q9fyhl7B9>jS{JR}~*d9OGsES#ssBjQn<_lZ}<*C`$9~Cc)(>PmVL7c()5ph|Z6X((6qvC@21nxX0 zUJ(~@eq20;^}7;GpK(2Z)oE6)x=y3=mLpf6uVxZ}Wt|v$masjI!cTRLJ4QEE0gi5& z-PDS)4LINBowT~jgjZRCrdZ`*}NtBM)c+j0-S`hH68#p%Zvo>^|Ky3bsB_4S1_&#bR?(CgYW1j!|UgwS~A zDo0nZde>X6#+lZJdPsOl!fS)@nrm+cg^C_fG-@Q}POG=60IkHT2!g-IS5?v)cf2e2*>9sg`+>=3N3^T^B?q zY-Mg2L^gh-$X-Nl3rmPEmF}jzuc@Pv)1sfR<*h&r7Nsf(^89Id9DO!n6-SlS#K^`e#@#hTt`}Sb947N>(1l)&8pLE zwtVY`tDn1K)^a!MKHovpjurOb->p^mqUN3H^G)YQgYET)@s>ildxY~na^ud%HNW0y zSnH1Nx7LzjTa4<4TLn&7EufTQQ%+;nYGb4x2DI!}Z(9Dc;~zumhVMLEww4{wYPPHz zr#@?)EOR(FS}h;*uQ$=>!Ws(Vb?1;mC_Znt){`G8k4X+K$k#nJ!yw&gEd`l%M>g$} zf|S0U0W)=jtk-bec93|~PkNYFQ+O3l+7$}_ zburD)j;T*n)C``(4ev=5DYImZ8zuEO4s;$*<0_p|*9D^h#5O<3Wjha#K<-3$efZ5a zi1C|WR#->eQ#h=|irIPOjkX~BS5Q`n)bt}XQgUH#v7A=$vL}~ZU%~X6dLT)B?;C2) zgLB-srh2U?v8ul_>;7Unqn1xiS5BaZV4SnU7w{?Q z2^wpIjDYYT08x-%YE{U$-^6QPj-NNjft#HpBi1i$+6O6qE{{X4*(<1Yjkoh#=2m(u zvz6V-ZRNKLK6q|7*Ufhe-Rx54uCZ0@7FPg2VT$y7=G*D5($?74c-Mqr%XCZKvF`X% z4sgpjp8*s~R`@&HqB0a=E}>`DWNX!OEb27Yr#EJ;%~`86Yu#GMfva1%yw!P!FpnO- zV>wM>ou`U~+ET;y=HmY2$`waCtF8~lYd*fm&&}%(wQAN2R;|%;e9wA1?s@35@vCv= zs_U=GW};Jn4Xkk9Iv$Ul%M1R<<;7&2^|H0T>`FIop;#e#Wwf$tI>(1!bMO+|C1(o6 zv!o;HPBSs-Aic5Kxd3X)3wKGn&=J#tan*+OZ>?6SPuc~?t0;^+j1l*jq7%;F;~s3Pb>vB(MbQt?~5@6X_&M-3)bnl`458KYy3 zK)=3MJ`@RaNhI{4TM46^B^oY)T!DD$twOi3ZQf12o!Kh(q2PG8NHko^D#9+5OHV(F$Tt!Aygy@&EE$y%V)4g7vu?)kQpa& z3Q7uB6p93SAPcc3#1(roLR3BQ0jwEBkr&aOJjLR97B8^)D2qYdQ6D4n5F=L6BE+33 zZ@%<+P)(pkvYx4 zaDHr9O7)WaF-SRs;vVO5j}y2@o9!OicgM6!kmo@LI#-jL`ckD^F9fRNpc#O^ulc+1}?(I;WS!qEBfOfbBWy}I+?kSRxwsWQATm8J3 zLPaC}Ld@0`KlUu?wx#R2O{FEX!{kS1Lze71)n!Y)U_le~VXtfbY8B#rbq!Jpa%R=p zQ1#nRV-4e_V(jS7dSqYDY0>=?&Z;BFCuf0#Ms}{iXgreJDqstuTV?WYophZD+7b(CgdSjgkBX`RX`m_>%Os+MuC3Cla)U9ggIDP_3bLG=7i2ip5Od@w`GQd=PBm{1 zBFTQGM~7&_2C%+sEv~FbR*}lnIC;=Gf|BRX@evp^vWq_Qr>WAX+V{v`5%_uxKggy8 zfnPlLQ9k5u#rKS}t8v@qU5NR>z!Sfff^d<&1Y zQIYRFGy)*KmODd%9K4&_PASJjNHllo2{bxi*#R0_N#k_jo^b`V-{a>lUnq~5U$TQ? z&KHAdxWipQm64BN$gri|rm_qJm_`>qSc*^Y zSo#i(!4N{cf4FhhP{}8;y8U8#Um=5VycEv-71L z;CKD(73(}>qGeGGiw1E|5Z*GtVo-5Y9injK&x=8*j z&!BkU)UKX{ZV*NX(poi~2t|RuYe$#bTpnbJhqx5s@WS_W!ygc;)Du$qNn^(BJhj8p zjC%KHSSGbp1IzU7B%pU(rrR!@*)XntAxpJu=kan$n!f|fB!3h4Kn@ha(m`SG4dL7C zb8s;p9A1oG;~%0Ay&wg(AhGkk7p$1u?ft$*A#aKK3wd-T%@y5%%aF%7aE13Q=YFvs zVKfc6Ard*}SuT_`w^nnjeoH1avg^z6_14y!V5C&qZuIo-$k0@ZcMGcxojlU)$gy|m zaL5qmqHs|2iqG$6)+?=EjlJ5YfLYZ_zm}*m>ZxUg15UZ472}FV8hYjItg>PU*Ycs{ zT43;%2pX#OkGPl=lBrV3?40-z+8TO&u}Wqe{aBAR`V+X&i`nPvjAJ4zX=v)u?7KOU zeJ{gU2bMbT{3N;5NpGiPd2P2^uhpwi#}LV=#kwuI<1CFQuP&f=A;QpJ-t-V1O04&= z2CjH(Me~?>t7p3j*Q+x^(#Ly`_gr3OWqv8EO*aW;S)UBz;)m8S2xs29j9_c4=``jN zAJWgQ9$^i1+>>aD-kfT6*WU2lh2gC*RvU`Fk@k0ZY2mHPo9AC2-CJyM_IMgKmfiYV zID2;YzV2^mO0?)$r1oale)KW|hgr2yvI3++l%)-oRX&CKu*2HkLx(k^S}nb}<-C?g zUNXA*8EPN!TjI{u_zv9V>W&bZuuZ0mNHN;n7Djjnt`wNxXW;APHxZtc zU-C;Ld&l@3M`LV{!6~mD@7cRq81LgSs0%R6wJATbJ=x81gtt0^y3ekJUUS!fJ~SSZ zpf8;jB>*;mMd$-e=>Lz<{~d^o-+#Zn7}ip$C4H-K`o~YHsOm1^>SXNsz%|I|kY7hK z(VMr*dsI?^7MOW(g-eL^N(KXi>BXzpE3aO@xOlyCgHY+S{vS^?PatH|5 zTOvrq#+4U%d%!{cV9X$Dhz7LQ`xwjwM5XsQ-b#&?;H@se4_81CIB}Q-go+F48Dlct z8KtJ})_b@r|N63Q;R{uzBk)`JkvYY3o-s`_31Nb@Wax(IA{SyStC&I`YXXzVB5;x+ zo48AfPY+wh`Rpi(KcKlW^yY*THI7;BD&G54m8BNaLa2;c$FXoOd~WpWM3P=T2&Fh) zi`7L7Qoq_*Lo@h={pI+(N;A}9fP{Wnjx{xkYtR*qN@P-0AY!cJD$F;F%mS?kU<8*! zpRR5dsR5gF);U*&`{8M9StsUB^-FE`3sJb%`0$RyW;^u!?A(c7&$5k0RFZcQZseDE zK{ZoK7K&nykYdg*T)lq&+Qo&%%B9PsnaYLBSI?dg4v{L(Iue-yhzVXoVos}hp(U#> z`XV(12SzVOFxd~KV9&tpg50`9;?Qz%WON6J5Dmo`9#%Nr*B){?Mu6u@Gb%nRT;_bI>ZG$x93xQIz1KST;yIJ%2+PTdDdXvq!oU}#&(P(>c};F(Sr z=247Vm%{M>FFSeYbp;u!u#`owj=lv-A?E0)2a*ZsF$#Td`T=7kh)W{2QbRzyiKaCM z*=Ma!*AV@#AyQvMCRw_;j{<^dIU$19iX_qk7Cu?ILp9Q)&G>DR{2|aQZ8tR1H*js zvn;k){5A@EvVRixAx6nO*lpCZtBB<<<9fN(5b}5U&VWdKVt3*UA#Q+n80pYY40PaaAe-$11&`~-6?sh5usep)Y2Z~~br)z*zY>z8g{0Dmo$A*a@R1kZ%j!hC^Xebqmx&oC%Dum{= z*?IoHh+(MHz6dTP6Fqa96sRDEd4m5OGml@ayi%DgKL<2TYH>ZKXi;&V$hkIJ1yLgy z6>}^AVcPg zv)~S55LDHXC;?stl{}j|oa&k{AdK-6^%j$SMscO|HrlOZS2VYRpJ6+%?Rf33>-(G!Tw=FOwl9?OU z@najuX02nJC>T!f94q(Mlj2iYxU|_u^L(*fk~eS;GSyalbI^^^Z_8=nw1(E9w?gC2OoSgvAEv!8bYvuN z%5{Ewgg;}D5gs0e1gfV$AvP)5l-GC^<+Ce%n!QhaQeE|76m8Wkjx-MMpHS$HF@EZc za8cedZl>iCANj&$gFaZBx)0`dhaL;_3#ppQB!0KR*5{CKggm1>H`c)MIfINLgpau*vXR1%1`Mf?wP&0pnEO^7a@oz0`#Z6|A=; zkn)ET>3%4Y>bI2H8iO%7R-?1D=$rt&V&uGzQ~(dE@~TcoibLQq3Kc{&2UFa!j)bMy zGKcz)#K3x!Dg3*lW^%Euy+Q-i z8wytvzxENo==ECy&w6Ki%K#?O362EMyT%`zx4=)d_K-+J2(pSXm(9SOI}v0R6cbqq z}jkqHoS=5=0Q&-?Al{0VHXgT^$q!JXo_Cx z@R7@WZEtO@(dctn+B3<-70M}Q*vPAFt+Nj+YI*FbctQ|k>^&$%7+_B)mOp$dneQ8v zm@n+gCS-UpmT088l7%Kx!Fb;YxP6zv=6a^arr|I};yC2mWO_V3Wz3}MQhjjrK9mt% z4Jl?h^r{pyI@!2`0pT0-uE~UP@E33)?h}JL#vqK!bkt?dgenFJ7v9V8T4a|DdQM97;L9xHDN4?-pnneDE1a=5tNsUc`pk|CIb1I!PGKR^& zpXBl*w|R}|sE@oj5ArodUOfZUIh92DvreN5e>sc!%deR{~%9&0*DD;;EV-!LUP*q0Otah3tvGf!b4p-x^%_Y3$|Ipsrke zt)d|MUtD)(=oUIpjF4f&Z}iDqE-(+Zz*Kp`>$KUbLg0#raytdx=_ghGILa>MDHQ2vQ*=La!+xP|}?wvXye3Bo1Zwaopo&TXgO;MUDB{TIt3!3n8>NWB3| zAr$Xgb>z{~08OyQk(?*aW7PS^U6K=SJ_ZQ&zr0<*Z!78=%Ks|nPtDUBukkmfm- zwbQI|~O(fh6Qh_XPGV%Igy#m=a-%H(@}Yfr4iWXYrT%Rr50W)X-0Jeu2OY-kmx+8lHssXak+(!(ONBb+U-WYs<59-=&gmTm*y;u z(h3nhbIAccbedY!o6sQ&n9L#Y7cQ=<8pS9zj8*ErrhK==$HdK#q~w`Io?S=HT61_ zpWy#0;YO=UB`8&>^J8Z@>f;sef^Fz05^OOgkFt1##Tbh@79V4w9LcY+_8N=pEZ$)8 z7K=}_c!$Le7F8B5izW-kG3D>FxWnS}EPjv0@3Z&=7Jtg(>ny&(;#({zH|3wPP+0Rl z*8Yk`4|QU>Ad7fF-tC=30T1$_!h?lOAzdgG_TaBrm?%sYN`*<(iiL5UCrgJ*<~{ z6j<6vl3&CBG^zWWl0+`)3y_p#LE78&0<-1Wk@nvS`}?PHLp=13<@u4%=kDp W2+u;nt#t7p{trm+4|nt9=KlimJ5`_n diff --git a/RoboticsToolBox/__pycache__/Bestman_sim_flexiv.cpython-38.pyc b/RoboticsToolBox/__pycache__/Bestman_sim_flexiv.cpython-38.pyc deleted file mode 100644 index 03e5e6811e360a292aeaeffc3a9d51a3917e89e9..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 20028 zcmeHvTZ|mpd0tobz30N=aCluVSzN7`Y-u=Lac>s2S@FJ0c{yZ;q;^%>b*I^YUPQ;aM$XQ304J3gqXT~qiCIOrviQ~t_a9#pGrlu+x{Qdj?^JMuC?`1N7!jtr09w!&^ zbN?3#l~G13V;gcbZBvd}J1fVWos(nU&daf27vxxsjwQP+=M}qxquH8RpR^|p!pyd& z)~D@hspnez)@SVfQqQ+$*ALhSq+URM&YqKc5%q)iL8%+9L-rwod)Pkg99}!Jb5zP> z&I`N7u3;aSvlpG?sgyk%SR{3-j?yBSqhl}|FN^2D@XQpTN3 zb>b<;oms3JLH^2Wz1c?PM6>#)G zd1s^LX#1NV;^w|?oey`tnu|t`%l56N>z#K!?H^xQICrCs8?Dy4<(9MAd{oow!MRrR zeyzLJU{}(AIh@hY3+gzIGwMZk0>}O8 zs&>T7^>Qe9M+aK%!W z)fF61sgqc`uLlP%JFd50Z`YQbdaHI_=Ci>`i~oq-JimgU+eYDKc8s3UGk48irUv{* zneAm!=5Qy!%kzSlSu5gL!j-aIsdy7>lRH!1v@&+h?VPu7XQr3iHPB|iGFA2yJhire zZT4#B39lVcIh1oMgD&P5{a3K!mt5EJlJ#!-o%ZW}4_v|4Upg_A!MHFo_De(re` zD|p4nU)(mlOwaI)HFGWd#MHT-u?CWSV7gzFn(;uc{hIkOhgX|+Njr}^tqsqy%U#`c z1(U1!pwe`k#K3mL33AN`-K`+kYPKKPrF%wIPg=EAFXUaowW z(bMS6kKZ}B+F5tbZPnZ7xBxe?0O8!9Tk72JfL`3Yolfg=XH#x-eca}HEkySPysz>$ z>7mhC@4oXh93eSW!7z>KOeItKiT~=DRb8mnn(d}nt1Z0U>NM&t_Z?JXApSnU!fRBm z)md5aHrA^LgL3#f5SN{oUIls0&IQF4-RyQ9jTYK@xZ$)LTS4~y&PtHQ!*=dwM`2pl zQQ8yo+#Mm}U{aorNIEb(ZjkHNz11McnF%Hu8(KS&|+OD_dJ`wwz!h ze7->*6XZGJZZK7Ec+E#nO(xd`m*}iWV=Pk6by~|oDSW8e=Mj9&0+Nu?KSc!y4?^0_=-a57 zJ6RBhVT4z6yBQu{g ztLetg{n=~n`h6_>O7vnprAhKnr62|Pn|Hist7Sc|hj5oWI%Y_udPUC^)t=7HMD4pn;FITPAx@)yN)-s1aZ=J8QJNG*s&ruf0yJKD2KtW9R zk1UZic)#6woPJ4lLeHXWL9yw|2=yVJksi@#qA3x8*q9An$11(cmrZ z{MI>~_Cwpnjk-Hz)l_=Wd?eP^nR)qI2xkZ0tS z`Y=G-hZmRb)^0D|xOwC5jc;75UHkCbm3wz@+*-6}oK2?z0d!kKdm^jTuc7_$1Qb^~ zH42nP+;c^Gn=^+0!nk2aEgLd5XwJVW{EGMqb9xVzp7Bv}+uY7>=eG0Ph3(=tCArZn z^oqSwFTax8HMYyW@|q}8D*LJVQFgnsJ+VF6GocCOdX?TpZ*rx8DaqA;9*9w~qQ5I0 z>V;mk;+zXLMDUmU_rq~Ih-@2kJa%I+oeYeoU(LXHS6uv zpo8q@mVf;-1Ny47q8+Hq*}zz`A)FyTsao5mx?20z9Mi7w%bm2KrS-*uvx~_i~B{=O@a0n`bDS{wD5vW3( zi7>^Uj?pyixc(|0&~LN&Yb?ky^i>wuSX^fDH5M0HTw?JK3o5-~Au86Cyo}gGjJS`R zZiqZH#vucoYb^2%Jky}8iT}v)4&W#JOQQlMU(n@=`FB6shIO)I_KY9GqV550j%K1- zE~({{S|OE6L{(5F9^I=9!0L>=-lNyp(D~p00GHAUkl9?#KW{PUc9GT4@g7ykUyE?FQO7k!(E8?NW+ANx|ZEG zy_}b)dHBS9VsPp)HHQD6(s-+z&Z9^J5Vai|)dmz83rxAMd_jgTRMJlkvR%rP5aTZ; z99>wkYiXye9T%2_Xro}D>6gq$+}No%RxNqJg7)OWq6k%zbqMYC4G1LgI#{`veA|>jhcA*-aJ=@WPSWWxOV;PDt;zpWTG+tHTS7u9PC|jh8ooS^%ML#_z-YOML^4^(AD{vgfM)M?5A^W_c)Rk;7&c%DpFW;Nw1_NHBqJw zrNBA^FH-e_b^1P3|I-#$^)!&}bn?O=#~Z$NnU=xcue`7*MC|8j?_0ik!LqY}gjbkWz^sD1AWh{&Vx~E!CRZ-Ao5|uvP3uQk}<_!`lMcFs5{+a0+ zb8^ADi*<%Y3B7p{WH`zaxQhIXtb~y8%E=RBwc`p20&7!is=tR3gjPY(KQV4b`c3zq z2Kojz$4KW3F=g zb#xi-8Hf{_QGxMr>_ingdh~0TvqVNgEZW122wRR!QX(U00(xL39^8cRwHf@EDf+GS z2vZo-Y^!Did^Q?~kdRA-_y0*K#8An-6CN@SY=~Astel3E9}-u?xNvk0b$yb>IJ;}A z&uW`$^LNoKMy|0M%s}grsY}R=luk0E!g~|~7Sm?>-```}?q0cVouRYPx&oO8o9H$@ zkJXSxj^acb$|!n)GlnEcsKR7NA;c5)lolh%FyURouEtK&BnR#_x;29Q{nRX3dy!z= zcwmw^LCGYgi4c#r}qvJJWWI(Jl04fk~v~$B2&2p^s=CU|7!y zIG#C78tG9;QU49pVqear^sE+IwZjbm2q!UmQ^fYehW{22T#EFN=lk<}P3pLZMMrnd<-XIa5vbeq@SIyOskX?kq1m4X>k9Gk$j!4z%TsHk`T? z>i7GKS!{-gX4b)?LTQU7Kk0f8fMb`Cow^|0;8{kWsD0A+MmHHHxI?mZb`e;58?f|s zOjWdo`U2|1OZQ@W>Cn?!JajRC{s>P*<_~3c#q_`Yc`G;qW3eF!07(2JWhVZdPjEtR z0{;zoN$k4X$%Rg4Tq~>T%gqLaKb*|Vi3UX)ou`iO zHsLFyO5a`GauFX%Va=$Xm!_i&R^LQWj@#65-VShpGkvE@RkVCD4#Fl8`xCwplP`~v z)%D0}U9fH;ve{|ZTMMZdh4(Go?=)cdN9IExaNmMRL8~_!&xD5i4h%fO`_LZQ3*gO5 zAJ)Ec?fvo461(R4I!9QSu3ULGtmo_6;(sA_@-M3V&#y(5|NOOTIkcqSz+Lf)2D9{u ze*M;sMR-PU-GO8D)?!F}B9p1d2=ff~!`M?_r_=*6ij$>U4Bf=bXp>mTSc{~N?gqtZ zlEMl`7-XUP43qAWb$E0|d4#hfjaGjQ*kN#JVg^(@X@YV|z46Z+&xiK{H$cB-z@|S$ z`0yW~0>>99_=OCp6rv8^45(CV@WL|e!oNf|5;8@0oBjeC=@XS!El4ttm-}q=??J^-NGBjF${eMH-hrD&4weKTd%X> zH(7j%#XJgdqf3iBa$gY!JR@HvwHZVsZI4x3#47siS9Aw`c-oO&cWlkmqg<@kV}v(iEM zp1PZLhFTX~ICK`oB3*7o;u?=7+NA~Y-e_&08T^diYVuxDBtw^SN}a4G>IvmCXoV&V zqS-Y_59^FXjOG#D4Fx`Wa5M4|hOJ_GzqMdpbtL2p$5qFAb>WRcc~7{|-8WjF4>)SJ z2jnj=yxNB>l%;QBB^TkAyn*8oMFhtvQLkKDx_j-;jZ2HQo3|)aYu9frUAYzP6fwN`k775WimN!G2_&?Dgr*@{pz2%J z3vwMa5sG89VVG0|qtFLcU$fqW;hNyq%_vIzmz_Ssg#{f-jKiYSAiM-pA?k!~bi}3U zkcd1v;SD<75#@rjei^}0$csY)7T3P!t>S#O(^C4| zd}c@>{>-z9Gn$#RB=ia%7s;5Vn(cq(&ra$Qv;px)qUTSf6gASg-8#EdDKgztCP|~T!|BTu{WqU)DTYA|WHOK=aRG!Y(S5;G zb*8ERW){J8qnCY9)W;cx72bj?M`^iBA}Q>3sO9<0(M->L3#oW-Q*|-PX@oCjchGJv zzZUW<_z!l9p?$8Hr&aX-d$QJ&nIGmJK?Cb6(!-$u>;4wh`N(kxr>u$(kDim^{a90PD=@&C~PN=`9qD(E6vV{RO224PO>&l;W)L&EBCUy!6&3M$`VUcfabYU))&8Y5oqlY492 zwab_8TnnXa#6EJImLMIK7W_-xlD;}s}ihB@{|g52EQ~8iT8pdh)+29E8~~QQz{5loOkI(HkszkR1jqd zb{ZjehJnE~NXc#ZqTqAN(dU%Z3gWBcH6%Pocj<}BL%kMHSEO#|cgpZHOIld3B)(>* z?&tk~{L{l{ar-?RpP7dk*@u4)Y^2Q=T5sYasaEs~t^~P8r@IwKAQNxrkVzLhIq1NM zJQn)9sM~Wff}}l`_JPQmk-iwZEXMdWhLP#$I3y5Aej=cXAtJ@8#ez_0pBJG7)__== z#8C41W$|ruSwkKc{PAoH&Tdu!~2}`i^F5=8CH0Jd% z$0dnF!Zwr{3AvbX$R_e&5(^zx0Fup`ZRR>Z6F1YVZJlBG5*XSKi444i)&-+!NiR~6 z$A;3P^w&#+xm0oH)!5EjXTY^Ycw%VSN6?SujZ23hujikCjan=cX6_@zoy3Qt)GXD3 zcZZt`kPXDVu7Q@}4v}y&-H~u7@r$X0?lBayM`(82049XCJ}@cV3lyplsbF`I7uKwa zqp6tpKAtlF=C?@{l;j13to^T!BLbOumr0O~*zGcIYtCXQtN<_5PaA~3+Rse*1h)dS z-L><+qdRUYB5&i4J&RSvb_12#FC}@O_JrhpV$O7PQ@@9%fO?9rq2gihU*2f72Ky;1P|% z?l&eLgQbA+aGMUaFve7%sex%oxokoMgH(&I<#?|0D+bad5p#WFP}+Hj*sPU#4*L+B z5LNNh9JIB9DycHF=D;p0bq9lc9(FOF+MrpHZ=}gQMbx9A(|Dm0RzbS!gHCP_YlD5| zisYJFzdZn@Ta3a=#yghl4Ci2^2M4~0ePnJIdt!tQb%Y(PVCO_THH-n~$qw^RQlmvH zOwnAB#YWR=F38KiGkY?Uq2jKx4$eufl&#`Ow53w=8PG0CR;Fa*m8*fKP?o;@39#JX@I;q z5I_#i9dgUi@T|X)M)@oCRs*hb*al?m(R5hJ&SDr6H-4^a$&M)r-y;@7dCJxAHX9Gl zLdu-&w(9UPVuNF|OHi~9fOK`{m@rUiZP0NAYi69_8>Nxh5k1Ck<*j~XZ{wLHvy=5EVr}cN{PF; z)&AWq^*=RxG@}k&WMI9Zrjfw>ZvbH~1tH78H=Z*;fE+it=Q6tTLFVC^T*j+BsC?hh zfV^EaGX4=feNYml5A}+1dNel*sKeXEZx=tzd=pQ7h@UzP)_%nQqlC}hL68aC5W}<% zE^xHRmisL0kLt}9w*W?&vA{14mQL+Shx?I)GI-T$BEyshbc)B1WuZx?Uzj>8Afw#) z^xI*Y!_C_UiEzIrw8Oz^xcR zGNV1#hF?CXoJZ$8Z|jbr<7cMkb~Mo{hVWAqtN%uXOcblv$D!mhG?Q91f8#VN*?95i z?CnI{+k1B|hu!;i_IA*_J*J~mcf;+mT%-$DGxG;V7?Q1;LFs;dLop3JC~{v%XTy6+ zW4dZc8jL>$w|uw&^pZ6HD|lwO?Ny;s^FP4mPI)t4!L_G9LE#^MH{7Hhq65(=e6^py zO+4`rhI>`LP&|fhFIbD%u6iGjOE^3zgvb#T?np*_*?X1AQz^=PsT03=>I?p#+kV4`-je-hDgC`ne6F3e*7B_!Q9@7k~o0B*{ zVbFGLV-oGEbQ!8QHi`9NIlix@h|hk0^CpWDKcbgU{Ba_#7vz$_BmvK z1coQuw6i+}Y{q^ccV+hkTkI6cWWZ9s2bNNLP|@E8LjhCqXVFS_T!aiwSP8bpecKdP z^1Tl;?Gjobe~(+e!BR^8dcsmJacexyL*W#R)*#{*TA49(0Ya@nL1I`wNx(}qyvI$1 z%R|KpH^srHD1RDGUxM+=99%!{TJT>Sz|{C15$0KevEUz!`b1$H6Dan6&)#c0n@9lU zzG{&=Ktz9T5uJ;0IE-{TY5`u3e}q{NX1ZHT@}Y!>??&Gum>xXWH0r{nj?ehOMAMDu zLj@(VV`y#AnGf&ZDIDy{=xy+&X;1$SpC2Ua03S&ZQjD340WK%ZEt4yWE;Mb-U+wSV3J1V_a7lOPV*taggv0cQ)kjjo3^6$B>>WYp!Jc9p|09Q7ZT|DR zDkQ+P5{~KF-eFe*l}kRXZd2nHJr-M=@Gmg_{D`XXQ=KA|@uKSlQS4dQ=%b(qyf8#YO$ zpJnyGz@b0RMSPFoU-HUpEQSZo1&L>|%0c7DuMv!uIO)dEHDE{?Ws(bo30VGnSBN7c z1W(&P-qiSf0~Us){}sBrsDF>+8aKEp!N_=YXH%mayYXe9bv3t$zo<0(*=fM0BvWls z|1G>4F}vgn>RbaUsxR>)EtEO6T2QIc#Kz|UP@k-E^KvV^qp1Vv^DJIx@g|GwEF@BM zm$eUAe9YqKS=3oHSU4=`g%4vpE^Cij`~r)=&Eh*Op0M~u7Qf8mQx?C*;s-1!tMu=( z5H$JMto=TVKCVbj+=eujj^gLOg92fhxzh1cu9PqB!+*InRhlVPO4F#7OB1D;$_te< zmFddC$`PC&E*&YoTsn!nCrSq^2TD0jSk*7sc{ofFLZEX2LQk*AceNyLjgMME{!!o5 z)MxZRV3Wh4zPI3EJCVCUFN?@Rd}c>bsNTXv-`{Zc5BbDDL19<;^`ixGjf5ZVIV>0U z@LAZ0-9{7bU3~WkJNy<9Fp4?-XY_0=TUHv#^68q;>Q2HgEURiF-zJ13qq+oT>t<8 diff --git a/RoboticsToolBox/__pycache__/utility.cpython-38.pyc b/RoboticsToolBox/__pycache__/utility.cpython-38.pyc deleted file mode 100644 index fbcfa12d0c8e58908e9508953041e65fa2c7242e..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2142 zcmb7FO>f&a7$zn8BVLlGThSsL1`xpxHQ>leFl<9GbW5`4Fmy%ctiz@v5EzrTxyhnK zQgLiJr}n!4kRJDMcGh*L{DYyF>?LJ8A1H>7fGCoB`F=j{qZdm{H3Z|It5=ojg)Ok!YZO)42W%1E z3oJN9_2u!WnGuOFqi`^))m{{9(&wfpnGS2?ulD3%q(rx8$m6{_+1l9rVts34Ym@9J z{7jsZXNHAjCrL=l%4pK!nyXXJ!l+zGT!NG6c^&7~mNe7|neMkedo~?5nhHF7@aWHB zGUx(NAf{CWv*%{t&A}?_25<0h9xF{SEC1nL_;1j;H}N2j3O&IH)d-9|nVxcG$Vo;G zS1A}IMv|vl!WE%uH{qHLh3S##*Ix)d6Pl>0Ul6WSa6lD>1aPI@c6~nF)~Y4jq?1T^ zCqf%{Y7PH*ZBV3x%n*^1m&3+cW7HTQ5vdqgp%Z4jt2hTG$5JNq>n(0Fm6B7MWZ)v) zEyQ$#NYmqLC^WaHiZVgdnXkpAcRKnG4{s&GdUw;e##A*q9)al&%@RYh6w;-M9`Ls4 z2+pouDI&Rqq@zn)&P!WIWw*J1HX1{{D^vgLL5^GbLPyDdY?GH)sgAiVM_xA4X0Yz( zf%{S}u0&P$84C0#Qp=$7?QXLt`@DJZ{j;6j<}e$JRAkLvuFVfLjSHNu_o+ymEqN@B zXzQOKv%B)Fc@6G-$(q=jX`-Q^ z%?oX^1DGBWEyjGR?LQC!R_AEjnfg%lLPx4Tz(Yb z({``GCzjNKT%AH#xEg(C8R7x~ zQ}`3#u?qg(Qvs}jTReo(Avy_|4>b$uU$dG41wgl1=UqDzo8}u`Fi@f|j5y_xL?9Vr zvF)S>HV!y}mT?t!MS8C?U*FuecW+nx;I_S6+6g2Ncmpcnyl@S$z1`(ze(BJ8^tfK+ ze_%t44OJil$O2pQT0RHbv!U9!?}iAlbvFq><^Lh=XG>@^wbx8?*8tGbG-m}EL-Mpg zFaYf$k-*b)Kj#sPPT|xn93~<2sWb$BP1+RkI!?O0f}!pMCIERQJ%ArxJ%|?ONskt9 zrUfKy%JR|>NL$~9i~MMmp{^NF&Kr=LPpReGzwVWrb1bIRTlb=njG$(fg6rll2k>*eBGA} W$n}Zs1Z~fQQh)>Nm|t71KKvIY223;n diff --git a/RoboticsToolBox/utility.py b/RoboticsToolBox/utility.py deleted file mode 100644 index 192bf9b..0000000 --- a/RoboticsToolBox/utility.py +++ /dev/null @@ -1,87 +0,0 @@ -#!/usr/bin/env python - -"""utility.py - -Utility methods. -""" - -__copyright__ = "Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved." -__author__ = "Flexiv" - -import math -# pip install scipy -from scipy.spatial.transform import Rotation as R - - -def quat2eulerZYX(quat, degree=False): - """ - Convert quaternion to Euler angles with ZYX axis rotations. - - Parameters - ---------- - quat : float list - Quaternion input in [w,x,y,z] order. - degree : bool - Return values in degrees, otherwise in radians. - - Returns - ---------- - float list - Euler angles in [x,y,z] order, radian by default unless specified otherwise. - """ - - # Convert target quaternion to Euler ZYX using scipy package's 'xyz' extrinsic rotation - # NOTE: scipy uses [x,y,z,w] order to represent quaternion - eulerZYX = R.from_quat([quat[1], quat[2], - quat[3], quat[0]]).as_euler('xyz', degrees=degree).tolist() - - return eulerZYX - - -def list2str(ls): - """ - Convert a list to a string. - - Parameters - ---------- - ls : list - Source list of any size. - - Returns - ---------- - str - A string with format "ls[0] ls[1] ... ls[n] ", i.e. each value - followed by a space, including the last one. - """ - - ret_str = "" - for i in ls: - ret_str += str(i) + " " - return ret_str - - -def parse_pt_states(pt_states, parse_target): - """ - Parse the value of a specified primitive state from the pt_states string list. - - Parameters - ---------- - pt_states : str list - Primitive states string list returned from Robot::getPrimitiveStates(). - parse_target : str - Name of the primitive state to parse for. - - Returns - ---------- - str - Value of the specified primitive state in string format. Empty string is - returned if parse_target does not exist. - """ - for state in pt_states: - # Split the state sentence into words - words = state.split() - - if words[0] == parse_target: - return words[-1] - - return "" diff --git a/Robotics_API/Bestman_Real_Flexiv.py b/Robotics_API/Bestman_Real_Flexiv.py deleted file mode 100644 index 9b89b6a..0000000 --- a/Robotics_API/Bestman_Real_Flexiv.py +++ /dev/null @@ -1,898 +0,0 @@ -# !/usr/bin/env python -# -*- encoding: utf-8 -*- -""" -# @FileName : Bestman_Real_Flexiv.py -# @Time : 2024-12-01 12:46:52 -# @Author : Yan -# @Email : yding25@binghamton.edu -# @Description : A basic class for Flexiv robotic arm -# @Usage : None -""" - - -import os -import time -import numpy as np -from .Pose import Pose -from scipy.spatial.transform import Rotation as R -import ikpy -from ikpy.chain import Chain -from ikpy.link import URDFLink -import serial -import minimalmodbus as mm -import pyRobotiqGripper -import flexivrdk -import rospy - - -class Bestman_Real_Flexiv: - """ - A class to interface with the Flexiv robotic arm. - - Attributes: - robot: The Flexiv robot object initialized with provided IP addresses. - gripper: The gripper object for controlling the robo's end effector. - frequency: The control frequency for the robot. - log: A logger object to log messages for debugging and monitoring. - mode: A reference to the Flexiv operation modes. - robot_states: A container for storing the current state of the robot. - robot_chain: A kinematic chain for the robot, parsed from the URDF file. - active_joints: A list of active (revolute or prismatic) joints in the robot. - log_file: The file path for logging robot commands. - """ - - def __init__(self, robot_ip, local_ip, frequency): - """ - Initializes the Flexiv robot and related components. - - Args: - robot_ip (str): The IP address of the Flexiv robot. - local_ip (str): The local IP address for communicating with the robot. - frequency (float): Control frequency for the robot. - """ - # Robot initialization - self.robot = flexivrdk.Robot(robot_ip, local_ip) - self.gripper = None - self.frequency = frequency - self.log = flexivrdk.Log() - self.mode = flexivrdk.Mode - self.robot_states = flexivrdk.RobotStates() - - # Parse URDF for kinematic chain - current_dir = os.path.dirname(os.path.abspath(__file__)) - urdf_file = os.path.join(current_dir, "../Asset/flexiv_rizon4_kinematics.urdf") - if not os.path.exists(urdf_file): - raise FileNotFoundError(f"URDF file not found: {urdf_file}") - # Parse the URDF file and configure the chain - self.robot_chain = Chain.from_urdf_file(urdf_file) - active_links_mask = ( - [False] + [True] * 7 + [False] - ) # Base + 7 DOF + End Effector - self.robot_chain.active_links_mask = active_links_mask - # Filter active joints (revolute or prismatic) - self.active_joints = [ - joint - for joint in self.robot_chain.links - if isinstance(joint, URDFLink) - and (joint.joint_type == "revolute" or joint.joint_type == "prismatic") - ] - # Validate the number of active joints - if len(self.active_joints) != 7: # Flexiv is a 7-DOF robot - raise ValueError( - f"Expected 7 active joints, but found {len(self.active_joints)}. " - f"Check the URDF file and active_links_mask." - ) - # Log successful initialization - self.log.info("Flexiv robot kinematic chain successfully initialized.") - - # Additional initializations can go here - self.log_file = "command_log.tum" - - # ---------------------------------------------------------------- - # Device Initialization and State Management - # ---------------------------------------------------------------- - - def initialize_robot(self): - """ - Initializes the robot by clearing faults and enabling it. - - This function ensures the robot is operational by: - 1. Clearing any existing faults. - 2. Enabling the robot. - 3. Waiting until the robot becomes operational. - - Returns: - bool: True if the robot is successfully initialized, False otherwise. - """ - try: - # Clear faults - if self.robot.isFault(): - self.log.warn("Fault occurred on robot server, attempting to clear...") - self.robot.clearFault() - time.sleep(1) - if self.robot.isFault(): - self.log.error("Fault cannot be cleared, exiting...") - return False - self.log.info("Fault cleared successfully.") - - # Enable the robot - self.log.info("Enabling the robot...") - self.robot.enable() - - # Wait for the robot to become operational - for seconds_waited in range(10): - if self.robot.isOperational(): - self.log.info("Robot is now operational.") - return True - time.sleep(1) - - self.log.warn("Robot is not operational after 10 seconds.") - return False - - except Exception as e: - self.log.error(f"Failed to initialize the robot: {str(e)}") - return False - - def clear_fault(self): - """ - Clears any faults on the robot server and ensures the robot is operational. - """ - if self.robot.isFault(): - self.log.warn("Fault detected. Attempting to clear...") - self.robot.clearFault() - time.sleep(2) - if self.robot.isFault(): - self.log.error("Fault could not be cleared.") - return - self.log.info("Fault cleared successfully.") - - self.log.info("Enabling the robot...") - self.robot.enable() - - for seconds_waited in range(10): - if self.robot.isOperational(): - self.log.info("Robot is now operational.") - return - time.sleep(1) - - self.log.warn("Robot is not operational after 10 seconds.") - - def update_robot_states(self): - """ - Updates the current robot states by fetching them from the robot. - """ - self.robot.getRobotStates(self.robot_states) - - def go_home(self): - """ - Moves the robot arm to its initial (home) pose. - """ - self.robot.setMode(self.mode.NRT_PRIMITIVE_EXECUTION) - self.robot.executePrimitive("Home()") - - # ---------------------------------------------------------------- - # Logging - # ---------------------------------------------------------------- - - def log_command(self, timestamp, target_pos): - """ - Records a command with timestamp and target position. - - Args: - timestamp (float): The time of the command. - target_pos (list): Target joint positions or end effector pose. - """ - try: - with open(self.log_file, "a") as file: - file.write(f"{timestamp} {' '.join(map(str, target_pos))}\n") - self.log.info(f"Command logged: {timestamp}, {target_pos}") - except Exception as e: - self.log.error(f"Failed to log command: {str(e)}") - - # ---------------------------------------------------------------- - # Arm Functions - # ---------------------------------------------------------------- - - def get_joint_bounds(self): - """ - Retrieves the joint limits of the robot arm. - - Returns: - list: A list of tuples, where each tuple contains the minimum and maximum value for a joint. - """ - max_bounds = self.robot.info().qMax - min_bounds = self.robot.info().qMin - return list(zip(min_bounds, max_bounds)) - - def print_arm_jointInfo(self): - """ - Prints the joint and link information of the robot's arm. - - Returns: - None - """ - print("Arm joint and link information:") - for i, link in enumerate( - self.robot_chain.links[1:] - ): # Assuming arm starts at the second link - print(f"Link {i + 1}: {link.name}") - self.log.info(f"Link {i + 1}: {link.name}") - - def get_arm_id(self): - """ - Retrieves the serial ID of the robot arm. - - Returns: - str: The serial number of the robot arm. - """ - try: - arm_id = self.robot.info().serialNum - self.log.info(f"Retrieved robot arm ID: {arm_id}") - return arm_id - except Exception as e: - self.log.error(f"Failed to get robot arm ID: {str(e)}") - return None - - def get_DOF(self): - """ - Retrieves the degree of freedom (DOF) of the robot arm. - - Returns: - int: The number of degrees of freedom. - """ - try: - self.update_robot_states() - dof = len(self.robot_states.q) - self.log.info(f"Robot DOF: {dof}") - return dof - except Exception as e: - self.log.error(f"Failed to retrieve DOF: {str(e)}") - return 0 - - def get_arm_all_joint_idx(self): - """ - Retrieves the indices of all active joints in the robot arm. - - Returns: - list[int]: A list of indices for the active joints. - """ - joint_indices = list(range(len(self.active_joints))) - self.log.info(f"Retrieved joint indices: {joint_indices}") - return joint_indices - - def get_tcp_link(self): - """ - Retrieves the name of the TCP (Tool Center Point) link. - - Returns: - str: Name of the TCP link. - """ - try: - tcp_link_name = self.robot_chain.links[7].name # Assuming TCP is at index 7 - self.log.info(f"TCP link retrieved: {tcp_link_name}") - return tcp_link_name - except IndexError as e: - self.log.error(f"Failed to retrieve TCP link: {str(e)}") - return None - - def get_current_joint_values(self): - """ - Retrieves the current joint angles of the robot arm. - - Returns: - list[float]: A list of current joint angles (in radians). - """ - try: - self.update_robot_states() - joint_values = self.robot_states.q - self.log.info(f"Current joint values: {joint_values}") - return joint_values - except Exception as e: - self.log.error(f"Failed to retrieve current joint values: {str(e)}") - return [] - - def get_current_eef_pose(self): - """ - Retrieves the current pose of the robot arm's end effector. - - Returns: - Pose: A Pose object representing the end effector's position and orientation. - """ - try: - self.update_robot_states() - tcp_pose = self.robot_states.tcpPose # [x, y, z, qw, qx, qy, qz] - position = tcp_pose[:3] - orientation = tcp_pose[3:] # [qw, qx, qy, qz] - pose = Pose(position, orientation) - self.log.info(f"Current end effector pose: {pose}") - return pose - except Exception as e: - self.log.error(f"Failed to retrieve end effector pose: {str(e)}") - return None - - def print_robot_info(self): - """ - Print Flexiv robot information to confirm available attributes. - """ - try: - info = self.robot.info() - print("Robot Info Attributes:") - for attr in dir(info): - if not attr.startswith("_"): - print(f"{attr}: {getattr(info, attr)}") - except Exception as e: - self.log.error(f"Failed to retrieve robot information: {str(e)}") - - def move_arm_to_joint_values( - self, joint_values, target_vel=None, target_acc=None, MAX_VEL=None, MAX_ACC=None - ): - """ - Moves the robot arm to the specified joint angles with optional velocity and acceleration. - - Args: - joint_values (list[float]): Desired joint angles (in radians) for each joint. - target_vel (list[float], optional): Target velocities for each joint. Defaults to None. - target_acc (list[float], optional): Target accelerations for each joint. Defaults to None. - MAX_VEL (list[float], optional): Maximum velocities for each joint. Defaults to dqMax from RobotInfo. - MAX_ACC (list[float], optional): Maximum accelerations for each joint. Defaults to [5.0] rad/s². - - Returns: - None - """ - try: - # Set control mode - self.robot.setMode(self.mode.NRT_JOINT_POSITION) - self.update_robot_states() - DOF = len(self.robot_states.q) - - # Get limits from RobotInfo - alpha = 0.1 # Key coefficient to regulate the maximum speed, with a range of [0.0001, 1] - robot_info = self.robot.info() - default_max_vel = [vel * alpha for vel in robot_info.dqMax] - default_max_acc = [ - 5.0 - ] * DOF # Assuming 5 rad/s² as default maximum acceleration - - # Set default values - MAX_VEL = MAX_VEL or default_max_vel - MAX_ACC = MAX_ACC or default_max_acc - target_vel = target_vel if target_vel is not None else [0.0] * DOF - target_acc = target_acc if target_acc is not None else [0.0] * DOF - - # Validate joint values within limits - joint_limits = zip(robot_info.qMin, robot_info.qMax) - for i, (q, (q_min, q_max)) in enumerate(zip(joint_values, joint_limits)): - if not (q_min <= q <= q_max): - raise ValueError( - f"Joint {i + 1} value {q} out of range [{q_min}, {q_max}]" - ) - - # Send joint command - self.robot.sendJointPosition( - joint_values, target_vel, target_acc, MAX_VEL, MAX_ACC - ) - self.log.info(f"Moved arm to joint values: {joint_values}") - except Exception as e: - self.log.error(f"Failed to move arm to joint values: {str(e)}") - - # ---------------------------------------------------------------- - # End Effector (EEF) Functions - # ---------------------------------------------------------------- - - def move_eef_to_goal_pose(self, goal_pose, max_linear_vel=0.1, max_angular_vel=0.5): - """ - Moves the end effector to the specified pose. - - Args: - goal_pose (Pose): The desired pose (position and orientation in quaternion format). - max_linear_vel (float): Maximum linear velocity. Defaults to 0.1 m/s. - max_angular_vel (float): Maximum angular velocity. Defaults to 0.5 rad/s. - - Returns: - None - """ - try: - if not isinstance(goal_pose, Pose): - raise TypeError("pose must be an instance of Pose") - - wrench = [0.0] * 6 # No wrench force - position = goal_pose.get_position() - orientation = goal_pose.get_orientation(type="quaternion") - pose = position + orientation - - self.robot.setMode(self.mode.NRT_CARTESIAN_MOTION_FORCE) - self.robot.sendCartesianMotionForce( - pose, wrench, max_linear_vel, max_angular_vel - ) - self.log.info(f"Moved end effector to pose: {pose}") - except Exception as e: - self.log.error(f"Failed to move end effector to goal pose: {str(e)}") - - def move_eef_to_goal_pose_wrench( - self, - goal_pose, - wrench, - max_linear_vel=0.1, - max_angular_vel=0.5, - contact_wrench=None, - ): - """ - Moves the end effector to a target pose with a specified wrench control. - - Args: - goal_pose (Pose): Desired pose of the end effector (position and quaternion orientation). - wrench (list[float]): Target TCP wrench [fx, fy, fz, mx, my, mz] in the force control reference frame. - max_linear_vel (float): Maximum linear velocity. Defaults to 0.1 m/s. - max_angular_vel (float): Maximum angular velocity. Defaults to 0.5 rad/s. - contact_wrench (list[float], optional): Maximum contact wrench for collision detection. - - Returns: - None - """ - try: - if not isinstance(goal_pose, Pose): - raise TypeError("goal_pose must be an instance of Pose") - - position = goal_pose.get_position() - orientation = goal_pose.get_orientation(type="quaternion") - pose = position + orientation # Combine position and orientation - - self.robot.setMode(self.mode.NRT_CARTESIAN_MOTION_FORCE) - - if contact_wrench: - self.robot.setMaxContactWrench(contact_wrench) - self.log.info(f"Set contact wrench limit: {contact_wrench}") - - self.robot.sendCartesianMotionForce( - pose, wrench, max_linear_vel, max_angular_vel - ) - self.log.info(f"Moved EEF to pose: {goal_pose} with wrench: {wrench}") - except Exception as e: - self.log.error(f"Failed to move EEF to goal pose with wrench: {str(e)}") - - def rotate_eef_tcp(self, axis, angle): - """ - Rotates the end effector TCP around a specified axis by a given angle. - - Args: - axis (str): Axis to rotate around ('x', 'y', or 'z'). - angle (float): Rotation angle in radians. - - Returns: - None - """ - # TODO - try: - current_pose = self.get_current_eef_pose() - position = current_pose.get_position() - quaternion = current_pose.get_orientation(type="quaternion") - - rotation = R.from_quat(quaternion) - if axis == "x": - rotation = rotation * R.from_rotvec([angle, 0, 0]) - elif axis == "y": - rotation = rotation * R.from_rotvec([0, angle, 0]) - elif axis == "z": - rotation = rotation * R.from_rotvec([0, 0, angle]) - else: - raise ValueError("Invalid axis. Axis must be one of 'x', 'y', or 'z'.") - - new_quaternion = list(rotation.as_quat()) - new_pose = Pose(position, new_quaternion) - - self.move_eef_to_goal_pose(new_pose) - self.log.info(f"Rotated EEF TCP around {axis}-axis by {angle} radians.") - except Exception as e: - self.log.error(f"Failed to rotate EEF TCP: {str(e)}") - - def rotate_eef_joint(self, angle): - """ - Rotates the end effector of the robot arm using the last joint. - - Args: - angle (float): Desired rotation angle in radians. - - Returns: - None - """ - # TODO - try: - current_joint_values = self.get_current_joint_values() - target_joint_values = current_joint_values.copy() - - if len(target_joint_values) <= 6: - raise ValueError( - "Joint array is too short to perform EEF joint rotation." - ) - - target_joint_values[6] += angle # Assuming the 7th joint rotates the EEF - DOF = len(self.robot_states.q) - target_vel = [0.0] * DOF - target_acc = [0.0] * DOF - MAX_VEL = [1.0] * DOF - MAX_ACC = [1.0] * DOF - - self.robot.setMode(self.mode.NRT_JOINT_POSITION) - self.robot.sendJointPosition( - target_joint_values, target_vel, target_acc, MAX_VEL, MAX_ACC - ) - self.log.info(f"Rotated EEF joint by {angle} radians.") - except Exception as e: - self.log.error(f"Failed to rotate EEF joint: {str(e)}") - - # ---------------------------------------------------------------- - # Functions for IK - # ---------------------------------------------------------------- - - def joints_to_cartesian(self, joint_values): - """ - Converts the robot's joint angles to its Cartesian coordinates. - - Args: - joint_values (list[float]): A list of joint angles for the robot arm. - - Returns: - Pose: A Pose object representing the Cartesian position and quaternion orientation. - - Raises: - ValueError: If the number of joint values does not match the number of active joints. - """ - try: - if len(joint_values) != len(self.active_joints): - raise ValueError( - "The number of joint values does not match the number of active joints." - ) - - full_joint_values = np.zeros(len(self.robot_chain.links)) - active_joint_indices = [ - self.robot_chain.links.index(joint) for joint in self.active_joints - ] - - for i, joint_value in enumerate(joint_values): - full_joint_values[active_joint_indices[i]] = joint_value - - cartesian_matrix = self.robot_chain.forward_kinematics(full_joint_values) - position = cartesian_matrix[:3, 3] - orientation_matrix = cartesian_matrix[:3, :3] - quaternion = R.from_matrix(orientation_matrix).as_quat() - - self.log.info(f"Converted joint values {joint_values} to Cartesian Pose.") - return Pose(position, quaternion) - except Exception as e: - self.log.error(f"Error converting joint values to Cartesian: {str(e)}") - raise - - def cartesian_to_joints(self, position, orientation): - """ - Converts the robot's Cartesian coordinates to its joint angles. - - Args: - position (list[float]): Cartesian position of the robot arm. - orientation (list[float]): Quaternion orientation of the robot arm. - - Returns: - list[float]: Joint angles corresponding to the given Cartesian coordinates. - - Raises: - ValueError: If the solution is invalid or out of bounds. - """ - try: - rotation_matrix = R.from_quat(orientation).as_matrix() - - target_pose = np.eye(4) - target_pose[:3, :3] = rotation_matrix - target_pose[:3, 3] = position - - initial_joint_values = [0] * len(self.robot_chain) - joint_values = ikpy.inverse_kinematics.inverse_kinematic_optimization( - chain=self.robot_chain, - target_frame=target_pose, - starting_nodes_angles=initial_joint_values, - orientation_mode="all", - ) - - if not self._validate_joint_limits(joint_values): - raise ValueError("Joint values exceed physical joint limits.") - - self.log.info(f"Converted Cartesian position {position} to joint values.") - return joint_values[1:8] - except Exception as e: - self.log.error(f"Error converting Cartesian to joint values: {str(e)}") - raise - - def _validate_joint_limits(self, joint_values): - """ - Validates if the joint values are within the robot's physical joint limits. - - Args: - joint_values (list[float]): Joint values to validate. - - Returns: - bool: True if all joint values are within limits, False otherwise. - """ - joint_bounds = self.get_joint_bounds() - for joint_value, (lower, upper) in zip(joint_values, joint_bounds): - if not (lower <= joint_value <= upper): - return False - return True - - def calculate_IK_error(self, goal_position, goal_orientation): - """ - Calculates the inverse kinematics (IK) error for reaching a target pose. - - Args: - goal_position (list[float]): Desired Cartesian position of the robot arm. - goal_orientation (list[float]): Desired quaternion orientation of the robot arm. - - Returns: - float: The IK error as the Euclidean distance between the desired and actual pose. - """ - try: - joint_values = self.cartesian_to_joints(goal_position, goal_orientation) - actual_pose = self.joints_to_cartesian(joint_values) - - position_error = np.linalg.norm( - np.array(goal_position) - np.array(actual_pose.get_position()) - ) - orientation_error = np.linalg.norm( - np.array(goal_orientation) - - np.array(actual_pose.get_orientation(type="quaternion")) - ) - - total_error = position_error + orientation_error - self.log.info( - f"Calculated IK error. Position error: {position_error}, Orientation error: {orientation_error}." - ) - return total_error - except Exception as e: - self.log.error(f"Failed to calculate IK error: {str(e)}") - raise - - # ---------------------------------------------------------------- - # Gripper Functions - # ---------------------------------------------------------------- - - def find_gripper(self): - """ - Searches for the gripper on available serial ports and returns the port if found. - - Returns: - str: The serial port where the gripper is connected, or None if not found. - """ - ports = [f"/dev/ttyS{i}" for i in range(2)] + [ - f"/dev/ttyUSB{i}" for i in range(2) - ] - - for port in ports: - try: - print(f"Trying port: {port}") - ser = serial.Serial(port, baudrate=115200, timeout=1.0) - device = mm.Instrument(ser, 9, mode=mm.MODE_RTU) - device.write_registers(1000, [0, 100, 0]) - registers = device.read_registers(2000, 3, 4) - posRequestEchoReg3 = registers[1] & 0b0000000011111111 - - if posRequestEchoReg3 == 100: - print(f"Gripper found on port: {port}") - return port - except Exception as e: - print(f"Port {port} is not the gripper: {e}") - finally: - # Ensure the serial port is closed after each attempt - if "ser" in locals() and ser.is_open: - ser.close() - - return None - - def connect_gripper(self): - """ - Connects to the gripper and activates it if necessary. - """ - try: - gripper_port = self.find_gripper() - if gripper_port: - self.gripper = pyRobotiqGripper.RobotiqGripper(portname=gripper_port) - self.log.info(f"Connected to gripper on port {self.gripper.portname}.") - - if not self.gripper.isActivated(): - self.log.info("Activating gripper...") - self.gripper.activate() - else: - self.log.error("Failed to connect to the gripper. No port detected.") - except Exception as e: - self.log.error(f"Failed to connect to gripper: {str(e)}") - - def gripper_goto(self, value, speed=255, force=255): - """ - Moves the gripper to a specified position with given speed and force. - - Args: - value (int): Position of the gripper (0: open, 255: closed). - speed (int): Speed of the gripper movement (0-255). - force (int): Force applied by the gripper (0-255). - """ - try: - if self.gripper: - self.gripper.goTo(position=value, speed=speed, force=force) - self.log.info( - f"Gripper moved to position {value} with speed {speed} and force {force}." - ) - else: - self.log.error("Gripper not connected. Call connect_gripper() first.") - except Exception as e: - self.log.error(f"Failed to move gripper: {str(e)}") - - def open_gripper(self, speed=255, force=255): - """ - Opens the gripper to its maximum position with specified speed and force. - - Args: - speed (int): Speed of the gripper movement (0-255). Default is 255. - force (int): Force applied by the gripper (0-255). Default is 255. - """ - try: - self.gripper_goto(value=0, speed=speed, force=force) - self.log.info(f"Opened gripper with speed={speed} and force={force}.") - except Exception as e: - self.log.error(f"Failed to open gripper: {str(e)}") - - def close_gripper(self, speed=255, force=255): - """ - Closes the gripper to its minimum position with specified speed and force. - - Args: - speed (int): Speed of the gripper movement (0-255). Default is 255. - force (int): Force applied by the gripper (0-255). Default is 255. - """ - try: - self.gripper_goto(value=255, speed=speed, force=force) - self.log.info(f"Closed gripper with speed={speed} and force={force}.") - except Exception as e: - self.log.error(f"Failed to close gripper: {str(e)}") - - # ---------------------------------------------------------------- - # Other Functions - # ---------------------------------------------------------------- - # def wait_for_motion_completion(self, target_joint_values, error_threshold=0.01, speed_threshold=0.01, timeout=10): - # """ - # Waits until the robot's motion is completed by checking the error between current and target joint values - # and monitoring joint speeds. - - # Args: - # target_joint_values (list[float]): The target joint values (in radians). - # error_threshold (float): The allowable error threshold for each joint (in radians). Default is 0.01 rad. - # speed_threshold (float): The allowable speed threshold for each joint (in rad/s). Default is 0.01 rad/s. - # timeout (int): The maximum time to wait for completion (in seconds). Default is 10 seconds. - - # Returns: - # bool: True if motion is completed within timeout, False otherwise. - # """ - # motion_complete = {"status": False} - # start_time = rospy.get_time() - - # def check_motion(event): - # self.update_robot_states() - # current_joint_values = self.robot_states.q - # current_joint_speeds = self.robot_states.dq - # # Calculate the absolute difference between current and target joint values - # errors = [abs(c - t) for c, t in zip(current_joint_values, target_joint_values)] - # # Check if motion is completed - # if all(error <= error_threshold for error in errors) and all(abs(speed) <= speed_threshold for speed in current_joint_speeds): - # motion_complete["status"] = True - # rospy.loginfo("Motion completed: Joint values and speeds are within thresholds.") - # rospy.signal_shutdown("Motion completed") - # if rospy.get_time() - start_time > timeout: - # rospy.logerr("Timeout: Motion did not complete within the specified time.") - # rospy.signal_shutdown("Timeout") - - # # Start ROS Timer for checking motion status - # rospy.Timer(rospy.Duration(0.01), check_motion) # Check every 10ms - # rospy.spin() - - # return motion_complete["status"] - - def wait_for_joints( - self, - target_joint_values, - error_threshold=0.01, - speed_threshold=0.01, - timeout=5, - ): - """ - Waits until the robot's motion is completed by checking the error between current and target joint values - and monitoring joint speeds. - - Args: - target_joint_values (list[float]): The target joint values (in radians). - error_threshold (float): The allowable error threshold for each joint (in radians). Default is 0.01 rad. - speed_threshold (float): The allowable speed threshold for each joint (in rad/s). Default is 0.01 rad/s. - timeout (int): The maximum time to wait for completion (in seconds). Default is 10 seconds. - - Returns: - bool: True if motion is completed within timeout, False otherwise. - """ - start_time = rospy.get_time() - while rospy.get_time() - start_time < timeout: - self.update_robot_states() - current_joint_values = self.robot_states.q - current_joint_speeds = self.robot_states.dq - - errors = [ - abs(c - t) for c, t in zip(current_joint_values, target_joint_values) - ] - if all(error <= error_threshold for error in errors) and all( - abs(speed) <= speed_threshold for speed in current_joint_speeds - ): - rospy.loginfo("Motion completed successfully.") - return True # Exit on success - - rospy.sleep(0.002) # Check every 5ms - - rospy.logerr("Timeout reached.") - return False # Timeout occurred - - def wait_for_eef( - self, - target_tcp_pose, - position_error_threshold=0.03, - timeout=5, - ): - """ - Waits until the robot's motion is completed by checking the error between current and target tcp pose - and monitoring tcp velocities. - - Args: - target_tcp_pose (Pose): The target tcp pose (position and orientation in quaternion format). - position_error_threshold (float): The allowable position error threshold (in meters). Default is 0.01 m. - orientation_error_threshold (float): The allowable orientation error threshold (in radians). Default is 0.01 rad. - velocity_threshold (float): The allowable velocity threshold for the tcp (in m/s for linear and rad/s for angular). Default is 0.01 rad/s. - timeout (int): The maximum time to wait for completion (in seconds). Default is 10 seconds. - - Returns: - bool: True if motion is completed within timeout, False otherwise. - """ - position = target_tcp_pose.get_position() - orientation = target_tcp_pose.get_orientation(type="quaternion") - target_tcp_pose = position + orientation # Combine position and orientation - - print(f"target_tcp_pose:{target_tcp_pose}") - - start_time = rospy.get_time() - while rospy.get_time() - start_time < timeout: - self.update_robot_states() - current_tcp_pose = self.robot_states.tcpPose # Get the current TCP pose - # current_tcp_velocity = ( - # self.robot_states.tcpVel - # ) # Get the current TCP velocity - - # Compute position error (Euclidean distance between position vectors) - position_error = np.linalg.norm( - np.array(current_tcp_pose[:3]) - np.array(target_tcp_pose[:3]) - ) - - # Check if position errors are within threshold and velocities are below threshold - if ( - position_error <= position_error_threshold - ): # and np.linalg.norm(current_tcp_velocity[:3]) <= velocity_threshold - rospy.loginfo("Motion completed successfully.") - return True # Exit on success - - rospy.sleep(0.01) # Check every 10ms - - rospy.logerr("Timeout reached.") - return False # Timeout occurred - - def compute_orientation_error(self, current_orientation, target_orientation): - """ - Compute the orientation error between two quaternions. - """ - # Convert quaternions to rotation matrices or use quaternion distance - q_current = np.array(current_orientation) - q_target = np.array(target_orientation) - - # Compute quaternion distance (angle) - dot_product = np.dot(q_current, q_target) - angle = 2 * np.arccos( - np.clip(dot_product, -1.0, 1.0) - ) # Returns the angle in radians - return angle diff --git a/RoboticsToolBox/Bestman_real_xarm6.py b/Robotics_API/Bestman_Real_Xarm.py similarity index 84% rename from RoboticsToolBox/Bestman_real_xarm6.py rename to Robotics_API/Bestman_Real_Xarm.py index ba59ed7..0ea3ecc 100644 --- a/RoboticsToolBox/Bestman_real_xarm6.py +++ b/Robotics_API/Bestman_Real_Xarm.py @@ -23,11 +23,15 @@ class Bestman_Real_Xarm6: """ - Class to interface with the xArm robotic arm, optimized for consistency with Flexiv API. - """ - - """ - Functions for initalization + A class to interface with the XArm6 robotic arm. + + Attributes: + robot: The XArm6 API object initialized with the provided IP address. + frequency: The control frequency for the robot. + log: A logger object to log messages for debugging and monitoring. + robot_states: A container for storing the current state of the robot. + mode: Stores the current operation mode. + gripper: The gripper object for controlling the end effector. """ def __init__(self, robot_ip, local_ip, frequency): @@ -38,38 +42,107 @@ def __init__(self, robot_ip, local_ip, frequency): robot_ip (str): The IP address of the xArm robot. frequency (float): Control frequency for the robot. """ - # Initialize robot + # Robot initialization self.robot = XArmAPI(robot_ip) + self.gripper = None self.frequency = frequency - self.gripper = True # Default gripper is enabled - self.log_file = "command_log.tum" - - # Initialize robot states - self.robot_states = self.robot.set_state(0) + self.log = self.robot.logger self.mode = self.robot.set_mode(0) # Default mode - self.update_robot_states() - + self.robot_states = self.robot.set_state(0) - # # URDF - # urdf_file = os.path.join(current_dir, "../Asset/xarm6_robot.urdf") - # self.robot_chain = Chain.from_urdf_file(urdf_file) - # self.active_joints = [ - # joint for joint in self.robot_chain.links - # if isinstance(joint, ikpy.link.URDFLink) and (joint.joint_type == 'revolute' or joint.joint_type == 'prismatic') - # ] + # Parse URDF for kinematic chain + current_dir = os.path.dirname(os.path.abspath(__file__)) + urdf_file = os.path.join(current_dir, "../Asset/xarm6_robot.urdf") - """ - Functions for xarm itself - """ + if not os.path.exists(urdf_file): + raise FileNotFoundError(f"URDF file not found: {urdf_file}") + + # Parse the URDF file and configure the kinematic chain + self.robot_chain = Chain.from_urdf_file(urdf_file) + + # Define active links mask for xArm6 (Base + 6 DOF + End Effector) + active_links_mask = [False] + [True] * 6 + [False] # Adjust for 6-DOF robot + self.robot_chain.active_links_mask = active_links_mask + + # Filter active joints (revolute or prismatic) + self.active_joints = [ + joint + for joint in self.robot_chain.links + if isinstance(joint, URDFLink) + and (joint.joint_type == "revolute" or joint.joint_type == "prismatic") + ] + + # Validate the number of active joints for xArm6 + if len(self.active_joints) != 6: # xArm6 is a 6-DOF robot + raise ValueError( + f"Expected 6 active joints, but found {len(self.active_joints)}. " + f"Check the URDF file and active_links_mask." + ) + + # Log successful initialization + self.log.info("xArm6 robot kinematic chain successfully initialized.") + + # Additional initializations can go here + self.log_file = "xarm_command_log.tum" + + # ---------------------------------------------------------------- + # Device Initialization and State Management + # ---------------------------------------------------------------- + + def initialize_robot(self): + """ + Initializes the robot by clearing faults and enabling it. + + This function ensures the robot is operational by: + 1. Clearing any existing faults and warnings. + 2. Setting the robot state to operational (ready). + 3. Waiting until the robot becomes ready. + + Returns: + bool: True if the robot is successfully initialized, False otherwise. + """ + try: + # Step 1: Clear faults and warnings + self.log.info("Checking for faults on the robot...") + self.robot.clean_error() # Clear errors + self.robot.clean_warn() # Clear warnings + time.sleep(1) # Wait for a short duration to allow the robot to reset + + # Step 2: Set the robot to operational state + self.log.info("Setting robot to operational state...") + self.robot.set_state(0) # Set state to '0' (Ready) + time.sleep(1) # Wait for state transition + + # Step 3: Check for ready state + for seconds_waited in range(10): + if self.robot.state == 0: # State '0' indicates Ready + self.log.info("Robot is now operational.") + return True + time.sleep(1) + + self.log.warn("Robot is not operational after 10 seconds.") + return False + + except Exception as e: + self.log.error(f"Failed to initialize the robot: {str(e)}") + return False - def clear_fault(self): - """Clear fault, and updates the current robot states.""" - self.robot.set_state(0) def update_robot_states(self): - """Updates the current robot states.""" + """ + Updates the current robot states by fetching them from the robot. + """ self.robot.getRobotStates(self.robot_states) + def go_home(self, dist=0): + """ + Moves the robot arm to its initial (home) pose. + """ + self.robot.set_position( + x=396.4 + dist, y=-5.5, z=360, roll=-90, pitch=-90, yaw=-90, wait=False + ) + time.sleep(3) + def set_mode(self, _mode): """ Parameters: @@ -88,14 +161,6 @@ def set_mode(self, _mode): print("set mode") self.robot.set_mode(_mode) - def go_home(self, dist=0): - """Move arm to initial pose.""" - - self.robot.set_position( - x=396.4 + dist, y=-5.5, z=360, roll=-90, pitch=-90, yaw=-90, wait=False - ) - time.sleep(3) - def pose_to_euler(self, pose): """ Convert robot pose from a list [x, y, z, qw, qx, qy, qz] to [x, y, z] and Euler angles. @@ -342,9 +407,7 @@ def move_eef_to_tcp_velocity(self, _velocity_setpoint, _duration): duration=_duration, ) - def move_eef_to_goal_pose( - self, goal_pose, speed=1000, mvacc=50000, wait=False - ): + def move_eef_to_goal_pose(self, goal_pose, speed=1000, mvacc=50000, wait=False): """ Move arm's end effector to a target position. diff --git a/Robotics_API/__init__.py b/Robotics_API/__init__.py index b142588..0ddfe4e 100644 --- a/Robotics_API/__init__.py +++ b/Robotics_API/__init__.py @@ -1,2 +1,2 @@ -from .Bestman_Real_Flexiv import Bestman_Real_Flexiv +from .Bestman_Real_Xarm import Bestman_Real_Xarm from .Pose import Pose \ No newline at end of file diff --git a/Robotics_API/__pycache__/Bestman_Real_Flexiv.cpython-312.pyc b/Robotics_API/__pycache__/Bestman_Real_Flexiv.cpython-312.pyc deleted file mode 100644 index f75125b2ca784932aa945447b8630d9bb88dcb32..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 43835 zcmeIbdsG}(nkN|f2*`v45)zU}@<>1uAfc2HdZH&!NtT3c3CU&IE-FPN2nk3OnOGtV zxvIU@r%z2E!$?$7h`ayT468ag}F^51jZ|3yF4 z!>(N1{|+v0asqdb6D)#t#B$EU&en5QcD9|f3pT+%!jJOjcnjs>N3uq<&pFt&b;Nnj ziF;WiIitDfa@oD?5!Yzmxjf~0{<(ZsPr*4iI~SfSWapxDMeOW3=dp5!Il=KQPH?_u zm3wurc+%RG6Z!IA!+kl+@BPN z$A-4;ICLJT3&GK7cq|YSCMauHC^8@pk4J~YV{%jN{`0}e@PL0{Bp8YK2T{G;HNO}> zACBTdP#mR#XClEN^<7vWd&6U)oKNY!dq1@xZL&!AC&Q5t&W=;zXpow~HQ#w5I7}Cg zGpD+Gjt`Gr8j#0IPL+YDMLOxQrP=K^ha3d{pazedO^exLdkFZYg;9TtF z=%27&D8{c6p#-_Sf){D2P>QrnC_`Ebt2p$_HhCkvaJB#+6w2rgk|dsz8ohEE*Mu&rd`{kv8_5s$w6s&EJ<%&L2L1F*FcG^~2HO;K*<+B*+bn zi{TfB1^hbtq(3MKVki;`MFLuNLzqV6AyKYS`JvWD(`X<(78S!IBbb7$uhe`)#2*?H z{Gq`?v=|ok$_B;I<%!VPz@%KAQr55CW)qX%n7xEvZX`U^hB3lvC?u+_W2|t)KZ*`x z&dQTo2%Vo88lt`g(Sgx0UN$V^r(V@)SO`&FVrVcVqAMXk>M;y*7%$5vHA)M{fnZ(g zk6^}QwCRK4#WCg49~y>;+zzX0U_wOAQQSjbe|S(Em#E$ixwZjXG*sCoR5Xfau|Vlo z`ZzTE$Ae-7UuaMakFv^Wjrj)wdGrc{1JU6ZLj4!Rm_Mw4Bg2s>H7-B!H;SPb!Xua} ze)M)+9FDN&n*4GBObfGKVto6l8ToV36}o~+NpDOag9e(t2}WZSKu=I6&VUNOY4vC0 z&xXJIA0xfVMG1&u=L| zQ6fuy5>+p_i}~86c7dD@^@pMEBK4krHq@=P9QCTtFP5oS`uUh)X7XsYLH_D9rKtCrNyQ+7cc^K1Mg%Gc}V)T=38Z9S47U9a91Y*YMe-1n^NvzIK< z2KCv+M)gWR>$PC~OVs;GjMtw4A8L(Q|+g#zA@`8<+k~xHl=s2pOn+O9iMyi9s zXfZSbC@ByC83(BW;HBU;D%~y)8892&k>Td>xW6$H6`N$Zq$4gB*cw~WK_rzQ2@ilQ zOE1sv8PFdCZv@aL#)by~lmX;4RLt5>uTsZeje{fMU=+RTl#$}25nFF;Lm;rF9Vj=t zWsuQ|{o-&eJhr`G!w8YUgeVNgN;H`79}7p(#fdSY&EM1aDS=nXp+a}bsloQxy6&sv zK)@K>T`Crkp?$L-G=p4)Kh_-ZcU}k$TuMhQ?As_f1-SiEq<=IRxfF=)G!E@5v=x~c z7yvpRoERCIG*TLYSb>5d`hhkA(TUL}zvPr(SQIWv{3)VBC7zA0l&z2%$f42;lI?hS zNV1`@WIqACEZGS=OS!DvX@-*}7b~q`XvrFmNOnS2QnoNGj)6j#vV-R%?2e{^U{iY&Ab4VGN0s?C+0Nnoza@^=UI&jC=MBC^ri}H6NR;N=RPdl zvalo7+L>tWy!G`DTTk7~=JJZ}*-(NSuJ|mUa}`WSQw6n&g4$$m-CXd)+=i8+%H^V+ z??e{2r}mym>^+g()f?Y=@-An2(y||E;eq=$njEsL^BkeU%~- zUqcySgEm^o|uKLsrmw~oX-p%r5-lKAj_b5=?)X##3 zS1sJ6eT{O3dVcjv(C8=DL$gscE0y)g_wuMM=x4*I=`>chT2mMId@raUq6+jP+SSEU z?NYl?>l3TK!23GX_ADGm%K7-;@8u`$^bO5M$ZTHa=$#{#kFVpzmTT5EYBKw> zxlxTu1An5-CnI%h;hq6ibj|vrbK_bFj6@?02ZzRj=NWT74DL~@qVK{m7~Y97Fny5lgvKHh;M;W$&X1=WBO4q+ z-MW0B1^8ZVCpb3gXSh#qRHtpW1^nG=<0d?LYry|>5V8bxcVa9G!IYNAe?A0kkNz0C zVCeg)P;^2Z%b@Gdhr=VF=ERARf7rk<(~GP{R5bg0f+G>|vuIenG8{25!yr>=1pVOw zh7<)q21y>}NJxAkBsTkl(P(HC@-g%t{68D`Kp-FzM2pqSg$IITL}{H5$&W)qGmRA$ zLC&5BJ|!28B}gn|F9Q!sOhzsc_>`E$2jvXC(wmKiucVDp--C^YXXw5vi@+xg4ni#C zZ`taPU~q}ij^XvekPFm*U8tjfg*Jb0Q@-d%i&FM*gmqRdreF3eK@qeDduMiAvPU7A zl=#R0$pB zOplNtSwaySR?L2p7%9oE_qLyXl-N2m3L-OCKgvedVI6&wyIbaSmVaE}c_nr|CZort zg8F55@lD5!<7VDWUcCD3lKW}8ku#H%a<5Og*DtvnR^;EaE#Jvo_LSain`ujV)+apc zmpl#2#bq~-%p4(lBVK=Gsrcx!&!1M`-X-@wAkVzQSB_ji^2&+pC*l zTz?|vs!F)3mRvQUOX?fvt$)#UkF)1h-^kb6*_7~XTJmgG+c`0FB3`$DsrbMzeKlIQ zzMFT~iBfl6Y0dO3xehOT%0B<3pzI!Jb(T|;)Q8^dz45C3KOMML`;)=Nuf%i%Ct z@N)CZpv(V;_RZ0g83OxfjQ9|+>@c<XHhq%z0y#5MAb%(nWi*B?G!cm;e$&FHMuf;(1xI@(Skp3oRzxbX2D8aw zl5r08L@JT6Ndg{}eWKcgqjqjxs;)Ir*ZPrTyLb#4%tv(=PreB9F(1{7nzSF)DThAN zD5KsqoG+U2nMUpuYF}HdUHd*K!Y`yz-O!;jY7C^iLcRDKNygITABVhx z)Z4@>1RCUjB-Rp1axInip6cs=^3>52NBfSR?e6dXN_Xd(zN065C3om*XaFjlC&l4W z5TYPtX#yI*TlP@6pIVIh%)XWK@PDL2M2e+-pNfQ|Vs`uMdtTj>^6g9b_ANQ~Ys;%X z;i#WG@{wb+{MBggt4A&_*jo4|rq4}iS9OgE#+U$2#)0uv?(X2O*`{n4A=l45rYw+C zTLr6`v>>W|qbQNn?9g>a>K8_N+xP71FG#HI^iOS>@!4i_Z!_MHXT8aqvVz|;)JPm< z3kLrRWZ%zl#L2$AN#^&!Y0_F_2jetJ7GTzz34bnR>u7OGYi$*$X}@NF(GE_Noei9( zBPMCH^As2bQ2@46m2-*|8?-)zlB44c&qaeGAOg)#Oeok7=q%=H**kFwa!F0l-3MJQ z-Btzfij-HM>csQPZS$S}HDa_35@a(hmmihX>Y<#ovomPkepGdYIPHsFBZ#sIX zDDFGH3x9NW;Yi%KKjk`*a2@!^yEbHc5Fz#1OJ2wZXLq#kvfa*W!sYEFD!c=V*)dqF$NEj#A@0>7Oh=>8{=?#WBhSy_<b*0n~K$&t?CR~*%S6#wYH`g+^E$-TI-)1e#A>L#6 z^~2Nc(@_A{yrLW3v$eCqSJ%&$&ROTS|6tD>d*(L9ivmloEh@AHh@S19tDOtJv3{;} z-a5biFZaB;XMR(>V&_M$UB3ZSvip%U`^#u()`X%aP|-}&1B)C`X}Wy}Xe!HT4z!Op z%QY#xS&bTO#qcVJV2x@s{UK6z8yiKK9s4+RO%AH9%wV^)Pw!AMHw;%a1dY zA=Z>_43sJ`9nX^m+A4^y&Gs_2X+e=VW9p&hHh-*Ar%s6Ek{LPmX>U`u0UkT$=6WYa z<$2zT5t2FkE?lWIK8t4cuppBlX|q0uz^wcjmfE7fL$X}f0la8F`)xva&}8ao)6_?+YjD8w!@oxx5i$a#NuTX3Veoui z!nJP6wPB?Q^moeJknlF7yiEyjQ?h8|G!tbhqW{@r^R~H(c*W)=S76!Woqj@{|5S}G zo8R%yz+&x>1{WZkKd|IFxZ){h^S?x$|M~5O-0ebVN3QL5MMFoH?cFRM*J+fpHvey+ zt47O7eV#`(?ip*t1kS3nJM;Xz&vrd(%6zjZE}YS@!#*psYp zOIGbomhGF)BQcC>#!=R?uz8^-?rmRkbSxKC#vPRpAr9<1*hC|lhq-X39NE4QX`qQ? z^!f#`OQxiOM%-%P(PYY+lDoF31~eZ69_@+(o}JUc<65Kv(UK}G?CX5e-`EF5w7(N} zh(PmCk}-7?qlh2HvoS(}`GF|30aZris6H7*LL{+b^#Lr;)$R` zGSxx)t&~flI+lGkDPMEK*PQYN628Fnadla+)w6A3-$Ec>(Z1yBSoYM!T{SAnq3OsN z$=&mQkht@ucD~)g-F7%T?6%v5jkJpPSm^p7 zkJNk>?ffsXikJ^y1&7%W0f}J2l`Trd4m%)hv#EmmYa}{1BcaWRa@&+W1MAosLz+&M z^J~-uPGi>XD+FQR;{vC-c!aUyP7b6 z5_J%1L}%4CN(_y|9t=oKQ4FR@)lom%W#WD&EQEs;axz(uz7TsqB~~it>r|Gb(VvTn z<*_EA_*M;&R9u$UkdjZO?ipd94O~M#744{g?S+{oOWqxXnb6sgWMWzYQd^GH_&4JW7J8DHHV=oeX*AMQl*WF(#BNj)Yyc9UG+bz&7R^tD%Wx2v5UWwzT54ILiayB;3bX{(rMGy0DAJM>kIpP7f5 zct>TbuUT{!BTa0ouUE}{7lKge2C%BFrixU}a8k6qT%y-XiZ#JzP}h$($V!x_xXjq9SuQ0woD6|E`V0A(;LbgWv@{59K)wn<)KXTyrlian(*MkYK> zbInYXnvzvvcx-SO(x2c9SSMshM^=Z5zox#@VqT_?ho{V7m`q1hhfc}TFJ46FOqZFA zk_}Q?A`)|S36gjTB_$iV0^~qtCq^UW!2-{U5Egn=l<^=#i#o|@mkmUS`H}1_7h6}f zP-$H$#(T1ul(i402*OlT}j)(E=OiOLTUa3pdX!(xXwMC&fVYV#P zNq;R12n+saru{crI%=!e98->qn)9g~pm<)n&b4e?YRv#S*BcS4t10O(pnYWP9D?nKkntQ5XDu2Lw)q1G{h%9T$&CZsW z%f>ADE}G9K^gem;T|_?sqvgTt_YPGeb^qh6R9B}3x> zLI^%m!_i4b=LF%K28F#s=xA-MwsSCY1G2@!voccy!vzFZ7$$S?v__4*4AZtU9WBeE z!+CyY>UY8IL zF=KlFv*I%-@WAqX%tzB)AXzAVQF&&m+-z)}PGpZFn|ZFLVj1;A4b90mWJP2{_Qc7f zy|5%Zc^ZafCwntw>@sW2uszQPUkp*{0ZPtOGD1lc5~&cZguvbjq&s6AWXipha_9s^ zkxW4R9u-jyWiXAkYkU_eTbVcV1wN-_gRoxWgCpY?f>Mr7S4cK0S|G@C9p0x``&Zqd z3Kia6HnkAPs7J)}OZ?@2m@2SS@T$xq5YnPmn10A~O&JX?Q<+7>kHO-vmAi-NoOh6b z&hghdk25a8=JnU!Qz+GF7uTQL}f+wa>`^n`<5JW^pBD)7{JERj*gPTJd_# zt2IgZ?>(`+^H6GMPhw}!-&7x-kIY8q&b;>Zc=h4wlSn>xd+yq-HTgJ$Dyk`-J_$d- z(&`VrTjy$~4=;PG)T@S;c*FizD{gh(+Ht4#_FmMw>JA91%9`2O>)&|w8w)vqRq(@t zr0>x5@s*9O?{qG@ZjB_jpH6Pog>z56Q8Jf3cV+(a{J=uZt&&^Wx5PW^@07-$d@5dc3fgm@_bz8G@!sW8 zBdkH-ZNY}FG2v=lc2~|`oZq-Gw0Pw+n-$KDY0emxyB1vwFUqz}zrDAGt2tu%{Iji! zS<~@!YOwwN`dh*GHr#?<5{?_k60T#68axhakZdAf>Tq;7bMNN3x*P28dfT_-$NRbM zKAb*i;3;i(_H3|yu%n@;*7n!6Jg&`yUHspe1R3U#mMCzWe5hnbBveWTfv{AhS zS4_Ms9gGAqt6k%{XB95qykjaiITZ z^=XcLdP6X})CjgI%{ei>1;}|E*Bmc8CLQwGK(si6lNbPvpde^28`Y!C@;YXn;Tno*~Ux#edX5fw-yJhlc}{G(e8fr8W;;VCvTvR0MK!!_&NAtPhMut0&g zTIQ98iepU98^1jq31;CU&u50EX5ocJgB)}Im|C*|ma@+Kl#kt=?zlD-(i z6&XSn5hA7mhLme#1_I8^Mqf$E+uKwi5>3p6TavR6mCMfjOv|EvDye`ZQwAwkCho2v z+}+uJs;~R>(e~c{6DJ9K_xGGU)!8kT5i0HkLyuVy96N!~UE#5wu!sl)YYMrYiWRFi zouvHqFE16)Z^L(!vaV2QqzmG=>1C!VxlRd_2M|L_-f{st8TZI%@{PgzzxRkOoy7cY4NE8d#8za#1GjJrEO_Lko`GyBxcGcZUg^`}ZV zCrURbOSh2Ftv*%Jny6?c1J~AMMbGrHWsh(6Qo_?T-9@Ibt59%fqJ|VqBqw0cwj^MX z#2n6DTY{1 z0kIHdda0weV<-3SPG{#<+q(xFIyc$g+r;BKO}+rmS(Y!H1*-V+=;IM6BWn$mVSQT@ zi2+dn$TE~cfYc1AYqnbGvvPbXl);O8)@Z>DZD>gC3n2{f$?4#HjTTA!opDS#WKR1_ zA*!`}2MAq3ZXh2FT{+PsGjydPCN($I64_BrA~2=N57OU{p)2Q$PKK^RfeyOvkn^gZW%@}BV+`CGl1;J!DK2QO+Z%FfTTgzMipE#-d%^<@Db3|6&d(U z=@9cV&^dO+)qu3=N`_|#uQvM!C-E0+_K)Iv6xXA%=Lu&LiUzU-?jYl8yq6(R+@J_! zfpnfU9gu!u6uRS|9fh_8wyG#ph0%0omf{1^ev=_e6F!_!n06(Gc|Bcqp!($3z$3L=vkw4KvA%o-jTSSlJ`v?C$O!1FFuKJzV6MM5fId zs9VXcVeItmy-jW%dy7P0i*Hl%uPGs_SJc>R@!#O5oIm^Iu;K-0|LV#4Dcq$aPA^*=N#m_7lHoy>t2JJMX~Wc z*`1p@1KfK7XIG=`yHSd)Rgv9S!Qx>bxc(T6> zr@>v+Gz#j-f=ShwK}Kzu=ov1WPz;dp!~6{reo}1ae9u6!Ou9_*zFECyct7{C@V@IB z;e8jP(4^!2ydd-~I^Ji&d(9YC!jwUg+_Bs>U0Z+a`3aSP&Vy@n}RO zY8XbcTxIshl4Vk|#A2;S#~^kc#%rUqaO0;gt6U|->_nzy_BRkKmB&%=6LK*OUCgetU^ag%1|ifNP+7X{{VAO$2yYb6ve

^Y);q+E81VlRXSm|&J!up+Eo zgguu{D&CxG!G5BUyfA`iW0U%Wi&s=Ie89$QgZl-ruOi-t|-4V*5|B7QkH|O1at-u6Bm0I#f*6mDlCw z-gi5@b8PRIH+1v14|pEe1DePYpH#zCeUFE!$dKb+pzxzSzz%7H_!DVtQoxezxJ7;Jul? zT*D{mka8fWiOADT1FrUl;R>_b%`gn27%j+%mC6&Fbe@L9WSWNqzbTpNBr;u ze1+I9S%()C#UMhN67CB7BM8eMW&8#rw8KA=!F*X?r#2m6vc|th>n<_QMMHlY*kSXG5}}VZPsaq|XT0d!0K{NhD<%+K5V5~Guq4(#a8Seo#B)A#gh-nPmh+`^8<)u!; z1%X8aw=qj1zgjP(oSR|yW!_@FnoB;wm_N&Km{GrsDWM<@nob20CxlHI9;3)h4@E*o5SeC_0!1D6s!)PA zq_stIG?FlCsL#-VDdX}a(?LDLxLfb~W5`U6Ed~)ZGb0Y?SuFx}oBycn<*Yk=%I_j$ z3cnBr^GE?Q5hj2lQsA}poNM{x^^gWr&-P_2>=nO-i4rSSzOH7ErF1RkAtbF23yoqR z*35KqBv7OoK@>+?^TrZz5A?_=7a&D3g0YSvHfAg&h9kN!mc+v^Fm#of%1A}{x(KZ) z7(%y;|1*j!d>e*I9KI^vrV4cgZKt0ZOq$DJM^i|#)YA-7#1^WvSRP^3?NIhkWackY zE_*E*p-b)oRaXcT8rFetQ}H5$6kUD5*kXp!iFQ>N5S?W~qleWSAswGqsd*>`lZ=Ic z+>x-1*(*w&MJokGH})k9{NL}Kx2GE05{+%ib$dT7*vpXkb2<|L%)!;x|G@Ev<89A; zUuyII#OD2}&7FzOoyn%Iq`y1vs`|LSJJo(V(SG_r*{tjGmv`^KVNbdJ3AaDx-T-He@P2N$6Eu*X>-m zoT%ILp}P$sZ}1S-CjND)nk|W%EvcH0L`_G$voBe5Chn?SE^R=_hC=+c&+`e!c-FQo z)Gg58`Gw2)+mNW;6L(dwlvli7^=efdo}J}wsq&6Qc}KFmYx)VIDAbsum8}cM7Y@YR z`{EU6mRx6L|ACE3&*r#m^Kb9@(Cg1Xt7KL!R%cNft#I@g^>>0l-*5*;EvJ@TroZVY&A6Of^H`+ef$m7~vvRSc; zdN6aD)A5*DOfL=V7+ihKXa(I22Bt5ta8>Z3g8c})!uD0Y3oJ=@h_f>|5Sa3IDjgy7 zh*8b7#gw}^x3k`&g^UBVfVm`5i`EU3A1#iyQI?5vngNSk7o}$V#i;T{ zBQBoG5^{vx3XVBERjTC~&Ltafvzj#>qM94y=!({=zaSmOa;3ur=Vrf{ZNRyC!5<^* z!*Q+-jScvfz%(IW84$||*?^!{GkzL^%TV&^wQD&txy*FModN!3;_jz(vl(Nq)%9sF zY>K+moFSiJ2B8YYfv{}w&cbddw0#>50pU(rsH(q*VU!Fba_aF=c7%3%B2^t2*I`Ad z3DQHcKb1o{>ErP!&0QI#%HmKPywscQ5L!hLK-Bwgj}5* zsQXg~`fY;N^(r!o0j(D@lq~)XB_$^WrwUSx({Ed7G9+XDI{IlJzBE256=D2B*nP`@ z)urlH_xj0zdvrL)jLRi2A>Uz&-KpSb#X6Po?>g?!K>H3E?Gt`xHaAiMV~_C}m0n7@ z%9p^%grs66#AG^g@OxC6A#oZU6^Sbdd_bcGR3q1jIRZDyxQh_D3(FkR*-9@Ecy-O| zmtMUzzbRS0GgW;kQGF;`-7$UglZwjMH@v!GuJE-c*x=N)Odp%I&KBaYWA-WhWhGoS z%e8o<+-;xbVHf-Hy7hCdI)YC&9Z1w40HR(Yr`T-?-?o(Rsf6#TWp72wyCLD-uw2!+ zyrFHmcEepKS9!{EpW`d@r;o#8w74wg2_!s$`Kcw(K9Cae+7n4{Z`|FRhGq9GHZE2G zgUeVJ4z+WZx!SmAUEH-!X2l;LyE4O`&ce=O?!97XSB~wys>Uwf_CC+!I*k#hC1%6l zeGv%@dZz$a{L~H&MOyU$AI1&Qz;vQP#&p_PYKOxNkfi2@l0C&MZYU+#Si!=!SAbgy zg)I#h7(w(|WqtxY$V9$zbwN8LZ$7BlL#S+%`Qm+8#7I`FB3jA>vrjOPsDKA7RCUQ1 zp`DrSU;0h~6klYmzRRobA=?D>^dZW}1q?qkeMdx)X1b;&q?(n>pi)fHEq)|?B_vAx z1nq~&woZ|k7h7E7-yutEqnVb&92h)L>!}xou%3#!vdUCxbE34FR2Qz5O_;!o&m}iK zK>-pFTE1cW*sImE>*q?A;R5&UQgO?Cce1$Um!*|=ZCuGVI`hZ}BGp~IIzTJI0XFl9 zWK&$$Oy-_(o-rW`mB~1CAwRa6qZ!Q(GZ!{9i)3BEZziD8MJtcUXE(D|R#%Qnvcw>I zQLmva|L5+jrig!JUrj_zMn%5b$F%;YTWgysA56$odAmI*&$F1A#-Mbb%4S3%;T?7` zqL36IW;JW1shS%^VOEsnTI|pj;;V?lY_g6v5QUE59~vP}XK-WycEM^mYXyVqJBT4R z?9pRQezy4p^LP@;<%tNSG&3{OdnfR+Dxr;-oXH)j-~4 z0y4E?a~n0)xb3k;KFrljHuXMwOc{?L#8jXWyC()fR)k>TEI_)%ES%4WqE|xD#?xbv zW6Uy~qBuf4FrnBb>x7=J`HrpEzK7sfn+cGyv^;j~Gr)qXq}hK`x3`mfgJ8RsK&)C< zWP_fQKdn|1sW0-xzsHmjB}y21l?8k9;0QKw8-wZu%4gE7=(=u$Or(>F=sIh~emO7g z>q_p-&mzO^T=~W2paJ>n7w%{8BNZCHqH-%Y3N6)${6nUjOzfMBv^VTOdQ8q6+h7Lj zSLYiwkWF3>xOO6GS`)t3g%k14(@9?+6x-M`tYTZD0zp2K6^EveeNt8P`ZKRS^ZHj` z{pyOli1sv@txCEZ-masdzYBX)t;Z6r$C9llk{f#y?p}yWs`ICN;i>~UiSrHT5Axs0 zSBk~Uo09I0D@DaO8)q74_ojS16TY2`$K$@8N#D_A(G$P1SqhI?uwhs&q#{*t6Y@31 zH|;vVdOC^?zb}i<|cO8x&elGs}Sp2zgJUkwEUskmm#Rx}IZB91r{b}bd%TIb1 zcgKBQORjE%P(mh~_H^#!-rMKw+6IEDv1_yK{mneC(*hW1$g2|Q$QS`lml8n!kNqc^ z{68D~H(Y0A-&Wc07<+75bnU}KO4%|?#V`vglmUpY|CjU%h-|K*M1WaaIsh~vf-Drw zIH2|Bg{-%;-%@G-`lM;b4pU@k7aV#VO*8Gt+fJ=6eu_8KWC%G!oRIr3w7A94EAwVH zQi4m!6Y{m%u&b(2K=590#{j>XMxv0}ZXGhbe^KW9;|)E!ZIz0SU%)PND zCdfuDNg4zTq#4E(J9nG55wX1d;WtcM!IS^6F8wOQrO0>9VHkm8 z8UdkX%7J;H#c5}|fO!cKzI;|-XB{EduhDz4+Pv>#;iGzfg`9fwX^-fB#g$!&&9=q~XFY@bmB(lr%zWJXD4h2?0~ z#?~Q>fbe~c?UKR_zQ~P-8qqd1HK3}an1V6q79sS)mS}YHbSziBJaf7ume(gv5@|pU zwfTp+cP*UwzoI~sRdSpUP6!k+Sjwib?GSl}og; z$iY+!;tJiPRr=|FrYj4UK&Xb|OgvFlYxjPXy>P{m5*4ZUCdy18xJzd9=7hPkac}dId-IC7k_F9&J=Lw&CGX)Iwv}=pN!;fm^X>0+{ME4^ z9$P9ubR!G<%E*d=x#+)ocFDbGxw1~)TXbRFqGb^~jPAeTTwWiTzx>8?@$Ef{^*wQS z-N&Bw^PUeq&4@q}hm0OgAneFe@m?TvV9nz4KkIqzNW!yj-t(8ff8|>)F1>kl=IG61 z_iWt89Upqu-7VmXHthP{iZT2c<8!pN5sOT4a{8 z?4c~B3d<2|)FS@54m-`_piEib*6b0mx3q#MA?r7{1!Tr^_?4UFGp_GC72n33_5cfEYv zy0Ualg|6F4r+eUMl+A)}X3J+C-!(a6M>LEC`7XhIrfoL>NaowaG?|Ps9A+p78L@|S zP@L4UL6e07fQeBGV#$QXG7=%NumXrwsO%T-VU%Je?G$qs(pVq~B?mTbjI|n=7P&{7 z_+5tB0R+@w&|PtI2?^<(j$X(B;$5PL;HrCk@)YgOJbd}ET$z-g{#&H4Y~XVsql0pI zIHL3wTO~Twv5?sx0_I{D5;dHsOKBDC4WKy_^U_KIlPXJivCB3aXAL~pn{p}XnSMwR z`sb(yk|>AE*(2NCQY^wG1tR=qfp3M>o(B>&2ky9*YR=rSf5(;PqOpHb_}R#hM^Xn) zCk~vB@9&HEosoB7{20-jre>y6rCSoETb4>&(zZX`vsBzhH?T!>s<#OAkrZZ0>*2=@AnYEy-& zB^x-{MwZWjVJZ~UpxjV68aAge zmCpxf^Jw@zf?qdL1>jea(|O23`~FS+!4$zL*NiLWu1&aW=XWgfx6Z(}W66EY0KAsH zTNg?evKOXqb>G@`XH)#_v+*;}#h-gVUi{VnXp?P73rpCxvJ51_isY@!>a2;f@=#UDF+6&qP-0dPCr2%J0qwV&thK_pMyY)P-&8;p-ezvy2p7!6Evcf0K z2wyN1nTMGz1|SPr^TE>!DWB${L==N2!C(-AEGo_FHH9?>D-6w&(u885@QOq+;B0^p zj-b;BAx>glu7VI`Ev7FtT1^~^UJQ(lP(CQtsygA#JyFSNMTI2gq7VYTaI3QgfJ zzS!5Sj`5~%+C)Ds6m7}GDrj7=>{Sq^zI;g7|G==`_sf)!Ve18gb80E9+&8ntTB><1 zh4h@xOW{nDeAQI8~6!3M*0xrwwLxxCEE4V(oHo?)Gd>nRtD zC!D@^@))(DEuLCaTAeCWv;6z?)zfZRJ)QFti)0N+5yExZeZ2A^tA`cW_bb_n-2_{H z^uhvyhaXJ24kcWN7y@gzJP;f40t1SB5<@SVSiS>2`nL7kHlt|X3|XkmEOfD`OFkZ! zZ;B_4=#>2u>KB)QfQ+px<<&YFVXdJowRRBJCTuP<&oj%b5yg6WI16hbPT-=Y3DTi( za3IvFjj0^Kf-_4D$2F(H8QC%AG-`CAiYQ?rP$`$j>Q7q-5i|T-FrDwxoWoUpu(;8X z&xU+fd2>kCCkf8hEgW( zW@(H_HWRbkt~swcu5e9`;9sE`jWu{WNLYRX;mb!Dd8`_sjIwY?gm$sHCN@Bf$`mw2 z-2<11(*?vPB6a4Ib_TVB#&6sUGe_ zr%akAen52#&fpi=EA&S%z)0{y7~6zvfw`zEkm=zoESR+Xrg}C|9)>WX>Gd0FjIDvK zE!1*4-NIh>(T&RF(xV#Pf;}C~n#_zBrby^j-iY2$rS0euafDYe7nnVO^2y1nk%lgU z-EFZUWqLa;Th;rIQdy*15t&UaBCE&%N>(B~#EK|QG)lyHEB@{`BKfZ67-s|)AF}~=m}CS>JJHf&p-VQgBY}Og_TWH3t7#899-&41`QLG$f0O&L z=+FXxW5?{tk35?aMTdSHASv{NqhZb_4UQgzzGoX z*@EXIV0g&|oRPayLB^y^8wRlj`Tm&)pjI0I$l#A!@|U;m`i|{J_n+j*B5uypfss&QTo+g*X z!wEq;+!MD_ z-tCl-0!xYhE?55xx{=}bDZ@C)G9X!^QWl$bkw`8w`u$(~8mN3aWJr-Q$Krot`=XmebzG5 z{oNcmhIsR*d!T9Z<|Dt`lPo&4=v^v0v{Z2D{nPj5sj&AXOdjz4KjW-DNVB*?@6EiK zymC$7cB@SM_IATtRVq*OU1>pwV&Wzv8;?ikCDjIU1J>ihp~* z98EG?!rS}Gx_nt_LP?FvB1RBU#PZOvG889N5d~l@wZmv_#;iqoX4qid)j=_RP-zc@ zFV{qpW;B$F{jV9r$(YbuYCc3BC0fqLXt|$(mZRyFaj1D2E$7O3h@WPWNI{=z7Hk(y zXdknHE;?0Z>NbfG0RpdY?E8q^mh{|4ijRWiPsyR-y@t^wyO)e3ohmZqt;ooTpMoJA zjUngbj=kmOpBYMNP9l0cg61{~YB8rNH#m$5rO`Mc&b8(sjd0bqs$r1!zj3n!JrS=6rKVZbehTo%NAjerk&LM z3XQ50QAE#-f|?aG$QeedVY_e{49lsZkB{4~p@?bI+BRHfBwKpx$|sn7^vkL_L6oa> zED@+gqSNTJSKcBAomb+_(idl;*crJ8zY+=~n@-=Ur0ai`bfm#L)15Jg+Z6Ad9l+~- zfBW^~e{=ZX#lCe=+=-t(h&lT%E&diQL`Y(+&1IRY zsD21lf!IN@ZC{Y2K$$M0CrF$H8Dk??T%Yo6NqDv-JuTCYW$*fwcSpiYQG>iY5Xo*j zm-cIVHc_#4y5JK>Ufk`UL%7+Nc>cB}N9!jg6^~Df)YPYHwkB$}rfPO4YIegj^fMP% z?awgeghl3j-F!>Dd~?zrc&F&E%6?e3xaCgWotAjpiDZkxoD*gx?&^5$o(~K5!1}|K z&af}Jsy?eXr872U(iso&3iue`N$x~$T-NgQwk+RT? z_nep2%?2gM<$gJXl#6=%$HlNPF%T8sLwyk<(v;OCucdNg^zdrN*wTu%@@BJTv}X1R zT0vh!^&rS|xRTPFM`n)1tG2@DWZJpnDY?0OW_P?Yko0Vsc7P5pEx$Q9GdTOqyd&vt z!GdxZey4*hpjKS@uN=O9_{Ooh;<-rDwF#EWC7LxCsO*wy7n_$&_Ff$D#6hdF?OhXO zYwY;=ARAzMNU`f!0uHg0t||GH#DY!Mp~dRo{|`?s{`o=i6mB-HlN|kQztjGHDW{*j z5hq4MxOVkZ5VH|7BEhfh{(d1m(BCf-2P+y&(h zl9wp?14?En`InTuL&+i~cPRPaDUm4odrJO}l7FIvj7CHsCAE|^P_mJdc1jLY@)RYc zHD?3GzOqO_lqE#KMgBdKZ*jk|=JAeu)tmW^_XaHueEGdo7BAmrxwqBM@3`mU`M^DI z9)IxO1}EQg&+FoM->b~y%kEX@^CkEEPQHqsvY+02{z6=B$mM%2cPqIwmS$!$u;*qrnkOr1KLIF^d}aQj$+u z7`9~}CH+G^aO{(3-U0DBWX69kLP~Mi>{%?o$;!6a?-z5HoL_Odzv7C1&3S*#wcz^K lT;;F1>R)krzv3!>#g+bwbN`BS{X^DZo~7g-M=2YJ{|}n;a*hB1 diff --git a/Robotics_API/__pycache__/Bestman_Real_Flexiv.cpython-38.pyc b/Robotics_API/__pycache__/Bestman_Real_Flexiv.cpython-38.pyc deleted file mode 100644 index 6b45f9e3abfbcf56e19a11d9915742d44bca6b9d..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 28893 zcmdsgdvILWdEefB@8U@ie2SK@C7C2-3Zy7eau8V}O_4HX5u`{+vR78ui#-?Mg1|0# z_ktvD7ECJ2vHFngbX>QIBNuejG)z57nx>82x=rk+nLgac%{ZCPoph#cI^(eIbkdnl z|6pCSzu$N6b8$h^>2xL&U~um1+;h%7=lg!&<9z4fm7$?(20vfE(3pSdFJ&@+$cy+- z4i`u9*L@d>%G5H-P}xPJX2>yH%gQlV%d4EqFBX;xwSvJsg~j4hsaBTr>|&)>!M);Q zb!nhBAoog(gG)oTA^-VsZCJ_~sg25UOKpoB$7*A8+*;fELPnKU<*kgWT+i0VRrY*l zqUzqNK5QO6*;=%w8%vg{f2PcjT3u(U(XOAf8jJOli`LcF<;mq$W}Rt;*_e6CJbdWz zlaD=q__0Hen~xu!dh)5MBZryq*ot$ZWBUa%-zOVw-aoO_Xf1}faeq~{+Vh8x9G%7C zLSxD4v?ndK!mQ6*-KO1Ic3PdbuIiXM+vv8MW^=L8?V58aUN_COJF^`J4;uCoUwoz8 zm=C^1+Bn^5Th&|W{>=0(1L=g}< z!Lh1F)fOBF)R@|e+i)CG+tm&nht*EC3&#<)TRni|sM@36hvOFYpn5-!W9pRp zfO_aHqqbGOs6MFn;%r=fTa04UPU7q#^@92s-h5b@Ag`Cbttph$_-;0nsd7k<`9tY+%vRg6?72qMGOx6p3#N0y zN*UbAs(#IJ?AGjxV|AzGnnei-Oqnx%@0p$11*_?x_?FXZEVf)r=?a$Z&gGWE)v2>) zLn+(pcCGGYSlm1aZ`rbSf&N8M774G}X*+gjaS=o-ZDsZQx~A1urZqQ*8asAW*qm)$ zTCv*ARb8C_t{L7IQsx)SM?~)}cIKzhM>xz|c2GO|3OCFpG#FH_*+5ydEA#VgO9Ks9 z>fmELHZG&0mO9E}S++H2*=UMoqMTSKckr>o26$V=J3^XN?}A6r+Z-@_IG6 zmqVJzlfsP*&WcWEp@d@@cPetH>I^Ilt`A8%?2OznZWwEMxf<=|70OyG$kmo!;fB$* zoiUZ`6~2^tFMHj1({Q$~kMllC+@|vL5GnX8j%Rjcc(o(5R_qmfnT2ij?W&~8Z=nSX z+ZJ{_pSjLAb~?Loz4JQWxR|rQ>+DvQ;Levas`_4zH6TZE{Q)&_!+;nX#9Mom0lpt< z><2uC%ycSbzOHn49JPgFZEF!Uqu8CxfNBh-&*|jsWA=QE&v{2vQES=U-*xN>JxUQ9 z2c#FTB_bHUzu0L)tR>%w+=N3B+2dz4;A>!U_ zKJD(EoIH380OTB;6IolgTW+WQWIg1M?&ONC=G<)|2bk@SgC?%D)s%U1<`#*qNy!%c zWN||3JCDEaB$9cI?i+j83@6hw9OHV{&i9Oktjb)>cK6DeaZ&F5RQ6IHBbcp~1J0>c zBEHFa)mFFF28#ni>>|poRfU!6&g!ytGwWq157i2C=Vrzp#Pi$F9K6t3vJS2`+6O5M zPD88EBXjVamfGF=v9qV-*+z}St3E@9CS5#xil0JNtC-CiRim1z{vY>Hig=l<*NOJ^ z`s6b*sNH9A5)|S-a^mVT;2FgBbbt{W3l5l&D7s5c_rRoi{DRfIn1mgnRUdJH{1?0R zrAGJSr2ABy)HZ-xccs|`SkJ92F0RJq(4;%!!$%#UGU===O}x*mXaca+#aco7$14c6 zc_m*6Ax9uhyxdEjc`t{TYx&baZ7)ykt_{e0=LNC7L3!K9Z!g>FdU-;bS5hsz4UNw$ zHDFYXOZ;L309p@y8?SyT2Y@38t8zwB*NtW`SH=t`0AosRx z=q0ZpJ?0H15F4$x+Fte5#^Q=4WqbKnd#>Y^@ljIjM9Ir{t;IR7tc!0g^OLAf++WR1}~`NCk%-E%j3OE4g{eiEy||Nq2^`zruTh=3azdj@CO^_&8j8wB0V zdj3Yn$T)@dA|O4u$DALNoF$dx-PZuy=b1mdURHp0d6(zK@Er&Zlv7ou>&Ek$Hx8^3 zpfl?ODCrCSdu8U8vMT3eHD0pic}`WMn(`TE@M6{e??z@VYh+wga+K77Gt`54p^(ve zKbiRo)ZX(D0BhN6*-vCXnQ0?cgCYnn-c1n5n3}VzPy?+76&7=@v9joNh0&~bV^;L5 z78Hn3mdspeLFZX%L+-;Yu-e@fsBV!EH}N#oWg3epJ5muJ2U7?2G}^1C5O`E+1n7q+ z%@aZ8DQejhljds;sFP^!O51@o6Xr2zEwD7&6Kg`O?dL3K#cuZrv)N8(5hBc9vCLLX zXX1x!h8zdXla0kLR2o!lUukt?`Vg>)J!p2C!WPOzC+S}9TJ~kjK43N+$6A6>h1Np_ zl#ZXAoU|!0+=unc%|@HzW!BPo>#aEE?OX4?v?)d+RbRb-dSo6iH?$TAlUFb z2uK>t;fjB_DjS1Y+{=y{+YR@TG!W`9H~~f+T@QIC zLjuvGF(7(Z$|)-!h***UUxi?DsqME%V2IBq2HZD7<`?!QI;;jj0rHNxPm{P09(MTU(xd%2lfz7&VDcoBBTPtw_ESurX7XVq6Ir1#`$u_$id9Uk4G2Zo=g9eQ<5AZ{ zlF7%(DK7+61rhngm37~rMnM0*Cph~p+&f7U%8j?^p5iWZ{` zPGA`NI3$V*L4VG2nim9zE?7&El_%Mv@tFp9k(X=3^OhqZp;a4uJ8s@Ibg%Sv$kr99 z-e)l}NW1IE-sKo(^6tL0PfvMg^DZz^PSgbp!W43y*HT@)YdQB=*deDomlJXdt}4MV z2s@v3&HYrAC(LCyrlQYxiyySxcLI(=&9)T<}z zCq8lF_$xD~&P>-vt*cfOcJEocwFKw}^dCk3e#$A$cj~P5=kcs7)@y#$a34tP9zSQ3 zR1o}75e$`_FW|y`74}7D0j|yK+0X4?gMGW6?Lk$5cMJ-TlXnVG9Y8U-lY1|()rTXQ zmow)wpWu7N^^$^Jf~Uv_1;fwL{xDRLGE|Xwp^D_8itLuW6;(ue`M4@_B%?}e`D^)5 z2`K?V%B~j)7`h~AO=7tOUnWtPQwPRaUjrh@JB}4$3|O}0z|(V8h$hI zg zimwhwd3j}d(dr&BuRueyOsmm^OOv-S0OCK>$hZX7Us_obqFZWQmGjHAg~a>N&|a#D zZYGLaf{2EazjV5B)k6wfEzjz`oZDLVRgqd1#noqNvUPFZYWwH?gE9&_*UmFP!*8J{ zyR_>wr9tQ&>AFV$OB)7;?g)_-vp8`e9j-%0I3g_;#8dv6>@IkjV4@uT6ow0f1k^azEd~u+l6_c}n!Sv`6JIs71FO4k`c-{v#1QdPM0GrR@ovg#(ZNKPV0H z@rB>^K1NvP5Wu+deyM>on@$KxC2}8C3KT6y2Dpq@CiQZPrem}a$ z{s0m$Ph&jdw?LybS{&Q<#r|>IDn-@NY(09X@M*t;ax_zw%CK06?+oU48|k7hj2Vm^$-Mh! zIFewr7<)pwg9G0@^?YLN?q}qnp=%$ZUj({;78XUE1avXs=rn@JC zQ;G6Iltt|5d9wg!w^qrj9S|07BRXDvC+*#UT2;1CZn5Aj4pNg zg8S|!qpPfW+Y*Th_m-vL@ZbOZnUfPKgA^}dqcsf%(_>nBx5U9kz5fjbb*bQTWMSt; zW1Fsq>9-OCn=tS8qa+7l>tLE;0Zw}UrVlCxBEW!wvd6u+k zll$R7il^5xG;hR3WKaWVRcON{s)!BIu80{m;QB8DQ;YT)e62Q;WKwHygZ`xO)ujjUHa z)mGM3?T!|cnfhvgsY6b4xz1+PMXIK0qk|muAX^FS9fIp{#DJ+`5b+l`AL=&0dVc_~ z7;;Ru2p4j&)1Z}ueZe2>hse3~1t$l4K2wAb7~WPyZlsFNFK@!jfdL-qEAvoIgW(S) z*uBPDcv$AP5?q{G_I}K(+6~oew7U}#DY)B@`%}caA)@JG($<|wp@U?(K~~a)U@+m= zt|e$99e}+-&HTEAR3zTXA@%F?0YK;8PP&Moxg1f(fz8k$t(XLrP`3^N;ayV6iyNo} z;GnWpk=FnVFr5uK&pEkXwwJq}UCV<$D57&cs>yoBAnz++6wz;kju1m69(#)DJVbOM zZYGPz9@daD)(Y1Op|MQ+sIaMh6pRS{6C0>!L+nGG6gP_LvnYeIfX5)P!M2v+$_EDd zE}?|gi}F#gnB!tCA-n-OMVF5VZ2A`A)_vf(PiFCG3^AKQYw-33zT*Ij5*7LR0$DF& zz679POKlb(flxoe6}4qtCaj>bmRAm%>I7&Ip)TMl+J&9uFs#4N=Wf(-URvw(93 zqNu=dhU;|HK+m8t_F6_oTa$(;B9U~;YhR_d-X&KW4UB5z zOd0@{Wv=rhAqwR=@6GYA8V}N8k~$I4|N3-O%y~4IKSZ+u{E?EoFw`=iHv}epUvA*Y z3??K&62%2fCax8g@fWmkV9LE!?v)lQj2KzN5YHPuM3<~pdimvAuMC(f zEYJ2TYw+c%!o`Zc+{?BxS?T8jl*!$n?B!y2b50c{4&ZMPKv#8!aBcLebl!jrUxK$_ z0Dpt{8(JIg4fO`|_>_x={hi+MMWg%GHYO^aVO8o4sIvGHD!oCFYPC0nbf7nkbnv|j zBUdhcRmgpWB#bB=wDkrc6Zb|ztV5AD29lnFUAyIC_49`PDG>FDPt>F9V`|vh+8fzG z)FanM5=1?+@;9NOhjhJ(CzvX57oiEQxfZ6j5~5lHPdYN+1Pl1Gg$eMMvnn{)Kx_dV zZaxr(b!_xg#-*V4IKHlhIUJ7&v}|!EFHvb+hKoB&oLG`2;w@UtzV{E!N|bv5lX!Hs zG!`S*rr-1y)DLR?u@cb+nZAJIreo68l3EA8_MUabd*p|$=Jc^o)L%XE(&lx*<))o< zdJOLMA&NdaJ+mPgLiFc?0)N4@1@lxyjtJ;nV;s* z*PlyBiom+uO2{CjNhr}Ev{~cr%y>oz3`Is2j4lF9Er-FrY19gh#pMeP{g^QrHk}xC zLK-DdF_X=>N7Dq@66Q<9_u+IBI$2s!&pb-P&-l^l@98sudhtb?F{Yl^E}5dxneK?$ zL68rO6`9sGU$ie z%DA`P*q$4P_}dN_r+KGXC>mMNko?}s=eE;SPgTx0tJGdXeLmUzhv%9k%HXGhlTbza zC0w}6LG)aAT&qZLLJA_LG6$A}KuvJpzrf<%G81F6|7^ooeHL`>%byx2%f3o`F5 z*OgugatG4qMqVUOb$vh;d!<07_mwGz;QRvQPT7|`)%77&a)x{5KDkp~D_<*za;FSs zs?u0RkA^aV6t4Z^3B4HPo)kUQPGhjKQIyPg5IxCU=6=wkMRK4^A zuNrmAO)O4eG^M+TBZheLSWG#=i5(0>=FvV->d(ee^7BqljMyY*v0uE(3<}r5cH#vI zhRPeE^${~v`tfWx@$C4qb2BH-pE@>OKYfOXRzG>>-0>6MPQuf1=)s_ZM*B2YV|3al zJ2rwuLvLKaxl|FT@+u)YdBe$(_eOXf8#k|bg|X`w>?S`(F|n9LC?j)@f->8L;c5Cn zfP_`RD#>&o!aE4Jp)QS(8|u?#z)c7w0wd6_Q$2pEr6x3NB1VWB-^PXe?ncz`v7#tg z0X-U6Q9>+s30P6yfE5LM*2jw==#NMNjWs|Dc{QN;M_ ze&8n1SIG&1Cn7=q`vaO59XVnx%%z8JgFevl8zgh_k!I{q$H%|Z6@Tsl za|!25IA3z#Ag~cffoNcG&hp~BV#_V&Z1`N0x?hso|8(ITQ8W0FDeI`3WX8y6sRewEsLVYg-w{)3|z^ z;Q&qNHH~W#R=&ws`gFUh*6ptI(N8jYi^*q@)P|E|bUy&OCl$*4LH-hPaxY5GY|}XD zqhuCwWX4!Fj+GL+ks618w+a$rL&2l+VMbWe3am zWC;ymc^xhfI5(ySM$PKIysu6cN7CYv^7rq%y6=FwZxsocdY`+m4@9Mo2*`q^mAY;sq(3PTYsS5K-wv7D zGp@=^tY@rxhU-3YstpsZh0jJ~5w^)D2ig`q2$T%{?VWUY=zdft`8n2gZsjrge)QPw zO4v?!cZ33dSNQMIA2@2ykpn|U_l$E~o`Gc%6gjm4ImK(2t)>`pGFyoi7hQR-y~&?t z2@&6?vV1Mirg;VF-r8U=$3XF?Ih{R(1v~yq*4l8}x*}C*ExT6cwd79Il!o<()kRlo zF8?#KK21)lNVK>HMM>H&wioQLY?(o%xF;^ihth2KWPwd64B=0L)&CQ2yFcf%Ivvbp z^&H&Ga1<*8UcJ2zqJ?Cg85T1p`;p2^7Mc76eq8f#cK4Ub|TERP8h{|xa$ z{uw4e{4?rRh%ZE1UXWO0xvoNEf{g}CtnB;$$j-jr^q#-$LY&%UdX%6f?}9K-GR{x*=Au&PuN>#cF3qyChWB`Kha|3@ZtIaW_=r%J=N z8(7LjfasWAEc13mj$-{T;;V$Qv;!rmdQuD>-^l*s=iF_&7c>v*tYAC)FSCkRfQ`Iv zMdfLh`!sTS`DSN%RR>~_R3#ciJ5%~(?Ik|7ncRcN?9a2@@qo9aRdwwF^0xXe&iGCH z3JdKrahR+yxy&Su$42$H2#-lAe?%@jgzT9C;WF@4*2vz;Bm9=`effvLd>%dz2w&PByTF^5UPQjaI;;^9(cnc@_zY$M<~X4Vaq3^oUAb3w zDq5U!q0(9fZ)3$PtQOiXCC!8Ica&hAWc^9b5%A*w0xwo!zwD9xqpIv|!8}1|zr2a5 za!fJh*Q(d5p#{Sjq)Owj#lyA;hgVPSNR^al#TjsqiPWg+_hA>sYiwX`QQvZ2J|oW2z3c6|N&f$R)#H@mW#M&4PVS(v=it`;!*9!GT=>*^2{vH%`QX zJ+=n-H6w8tp?<@Nxv>#O5(Q=!5H)07waGvBQ6`~_K|I+HL%3;AHvEw8kETgO`Gzjt z5Y50plIFyowi_tk-Qg2tI345nrn{9G8;v9fe$qW4ejM64Bw`ATM32SaK)BSHM$Cu! z8H7zQ+g`2}T==27(HD!)xi$t$#A+!OTY_xAgI9c|219DMuG*htNfEFTVET=gfyg0U z+rl-l32%OFToXbNcjZTD(VgrK$fsxwu8oF$2bZ+e-DGRVVg`NiK-9$ugpaaO;u6}L zG7=*kKZeX@hO+P-YYZL({^su#fyGz~3e3f_(Cj#HSt2e|*N&u=oUm{h1Vs`$j=%2j zBXPed+W`O~*R!JZNL&yX_Y+1rwF=0TbT0c0z&gB_<298U%)UdRxlUWB;J~fCP~kNv zG8Hx-c_$C+h*nAwGxnFA0d3772cgxsRy4E%kg+uhBTRg79R|352jGgRD0h#%F`@>Y zQCL3x;F?`4T`Ps)%Kc=98ehLBxJEP_15y$ z7bZqQ{UH5vm9SXb4#Al-dbf3pu{7M4x|+?Fq_Vk;lGsP%~Jd8A4AeG&Xc zJrIthyM>*k-_XVBg{!wD$Z#VVMn&|!1~A+rcxJzgFRoQ!lKHUm*Lj_@RxOQ{6a-k4 zesOuVHirJQupvnd5^E1`xL4<__fpH18F_Cz;j6`^a6ZEM^JSv6M!?x;^_4Yd65+(l zT^kYo4#ViTvNqt41bW7*jr$A863A-T`E~?@q1f`aHUlw4nIxUrJwg~1f6p*h4#2n@ zH%3MM%o7rg?DvZ$dds;!esd5^DL0{q7SSRSn1J9>oCKC1R)$C*5%kIZ&?j@gP6;UV zqdN}w3W)9)V|0gJYj|l)FR*JM8Y6O%yz2KgNL_0taY}&?L+>r*J8A=6FP#wYw&Y7n z^=*TG$aqoM+b4ApT`>G{&P~IJyev@kL_V>&WpsNrSR0Q;Sp>i$g;Ni@nH=-$#T%I$#!mb};6=|NLiO{7l6DaK zdE#@g<@Ji*7Y9zhBCLvncG2}pzR0UWG5C9iRHR-yaR_7&)=h1ZY~AF;y^< zWX5#Xa;{kLVp2dt*vL#X7j{8?T=C~QBQX|v4BZcgb{dR;$zJ0~yeMXmALpBu8dZbA)54D&CNc&Tkj zh}n~Aq&WxEzV2_UlxC6Jj79nrwss(B?JMU$s-@~LqOgAXI*fZ!L*YTdb|RMT4$auB zlnCtXDYKRNmXWdl3bJxnuGz+l;@VNK#0A*U<8M)@P8bq$*7~CyDghT|ZYdv9WcNgeJsy-5rFx->|Fy$}g@+>ing$1Rw;HEgc} zeHS}86`G5jjNmAGMNNIR;(4J$FT1oe)=ga&6fZDAGZ>M` z^#2MjT;=P%z%lz(2MdER6=}SK6`0r$P%rGr;+R{YrjqlGbS?-g-9X5;pF^TKn2VE> zMy8Fu_pl_SqPK@OjZ0NUk4~9TVcKaP^J}9}5u&1{wKU{Gc&v zkjM&g-iWT#5=cQtZcAyeM%>QbR5@f^i6z#Z1_|16K3xUWXWBo<-r07HOATQ@fdK;M zoSbx@h{<8yqR?K}QT zAX`jy|9xE623lRet(K~lgTBx^xQJ|@Laty<+Mr)6BsSghUayyF1 zAnsq2S|&Pv4a(>^$JpyyW6K??B&V;LiAhMSMH$hF5PB# zAnGfh{hkcorQ{A}I(AN@P1;9Y`zNp)tKD@b<143GXTZ4Bk4wTkwfuZ%rW4~JSr!v< ziQZMjE6AYOU&A-qUuTlWC?PA&cbv{Q$tB#7C{xKo9p$R1?c%xxhol0vKeC7kbUF#+ zlw1;W#v(4W`Wyj&EqH-bQ-~K4UWi9~r1y z(vN-f%eyZ^-n2MTVs7T_e~592$idysegR1b>=}H#r9N^yM#~@3q9nGSpFAAA1**JR zwEq!H-pmDe(_gD~4?<+A0bKtc2~qqc*wbcIM@OU*VEm;&RRoCH)QA9&6Y0_^(BHJ? z&pHKCblTq_4(yiBNToXaA2XLIF1unH3Og@GZuUQ6uHRsiG*!GIkthKT{b33XeIQlL zvx4BQ@BP^LFTL;|-M5~*Md99L68z9MAg)mf?b18JS*IJEF_L4iuoLF*#f<$Ip^{=A zN@RX+LFSI2fEL!or3Q1H8%1LWCB4wk(aZnv-wLq&ugrVb4f`7I8dwxqkh`VuZYJ2D zQ~~9 z&H7XV`GmT20kcCFI@oD2Tm!+PAc;FyWVx&U(kL6me{4}*vivyI9-72|Yd|bs629eQ z?_H^u8uzO7AXfgTRoTBZ!?$7s^@raVh&Vo?^1|IT#Z~GLIj6JOb^dOf$$Db@ghoF6 zRM$q*<(@`ZlrEet(1!M1T<#kTKJuWrH1^;`TpA3Nk&UjXP3#2E!~zbm;v-e;3&ohb_s{~g-iP` z8m|HnE@c(I7C#VZ={b-b)#jnz4FXFB=yy0``M?1R_D3c@2MZI)j=Vf zH<|$*Xf(n(bWl^W;r2hrWBa#|1Wl(%hfzAxN9|G5?SF|JQKR9Nx&5GC&=SL(#HP11 z^WVSuKYsW0?5Cf*JvyU_MZ5K3gPxoi;1=H9zAY^94@55Mg?~Sbn!OUX zypx6JPx1<`7X0%E8~YK+G|peeJ^NoU`BzN7$}D;Tx-R((xKZniV$&doEp0jH&<(~h zDn!O>q>plTbJR?fmKle;7xpK9;Q%h*DdrwRpjtVH5DDy)UERbE?JMnjUwiN~|Mlgs zeR<;oJ34@ms1F6Iz5DfG7xPSCXnQsu4VsPCY2z|dRYXWegS0;YMWMM zR*n%9v~VvS#*^WM%xa;RLJ9KP|4*5PO(kf>Sz&E`@`}%~T8=Bju_`&{*4;RL$M)Z1^7BmoHk0o#Aq?C97Kx9+wUNHcYdZtN?I$?F=h$ML-$f}1 zWz*Z!$o5lYY5NygL!V(V7$PX*@;^#2egwG)#$2vA44<@saotPrXgdj|J3rz#gx?}c?XN=-dI@o@qOOa{Q zIM;z_e}nm|j5M2+XgmF1Rw{A>0^p+jw;lUenQKV)4Q?!A;AU64_TOU}|A5H|6G1qB ie_F?Q| diff --git a/Robotics_API/__pycache__/Bestman_flexiv.cpython-38.pyc b/Robotics_API/__pycache__/Bestman_flexiv.cpython-38.pyc deleted file mode 100644 index 25f83aebe426009cabef57b1ede516a032bd86b4..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 18731 zcmeHPTWlQHd7hb_-JM-tL{XG=u`Eq&#p2o&CEwyic4J8tWh=2Lg_3M%9CtI?GbESV zo6Za+a!R^weL_zPRX9-|`j&E&5Uvedt3wD2f7oNCgyVfxZO+-0%O- z%ow80aa*dJZs58oSvoY2jcXn}I zZ%j0IJG1JS;Tt;9Vnr z|B!PC?-v@on~yk;=-SJgC<^NxO;~pf=djSPXqD1E%t>3Q=uv)d#jUqcIb3hu@}%!o zZ`4~}(+%piYP%EEoAnU&HubO;+X2_ZL$k9dF0}l>Z8T0SHN5rut*R6^PBiM*tDOz> z+|_XjQYWaQPvH4A`dap9;^s8|d`;IhkMRne>cV(O15R_o#MKmeQNT5?M(-3vNtAJ4 z6eD63S4)hEaa>Dcmzcn{EOv`YTt~znF@@`>*ef2wbxiCN`*9r?2gE^KcZoCNka*-B z-I)+)#bI#-wcTP)JSrYTZBm>QkBeid?Gf{0T2xS*5>JRH@#bDJBaY+xkeC%GaNQ?P zil=bhFP;|9;CetjE1tvkp!kG%9@mG(C&j05JtWSH7sQKz^oaNk@oBUi7B7j@xE>J? zV>Mom_MG+npy{@%i=Nx4o@bQS2uaD0h>`B+@aMNs1lqQ~sc#y0jZLizazvTi%%L># zB!8Fg3xT#;#MMGeiCfCS$m;0!STHX1yT+Co?Ao5#H1F!@vs)M<_YU4#-Mu<_PP;>G zdxVK{N@y5jej$7ui+;xUy&zrkdf09~+oczOc1?<<8dfy@8K}|Fui(#r8pSd`@zxVt zdZ2CUfxc?2=I$8M+|*aWK{pKl39jikxa}LpO%tCs97}q)+Ksiqb4ndq_n9~=`KVm? z>qLaDnirY%8=Z~FY}8vf9P8TRx%015t-RWJfbKsX<$)tS=KupPojtPZmxN)+J=*RwY?nW@UmlGYzv^IiP9P2_pUG-Mx*?8 z%#@MQ_9L_71}l+CScyhzYf^f^m#9lgE9ZyQ%Cy|vPYA^2m{U|hJ zTrc2XS-12u>Z5e`p=E}L?uX|ATFe0wv(R6n0%8Xf-PPn})Qs&Mm_pZ8t9ciKDadaZ zgpsyU&vV?fD00+FjKTMo02A?>#w`*4$RB9oW}B_1qIPHe`1|@1SdF$MAc>+I2ta9!}1;-0N8O<@m#R zOA@=^SI7FUhRe*thm83 zlwSPKQx$u~_3c*MUZSbb*iTnzIM>_lz!NrDyKSFYLqU`t?q4Lw5g!vVD}E#V*tL!zoy=&CIXABRn>2B17KUc|tbebSJdR9LV>k;#*-rSLDJ2 z7u&Zy+m%gX1Cq9Nw_$hMzBd~dUIFz@R~9Pym>!_ZNK(ovfOhsREMBc%Uc7Me!qp3} z%~$8&n4i0L^}?kEXTn?eYEV9xC5#~wb#ey%`w>uFZdXZ}T);D*HMcRLhYt^%cHFZ@ zYnA-y4I)bxP7)O;NNrUY`W@rzU)X|X-!?Y&@4}eegq}X2#Wgdn<g}- zq`J(|Zon!r6$_4f)WLc=bDj3N`f-ja`?CCcBH;Esx^c&3zoA}VL#o1|v?Q9D8^ zC033L02QNmTp!LJnch9rL?fg>M)4DZY_%LvRMPo~u!hXU*QtblhcV>`Q7)`^(nSM2HB+aA&&1Uc9-X@BU%Ck*9DZI^ z9bU&*d<%tE9EAYH8Xg_WB7LvbSd7qX_|YN~k?}%)6BVdXG8xYY;Ey{x6gSJfRdyu4 zY#e|!dro`n@E43NV=IR~`Aq}-Q`pR{n%~vlH|dR=M_aD|N)b@Lh8|WA6jS6n2EzY*~)fkoU)x9;DkrcKziQi zHCT3e*x{`pDtWE7CMk1I9>hAxhfzc(4KXGed4^i_x-8LSvbj~kR^DC)5|anv1J?0U zTcDFq`dTZJB0QuYL`m1MGK^A&!sy|9Qj0`BOfEEpF5fd}-%jz?%h?ys{mlFfoSd?+ zf?;8iLGLVp4F^R6F+TlbEvp!>l)f=kyOfEKur}4Y2rvVsRg2-_VHoLl-5Cv}@iw(> zjm_McTkcR`Ko7sY13HAaEIk-+OEz2%Pqdr%^tnstD?=uj1u!V{QXCBeybrESc^pHA zdjoYsW=N#p9EzwS%^u@=o3KQJf~aT*HLQ9LASu-mWI_cHCIMV*_^CSmV-$TqJHw1F zsJBGD200tggIUN2lmFk8LIRbfH<`vef>mS%!ZPIx7&?4Kx3%m^GQ;E{Du&rDV_gYd z7wdn7ZV7RHY$xL40e}+M2T+%?**M@3n~?=`016gpGs5?FK-<;1%l0%m+V&iDo+s_g zHz^Km;=&? zADI>9dBURdg`!bK93VW5tY6^oCnUySip9R__uhM<~!hXE8H^KB1G+*?}mYC&6OH1^dMIG{^wEd)hKsbB9J zW&i~D0ga^o+xQZUwnXz2U+(}AN@;KjX;7zF8oaqaV{gpZ;S6kt2Km$Vptgc0_x4*h zn|(a?S|^<)q(i!NP7zdk8L*-;f>;gORc{Rjc;eZb)Y`v_k7QR7gRUl6AP>R&0613$ zUYXQ(*$AKdsKp$Bu~1`L03`YmlhPkA={u;9U_tN`VkPkcw@nqVL0e(92&=sSH%Y%? z$xjGlS>G&7XIixjI_uhX^tj#sNM)oZW=5U*KEjVWS^88X-XI^0gO?mH_RKEjL{0va^=vMiPL z7KG>5CE_4G!eAQeM!KTzw)_ek{Ol*FF0r719nm%aiKAl&?3UL!WnV(fzuj^hvv$TU zVF%F0^L7m+9D5R7z(b+Awc355;kgG=V-`@Z$xZ+-o_VAC+Wf1-p`|X)gAI;gotc}v zKdc8E+QN@Pi~Nf#|NT;2`L~xUCFNAj;yH&GqDcxbeCE=H1q2x`T|tcD(t_ecR?lS8 zupEtc!|xNHAq5D&WO-33s))<0=#x6jSc|ld;YP(6NveVwMmbnP{j9sciQov^k>KSF0uk zBp|G@bpZ#Co)lN%hHVwW$p*OeG;?V&=45JFOk0SPsYNi~QXk@EYPltJa5A;PqlXiC zcFW>#A~GxA3(6vYNB(W%tN_D#osld-YI@#y1zY4z4zID6k~Q{^+`cyUc>w9@*Z1fxsBa0 zGMemZ2zw6^9f}ARD)M&fdR-ui4Qu-Ld#W-ZBdL=r4W7a~p<3pWwojUu%3c z;JDikke`}8*@ev9C7;7eE>wz9ehoWI*l$!*gaRW3^|48GkkstlnZ>K~S1z1cs9wB8 zineU0rTi0_IKN@^KDu45CSFsh#nfgI?;G9+M->3(~SzZv2R*i zi4G12P{j^yKMx&dBJb&sDWw$gQ;bKM(Iwg>!dAl5!_4UW17R5z+ayO%*NT9d{750E zymAgVDTg3qppQeyC@#raZU?6*MU6hiO(A|f}EvG1%;7iYJuAjR;M|&o<;B{tPS;G^&Nm4)xDoCeGkBTJ} z&Uo)koc**xgX|NeIuHR=gB9Gbv>QUM&^vwl^5ges(-=L-5K~zstT=OIg@>Q|*_l3u z)}tAT^XZQ@<2ou`%_vA0k}*!2ai}q#X=f9d4e6(2EHWX4)T4u|ld(tmmFH=3 z2z^>H{KSJTML&q1oClIfv;ZuXNX$|KC?$V>FNbY+eKU8XC=bzoEz1_P93aP@Q??+j zV6o@51KOtXJd$2s)B>{VA!%4HncF1I5DJWK6?5BC?z<>J1{K5qOxK!IT0M@W!WQe= z*!?ke`}&6MCUHH)+&FGcjckqH*W`ym?I%07XQ&PXT2dx0rKwQffIWR|{n(6sYy$=D z<%Y*9-328zsy?QS^*$$o0YNp!eY(NFE(G~BP%EFI;#m~P%Fx#%eIwGt@Gy_3x#n|H zSpO)H2vmy7uTk#6JS9WPO9bPKRJc_1YFFpT&Zc+u?3pX`O3B93!z8Rkd7hawPB+;6 z72bNa>2{oQ_ud&zLjGj!J66lPt?d3j6}wMiwAyP>*E*+(W@#f&vnZ%#!S^JQQl{jb zCx_AS4sd!z2t~9~b-IZgwaI`OvqAZUlrti9CQdyePLLyFT!?2OQiX_=NfD{ER^Uh$ zwIQ=GeoAqwJRDPwgT-W=-!8%2&O^Z#H!Y4tQ+{kd{EJ^4Idf3({^U$sJF(~Za}XnW zbIPyd07|7SFQO$fYwga4+Nw-rp?w6VLLe00h~pVjqV7y31d@$sI(yhJ`Rl~!+2+Twj_GrQXMH0^VOLc9AIJs%+tgSOq$QDh(w-E-qyD+1S(bd{uN4IQjK z4XMSt6I1Ki^c$8pL86|1;XE0!$gH`Jz3_DRD9+&$ZNz;jix+7_lmOP#Q>9`nWcma9 zqC{VmW9ZXOVd3fS8Y zUl_&&jCr5-7i&rUWmpdUg3?uhr*-oSVXn?o1E(1Gl^A2 zUa!a;N{IADXN2?EfiYQMcZ#l$fQ%aeF8h10(u=#7)*6kTIHog|O>8IaM7bUEEcI3? zZ&h9rXCj>vlox#=vXWJICNoxA@>G{jnZEDI78G|hlIfxZ(F{HzIncUi2;SO+ZCXKX zWD-%-Nqq{sY&4!-1}f8Rhwahcg=M zP!3r+dyx<MoLe8{L;a==!I}@47RVfdj8TTH=7opoJ*5H(x``A!+N?<<4CGs{s z%Xu4dh7u>?&}+`*z)3V;l3q+ULN`#CzePQFCFXAo(;10}jHUCOsW|<+hpi>Cd%wcp z(`8|ZD8(q>B0k%X&f3TX_-aa@FeY*Z-NX?e{QGI4I6S9)2&5h^qlu7wo5LRqP33;N z&t)lp&TZ5XONWO*@;yd}ogU1CJ8|K&6`M~e@E$)lY)GvV5p?Rc8^@t+j&~X^!i_jk zRPPWd@)1CLF6E@qZlb+L;Vau6s{aOAB?(Lq@m}dscM~E}t7;+QkM%MP!^)6NuQSZx zGEaGegoYg;#vX4@ZE=nD4AhzE|5pFXPvSt5LUr}RXRD0SzoLa_H`fXuAEwL(U+Gb` zM8hM=3R6%|ooj?6iyT5S&R}lgoMZQXAD;q(srD~hM)>_eR|(4T&NFgcumcWhDXe$l z76;h2f_G8EwIIr9G0}g75}H;2O>?^_M(Jdv7+cQKiBPmybV5?>zMCU6P)r`s#Gcc% z`(6;^NO=AsAWUT-d%6VVt)i}l`|9#ur@eS?scj zn>dpe^d%wL$%v9FABs7HTJ1VAQpt_Zi29+DG|dcDskRI<&ZWdVI;W(^Q0Q%Zx6~uLbwWr>^LnD-(_( zIIk9f5Apm5WTrZ1CeW$=4{*IRF^$L2cJD(J;l7vEp-4pzL`Lef+UF&MrRBQkC8^ z+zcF?cE+XWitatMXoEH=DTEqtEjW1^knbR^P6kd64{@m3y9K+_DRtpYLU}FHp-w-T zudTEfz2#>jePI7^G`r+KBnml-4{61th;QH!Dqr+#8RHlx@PGIBPk1t>vvFOOVbnvep41 zhEod|Tuj4i)5{aH2zGoGSdS(;8+;b8e)DD3#ToCtm%GtTWq$l2YR*XAPw*iei=7#A zI+z|~Z8;i$4Iwod$UmU>d)Yd|p=4&oM7aDbQ5p$1&9vAS8rP@HGE>-u{-_Z?HjK9V zV8}1y^`8AH5^m(3&f$-bM}k5$d63FgKIKjUwRdwmtv%Agc_BRCz%?bey%TPB2glA zJqfiIpZQV~pmTHSaAy(#{k>Sibf?uNQi7={I3Da3CgDSPM!e{@X}nPJP$g2A?6lV-!UNqu6;Q^cMOjw(f0|BPa|=*F8zQH!V*(C z=5qULn^`M9*A(TM6y(?ORK7<=AK#>$wcHNc|3geeuC+E^fV)0i04Jas{Jt)T?*kEn~KB< zM8`gj40|$D)*;KZ@>a_D9U>C=K&_7cKK>UCocEi>Mz#uYOVVf@{fPD|EuH>XBD0*v zCclUdQO@7+C23Q+k8X?jk2h!`qoRAAK(5f5(~0P6RUV*T6pnCqss9v&{>PmvcPilM tuLu3Ti@J~+Wy}miePq3;2OL# zSq zgLjO${UPTN+80{8+YdMo=-LaKC<^PgCahbAb6Dt?wMywO#-uG&^eBI3xz_9;bGX^L z;Y#1FUTb#Tb}eYut6n!~wwodH9@VfGy@2!Kp}D!^=R1B-YqgFyTJBo&MpcSy$6L*- z)$TfK?&{VEQa7lgPT=}=`daof@o)-1U(+?sWxN8Xx-f2Qz-dmHxSJv`3b^N0@124u zi87vxVoZ$VZixvoiF--x5>vRB#cnZ;`N!Sfosg7VM2z%&20y=pB+$0>4SmD7V{B+ukR#IE zMh>ZoH~BmCTnMz4BJLJSN?cM7##Y9+CW1+!-!V4LVAs~vhIvOvo!!C^x!Y*9vU_Fv ztagja_6QT{jL^`<{6hFJCjGSUyFohT&Cu&S(W4iCZdHm#9W$C<26EK(%lP?^BU!>H zUVCIy53~(E&{vF=+$}?z8~O@3=$hd_!a4mKm;JhN-NdI2$CB<1ueBPuPN^%KJ`-mp zAC;SalZdcWcO$cTt-BtXt!C$%W4*k1_S{R9E3bIXPEZZjyY4$iWXzp#@?7$cCdbj4 z@1H!r?6uwF>$T2tnt%(K0EPSGi{4caEZ|@9yw>yH8o#FbQPe(%Z*i5k+qzUp@B>rw6{Zz;;5ab#Zb1kll@bd1|wW)_UbxpmBt zk>UA~*{uc3kx3Yd#_Fq5y1&WT69G8IxoBZmSVls!_8`?=b3Wvn%%y^im3#XF??tcoSI(qW0p+-;YEyCiMdT z%DSbOksqh0cP%qKv=g3(=r9LJ%s$^j280dwV+-Mj&ec|1f$i0Ko`mgnY(}l^yAoo1)(+%)v$JFeo?UObHEGYy z&E2JW3?EW0>$OhD3+$_|dhd!^8*)3KHmDlfG5mh8cGFL)htu<&+Eq;ZQv6}GlEmtF zIU!XVu3ZkAt(JYWrrt+-@|{^e^^0Uoqr^faFa2 z7*|L5RT{J_28nr$KT+ioR8Zf47>Q<-LBnPK8`n*JJcp;8$JTTc2@i7sz(l^5Qz1$U~j~UG|$0u~eQetkQl2ZtqTyg_OsuCaa0ARkO z^PmR@xo4H3Ru!)+jX#g-KGC0MghxjWWJH_o2V?_IZ$K-|f&6SLz7+=aL@qpV!Moww zHQ6RMAZgpIwd}6vyK`aTMNr?d%0eX{(*yJvNlLj7prd^Yi&v_b7SCTef93ou^VRuJ z%%6Gr%K3{6(UiO9)}eSVNf<&T>f{rsKZt0%O z5-hK3V$6g|aVw{5ovFt(f9(VW7XAGK39d09bUZvFLETc=#2lE{cGrfEvV)ogaa4iW zNUwcgw^;Kz63{gTY%c9brNA5_j{_r)dHq6djZ)}*7*~D}<-%q+o;0Tfcxt4`Hal^4 zjC-dZ2B&U{2#4QSRfSja72iUl6~`eIF^3P0WSfCzbrvnu3>Qrz5gAY9Zy*D;N+#sF z06cR`hcah5xWWsI&l?9|-JaE6JNzkQ)7Z?RPJY7xPZc(DE9O_Vw@qqs{h`i_fKmjM zFQJCj2gMY*6v~5HD8k%F+}RIlMG(Q1!Z5`!gh?k*lcOe*n2HQ4$Sh0=nYU2ypq-kM z%^9jWEtdvJrc;AfJ?00pa?(C}73%b;jafYkVmq3CFf!>yXg^QdcKc7BTwo>+^JF{Q zp>fi7a)1*aISq-rLo--=dD!KhAS$_?)ix=3S02VZ$RkK1le!p_jC_eo^rkG)YqG&r z!j|7&1`?A8;R6V1~50eXZp~v^k**8g<5vp+OH11Bf#D_~eyY0yOrV8daNK#WgUta%mVmC_a?HBOoM2y;_yiU1=}nzk4o z9)*!!)$P$R!)Vyj*z}FLmS%!ZPJKbRFx*T4%|X z@O5d@reu^YGtraKO|kYmswKqrzU_#MI{->t8$w;mW|M$JY(`ei0Vr6Y%?RJz25nc) zT(XbR!rMLro##sX60PDZN<9_0c|kHN79^-FWQQRnh`L)uH^GLf>|${>5>4ZT zrl8)f63E}qV9DMNOG)KEBoTsABxRWp$bxr-R)|btU(XIQLt%*xpGfB=G8cvim~B0b zq9LmTM0JmV6U+f=#E;C1^5+SQYJC)qE8+m*p=W~ve?K8H{!%RVRmHCnLP@17nIFz> zhtyGxhY*|t92WL8uN#_@aw&qft=t9{|0$~fJBzFlsHXMA5Q>lbwhJZhHX3d{@MK0~ zUs=YYw6WTOqlA?D)t+GnKv+_skkr42FTrR_G(Yk6HUOcNhNqA^b&{pQC)Z}}^;tWd zh3(LyHFh(oFQce-^EI2zKJI(Hn~oCFA)UIY2r9hP!oOL>DnB{dh`mBSV4(&QOSzWM#Nz zvLef)tgupq)n35TNnh3EQ^Hu%HwrV_rm+E^4@zE4OL7}nO`^mRxwwwGk>`?@Fyj(} zsBKxa8i2DE@9*TXSRx+H*M0!QBE@QUC0eunu3K+5nswL#@ZmI4V~Ut!mdtg(iKQr6 z_ub`nA8yAC0S%jcS(eJK7KH0JC43`&!r&OxjjV}gt>ahN;OBLMsuBwd*bzPRpEx>R zfZcFgC+&;a1MoVv)|{PLmhb}5#(S?05{_3AJ-{QO)#~*fq2av?QezfSOOx#YUO4@U z>MQdvjfR#kaqg{i1ncyfGdsh&x2`RG7qrM%ocW*U;>^E4S1GAQ)l+z{Tn^LZa`@QA z^9yi1T)Ygw!^H)~hpe8-q+vN4?Z&!Ke2f$z_>$#CrN~QWNlT}6v5dJ$^XP6=jFF@= z7-5uy6*S1Y``d7supJ3b&yv-*0bBWA$ELuw;|4gFxHtJ3qeb#|;QIKNOn+MOG5>xC z89}_O-LHX5q4nVGfOWNEl9K|$I?tR;Z;C5ehHVzX$p*Oe6mw}Y=42{aOiPH9sYEc} zQXS%CD!Czaa59x(MUO?`^P3ib6W&_+W>6OSTl(jyH~QAthLz>wUj3(G^bpq$v6 z+$c~FuZQe=D(=Mn4fp1chw)>+3}kJE)y(?OxPL%0{r_>l{6$dmf_x3>5`MU>Q{Vp} zJC}Fx+Gkzx_g#hs5DRZ;PUdz0q+Qv?J0=72Lw*`x7$q=d1uvq~{94^*$t`PC@ne+C zQt~JgNTUaFb9N6Al)OrH>XgvBTe?W10^HtBPsj$95Z4ZhkwYopH(+ykH~U1%KcRvD z5E5;ytg|plgh+BXEI7oi+>Ab%8@+Y56Zr4`-dE`Q_H(j|Pt<9#mRw{s(2oVpNo0um zg+by#?ViHwBZZFbAEf3=vUy`{6*lrv?ojb}OsIGA--nfLqte7qQAlv((tMq2lYURaAr9F<4H&%XTH3Z`F)- zSxHS4()8dLTESq$s0s~YA8WL{8n#+!!pO|~H zU*{8CnDVQw_XZqS+XnJub0>O`nY-k3n8}4oG0LxEw+VZYYKTx^grEU7i4KyQJ#%{T z%KYW?rx&UhE|Q|Hp1ZhsWxovL9{H zu7Tl3g`3z*tuIFhM+2y0*LIMH4l|MW)yI%hiueKgqs-_MZ5&~H;qg&s^v*z7M#VPC zL#OIRz)UVu$SJR!#Y4&=h#(l?5HgB&ImhMT6s4$9r?>&-OlC224H?BoZNnI06#uky zjhbC>r?P{|v!IH=Ktw;v*NzP#@><019`%Ae^H`U`qu+qf9)SyRp20ghYk#tVbz1`q zw+2>jZRFd?x5L-$m$8j?(*ESaYxuQa`zdB#5;wdtQ4gvK0u$j9m+yc@a&{%Pgd4#8 z0kdw8wiHhb6-%~3&XQtg284t3Q0DBRoxs}K8QL?c2d^^Y$}ZlZB}oB!osxhONr{id znd~2lv!6C-5RHQH2O@xKu#D$ruO;LvwHa`9e_&@ejnRXMG0o=?HBy{8vckg;|L{y7 zL+jIw#QF4N&A5h4PcsVAg=CDAW*ljZXUf?GW<&b%*my6(kbWgVP(*4-mo^GLT^f{O zShK{qeoEvaAqutTPeiXO%LeAt&ot<04a&MeE@SQ&D&y~AVG^IPT`8NCBj(5$lWI8% z*AYuYzsDttX;!HAD68@P!(0%XkG%!X5#j;zi)yjjv&e)bbJ3VK!l&l?Wnx;#YtwJcjua)1{0 z9L5D{1&cke9ndz6rx6JAj24hp4@tvv$=r%vfw8S(!iC4OOB5i3is65ybIl>HE_+g8 zi}h^mL7%#Pbsd&|?AL?Ojs4bC$kynNCO-^nKiRQ8O?ep5k|Jv|;mQj?UUg z*OAa(Zg{lPn^01t>SM}SA6O(XAgIQ;PS5$*gCL&)YUM{Mc@_zxGW4}bUyt-KJk0%R zulgJ)HrNXU2bH4oD-=60PXSW$Qv_o>?dlxa-t?|MfBN#gQnIo1FbOMBo=4_P(gQYs zg}YX5*Sb!*_w0-(E`PH29joKsRCfP>iruF$TJ1Ne>)lgCv$T)8Ru=XU8Ql@~N z500YYZQ%5Y5b|iH>U0ASYLfvzW`kl3DP~0IOq_Z`oKT|halxMjPZc~;CV8aNT!B4V zREB87_$~RV5R-@$ez2H~^IIjD+j%J1;)cbZXo{E3hu{41kuy65cP3}r+KE?=KLRn* zYEG@|*nv_h%TJ&rGV5Mh9vf9$X`L;nMnvF>(6xd#IBBPp>k+k zB`9@D1SRD07?gzvMiBu+dX>nD^;1z1QWDq41oh~>5){MgBPk+hik`~h=l=~73Yfly zu<2igRd!3imXikqY-x}r402oWXP`X4Qn*DsS;iN&220M{7S*O`Zp671sXbhtGl*C* zSzhv5?Eqxu^%to%WFle(AukI8q4%FRM81Jo88}>%XD;P3D=2N1L2~+LX`{3?1`;oC zzLK%0Y&FpO3$7r7u6x!Rz==%t*FlgH??L`Li$UQfXwIXnR zL03tL*2vD<#~`&>cVcLLn|`C>CP>uN70#0pi^!U**b7g0kK!0E;lb}ik-Z2Tq5!bI znkqb7A=4k)7bW_lxI_OS5)RCt-qb+}%I*gxsQ`RsL1cEtqdbddNo$kL<~YUhNZRNp zPucfy*TRpCVgSavPkW2?#P>2PCVoNbDZtWtv4t>Id*KV8#jD8ZAYM0grRQh1?LUq; z&NOBealRsRKq1l>qcM(a2exE$Eh^T0IAdx7;66wnqCFL7cVo5H>hobbGuc3Q(o7WB zAumvE74BB$>2RjfAwhBR7p!E)o#~8)mb}%aQ>O2`vIC_Zjb*ARA@l_GiZC1P8G$#C z#1GA$HZ~0p>a;!sJvNye&rRtwIohl_v`zHw2v|d=Sn3WUd!O7v*r&m9lP!ZZ8VMbW zkB0$9yDc!%ATT&>7|%ddrDY~Pi~N#~7)tEM-qK0a1<+wDC3>bkR8bJcH%t~hmMBqZ z9YjUB=H8>I7|m%E0SuXuW$_F{4{xd_)0Bz{;<+B^^!1=2*t4sMRWtLq4WMj|VOXhX z94T=I12wXPgG|H$4!?_FbQVD#i-IMDEO#=)=o6gOVY;H++Iq+aVTR|T91eF?%qY(X zKAiDbf3myE*^6LcNztg)4q~W%a`j>-+nLG?u1azF%(!>Sw+OxdYq&*a<~}x(qY{`; zaf$pQz00}ladZ*|YDjA4Ob?w(^CjuTWE1pts`(|VxhpY!W0=lZ++|Fi>&(RA;C*b( z>^`XM_w-m8B1$pJZxWyJQIxSM@YReyWlZG?x{2dH`1?VjI6B6C1f)JLqk)ioJA)qw zr>Ui9?a3_VpQ*L#@S|gOK(akXhn*hM!&2h>$166USm6DAY}SxUB_ilH>(?HIqItC2 zs=?KW14hj*fui*RXs<@`D72GkuafJ^u8q`v!>p1xrAMq>=~Zt7B2lSoBI1wrGYrGZ zkWK$Wn89V<@&E}9+dzz0yEzu-&O~2FovFcZ4bJ>D4k#&9SI>XE${77ins@`#qgmm@ zqmZyC z1-svnmcn`iOX2`qR#;h-a4(25N=(!rqhMxLf5Y4=it!~)O#D8M`(Za7%U&@_rzP*? z$N&_(4`^cg6z#hg#2$n?{}tNLWZLKG*dW>&uOdiIrxQyt`zCu0iUw996a^i|QmPV6v(r#+s(KU8F(xJ|)wN*i(;_w{ec!{?f+@*DN zMd$dKZ~_i_g?K*9^PiBF2r?ub-{o~0<(mJ9Rd>dnag^S^i$uPKB;5CcIw+|~f=Eeq zBg|hSS_${6^J0P0l&bDYdjUtrXxezsJSr#xaSE3?f;%$WZKq5}pCtm_JAq?exOjHH zx_ISfXBVGmsY-VV%M2W(b|$4;6TN3>(iUw{k_$E7T5$5zAv%bOo0G%Kr~qn!ZRwPH zaHgTa7U`g;AI#U6y+wEFLyk3<`o9Kt)Si3sA^0~8oU549r#Z#(Nt8^|-n%i5 z(pdxN0)`JC&t<{&5ez0mBg>yf+MCG4&JKjC%G=@dbf}lkU%Y`ZnkqyTPA!qEBad?t zxwj$o_^4!VtAGQMFX1hpwxAkY*g2q6N!MS8V6oPJ8Da&ZB|L~ad_F^oG!`g@P2)4h z_1AH@5_b|P9SgM(=7+s{^53}M&6Iou+t_`-|)N(TsF|ZVuU@g(keHy2i zntL~!*ilzj6kf8UPmmU>yFEQdN8@P*>6}COKp)*E--v0?j`X>3Z`>zRxe2S{1m^V1 z9d`|Jf^@o(bq`1}oLNBUVk%bKUapvfyW<o2G(&Sd|yBuAHu{rE%V zoUx{#;6peTFKKA$!4w(m%<=eZaH`2b{sFb`XYdF|m6yW4Fwfl@fZGdgrfNGW0$aHzNcDBPnq&||k4c^`=_c(Vj+aAFFf{J`fCz3axu0ApaROkq7CVueov_S1h-IwZ8x|HrpRP;<;>_t9+l~GvqvP>&)m8=PC$|_rv$d#-yYpQ&I`B-_fytlj` z&-<+X)Z&ilPA&e3T9oNOi_EbXMEzroeO$1OUjiSfH&Nfm|JJ~Xzd3AWD;KvU zji!7cdzF?>|GCH<2eR=-T9otGeMufYc|VelP9DpxAt@UIK~y^E@lq+ra9LVaXCqX_3&5-Myn(lL$RADzmjKPo|lCR>i< G#{U8|O80UA diff --git a/Robotics_API/__pycache__/Bestman_sim_flexiv.cpython-38.pyc b/Robotics_API/__pycache__/Bestman_sim_flexiv.cpython-38.pyc deleted file mode 100644 index f5ac090faf3b8ec1a2010ed8c2eb4c81e5c6c7bc..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 20037 zcmeHvTZ|l6dR|v`^>r>B4u@A!WKk=N?A_r|E3G6iSC)8}xGN5sA-U4lO71lK)C_yr z)7?B(L(Wus*2z+vbsWgv7`B6CFPX`k93w$4aO~tIF`PF)2n=`zed<)zslU$UzyJTKyHiu;4F10Tpa1#tAKuMm{)8v#zZ_03 z;OG5s6e^>PX2vn(Xga1GvrblymSf2==j7y=ck*&9#K)pjlJl}t#?fp}tW7$T24QBK zQ)|=CwA8KUzO@-=ztnTh*|h`C0jcLvpL6DJb%hL^-e~WRQcnKDnBuu7nE^3 zQ<->*ac34QMwq*Nzt(7>a-z|ClsqOdbQxDf` z{wwZ!)78$mKETa=of;qR_*D;$T#xOW4bOkY^L221Vd32M7H%|~=a!r9M&nUcs|V+r zjeFJ3W}RJ0|5-S>fS)&GWHPQS3>L*u=EoT*7oS}LdVIObGA6>-c*Lv#viLQUel zsHW63jwQ8E&EQy8`_(Lt6Y7AP!*Nm_REKb!Qis(M9H-S$bqvRS>N#~B#~Jm!I)USU zbyeBw)W?Q1tFEaR)E7`Ypx#k0sne*E)t68^sIIGdbr!WlYC)aDlZVyI>dQDD zQLm_1aXhNNqF%%Cn0j5kf#Y-P=hatnJg&~Gui^N-dQ<%ZjwjUD)!XU zexuBGvnVax$?fnw?`KvEI2LiGBv;D*#Ombsls~PE9dpa__ifK~tsMhx_A66mKgLt5 z`&VbLWS;Qa0cD|_QyFwIw-~&H6~E|tuAi)TBWSn2(nl_DVO^``I#xLOGf`vLZ{z2^ zf?@@)_~?sUhM(ygzOibqW}lea>Kdyc$p@zQMX4DN!aLfdz=>{@hagUK{k{-D$U*m)aY0o9p8>)~XS@FW`OU zH%Slm_FCtypW_J0q4I`lOlQiO^3Q^o$E@l?wc2Pk{AzXK&1Sn^YkF^?(gPCg11!8o zRh#XV1%G|5axg4KuLE&8Iq6lH)9hSWSkaA6$JJ<|-G}RLtG*d#-)pagSv>4mH`)r* zvWC)`kmqg-5r>oVbWGBr+4e%KQ}geK7H1}$sIP17V$M`UYp$L%y{y}7RjzDxxzTjP ziRk${c}$q&gnQvst?oA-xmB554_u0LzL* z#Xj5#i@k|DhBNO7ii|mJk> zlkt8eFbmScP=z-#xM!#$aDFxJLy7kuMrTD;=9v)T#A5L3wc2{qx7&4DCuO%=woI+< zxf;BF-uCroqqSoDZM)udYua8|Sa`~H3|@?$tk+ttwr}5aqx-I!x52!9K7*!#9m5|4 zYd5@J^I-OBt9B2|z7oF}Pid0;Qz=LR!NzUB(QMj}YZ2V#wvOAPSNB|?xNEn8xFUFJ z&3U_no_greeYgI=_V3sH(t!@cR6l1oN9PC`<<_t`V=8edl*+sc~uVDRl~?UA+M;)2o})ZdkICxECYwj@;_-< z#$*;}Sxe4~z;SSrpXGDG$#K}7f;WKRT!aL{IbuOOBA+0!XbNU<>TX9-qRTYL6yL)m z;Svjrm8?vdGri*aGN}>!&?Ll94H@+CAP=ky)M`ZQV9a>K*U{V~{$o*sj8F$02Ljc{<(4NTZ^s8t;JOPE3 zc9jBU5%)Zi-sX%EJU4FGam%_)4Vv?B3cmt=!kpekrE7dx*fO`WTh>-?E5B9Pq9ixE z`EH?G?B-Ui9b>E1Ev(dMIc7mj{h#V zsTcb3igRLYGI_gZQxe(V-k7&H=j~wLe)t$aoIS+p!{8%MXYb}mHpG;DmD(a|D^1s1 zNZ==xTQyx+LogNK~A z&m=w9?IsyZ!>`}RU}}#m_T&4mc9RyuL&#*%9V{r=D2H=q^syQqV!QN6fm61wsYb1p z8g!W5*bJ_HYCvCgSF{UtIU5>F4umtrCsk{wSo5l)URS0freSd|LSrK|HbWx_&4-Wi z+rV!VzaTWgdf1^bC*ugS7{@TmO%X)ZZ%2Sd%_;(OGUZug)|fMbQ{#|t;F-@v!F(D8 zp;8*k=;nxoD_JUypJca+-Qu>nlljovDh(juWVb{lTqy_&mYxL#d+0R}Y_%m*1Y0W_NoPHISEt!xT%$e^{3P_|#w z5M&%@Xw-gc*rKv%N?}dS{VB)slmmE5bLD9sMPS-xh{4c;uF}#ru7tVA8q&RD3Hdsu zd5YJUrDCREcBd{vklc;$hktFWytF2c#i;$e&E{7g33%;SR)mtYJb!UCnNp zzUAj=9zHRj7@T@cjS>8pG~Vil`zY1`L~VygwGIWw22<`UUyz{-<@8g7Y?tyR!uX2` zM;BJ?+1l-B*MlV?+9()k`X%!*H+F0F`?fq_LwoXJQA8@q8ie-RIs_7U@>*>}>W^y8 zb@Z3YWGw>Lr};`=q`9VQLcu_{75(Tkir&0Z@JwEs6tBER{{5ypV>X ztiz>I48PP&=X=waG7GgPi83uH z1@;+ukt*ly)AykIpSH28r-5vzlNW{-Zv^%wS_Zqn^8BI@agd|EZwKai+sOh>aPlIw zzdas< ze;+Nq85EfU75U@FF_b(8m2sFd7RCVGQ>~|}DCjbYN}uk9vL7e&28ooS?CV$l-1Lk& zId9*=I>Vxb-nwfb}=KOmLrpt$OxK%KA4FQHz9m&2LEM>ev%$x z3S$~A)u@Bd#^VqY@~H6sKM92%RC4cxhl~Rop%oCz(s1%a;%XQduCAi4PqG+icTM$K zZ9{GRGc@ZV*H{f^p!LYqC1gfQCz(;@sb4F5j}x&{=3-hRlOabc>$H zO2i^ZaUu<66urP1BN8N3VX~tTdK2}O79+?o;a$S6#!k~D2kzH9Rf7Dz)GXP%kzlX! zz$9^kl1WMvAtnnRU3#+w!~RivkSPr7`5SuNnYLpO-6CHWm_$l`jOf@L`iRy6hV_hq zdozbgBRvW!>c5U!&zCbPJ*!4m?J&bX!byzZ6tn%P;lBq2k0L$d`N8~dlRECi7DJSGwiP*erRI0#DCeW(&R?x$1*;;3xg#_Hze zN>f?!m7Kh*NE1MOLAjkAoX8~1T$Y#4$Yc8Z-6y3LRzg>sjADa(-zU-m%C3elTb&jwuUcS6HtY_=m;-3;b`4?CI*V}RBzrI~5MV8cSxGO%cB7<=mLlzJdWak5kjk(+oKZ4wI^YmwB^-LMc( zQdGeR!z@&vVbVRa29K^Nk8oC`(dv%@I|>d>%z$bqO;9eWxA!y0^WojV4bX21u;~vG zKKuu$!0`nNel7zlg{Xr!11i-jys!-WFy;!Yjdu#Gu)Velpkx!2d_kzR5K}U*EG1V+ z$-JV3ZrO&E%qx$S0ZQf-nDMauF2TvhcOsmmf8du@?uqeT_Qu$rfG;>jY==oAj&{t<-IZMNyKpgT>|-5BN(mus|@_IPE9#gMgrJfZt#c$eFc2UyAf z!2{llC^8dect62RdQ7BuG>eGX6|-~3bas4fe=qQV`VapB|9<~%-NY;EV%PCy_@mOm zpD-F~E+l?s3Oj&-E20Oy9x-j+^ZQ`O6D0_aO^)>6=)|MYtxf<2XbW;W0|p%NLjKT)ln$;$rp2P0H2kwVO+q zuZBk`NiWwl!lwu^-9R2nyLGLt>n>mkIUE|l7~%BbEQR}qMi=HEBe$b|KRh-b074EU zj_VVG(*x}xf)fNd31o}3<`M(h2pYUHPHWy9h_Iws1$ps8y@2`Z{ZalVq;Ul&w1tEq zkl8du5Y&N7dO@y(Fe15(HjI~wgcKs74s6!CuwWAeyAg+r|FRQFw7MWh33k}@9z>Tw zFT|h7pN;@EJrl7@C%QrJJHlP?+AkwYii9dS$lw8;x4*rNkm@qRzsm>*uc5w%`WiyP zcM(=cplk6X{MJ57rt&Wzt+K}we5m`?-URk|Nql{eS)@a&*GeJMEhh8s)0WKBB9!AX zd_n&Wpty5@NeQ)$%M6Ls{r81tbr*Lk#Ym&xLS6q&7T;mWXXY%aohd#dBD2)DgRlPi={<%vAOlJC{D}+}Sl_Cc8I@W%%GWi+0BFWiF8h==RLS60b;Qk&2-?YX)yT8#RnT9S9CWHo=yRX@QDjoVN8J zK7XVf5bQmvF4p_!fTw9~uCE*|3h#*>b&Q++=IU@?kgZq@%im;z zz*S~!>INZ=5wFh4-PP{urHi+(Mp8Cr9~P%2%t^PMX`UdstK5z1TCL-h`{&MN&!tbx z$0@el$6_1}N@pK$P3!Y`4h zWRFywcj!(wneWV;5M>B<8WDCz5yDkS$t^gg;C!;^d`fD0aa!>jGN0qSbV=o)UW>~s zR=0E8B{-ZVKdf66r!#Z-bHQ(ae)uf@zrFF935l_h_!q!N8g7vZC%%$OS+C$qXw}=D z%_tO^xIKsLy2#r>Pe$yz(7%kjGuK0qwCmD25PLP!7bD-r80W??G94X<1Oh2Y1XLkH zq+X7(Ak^7sMJRzaAeJUElpKCp{JcLz!A%@bu$AMNVTnC49%S_~AJGf4LLcl+u7kln zVWi6Zer8!%=f_31<_;6&u@v|`yqj~#NI{M(&_?XEqhOW~-$lFJHhi1HF!KuAgT7~~ z+%j@u;9KpT%T>8n^h?`ipq%8yZchNk%U$?M!G6mM`D7@OONJ78WGmLz1Z={IWyW+% zwU;qr33lE=z}bVwycQ+9Bmqg-hB6}&7ZVQIKtfDnro#$A)>)&)q~|?xGrijO8OAYz zq5as%z)NUdFszpJA_aMDOfAlXy*QXu)l0z|+gbYzxRwY{3=JC!`a!*Mc@gCG{0pv9 zi$&(lJ;b_`KvA5drP^@uaF+qnf|%en&@$R85>2Kz689v2F^AAQhC((A&2AaMgvizh zCPkZqA{8PP^$v5wnl*7W75v`EQzqj4V-f`=c^U(@8FIzi&e$G1C`o1B?+L;gd~7s&U9l#zl)}T zdWx^1;^FLHUT-!B9G1>pdfN?U6EipThin}sv{hw6oS9@m5M6{G7JF;%%%&DRzAO2o zPMP_qu5LkchZCtr5hIc`49;$NV9dPzI%k_Jn3pvooH1k0Wb=lF zy@&Y!vwUIvW}7jP2FQy80p!Ts=^LK4*U~6|xz?=1Uk=-Vj6I$XJK16kL*n{3Dzi|esW3mY&hW0ugSFmQr3BFMp*~4z- zt$v`PCr_h=h+j5HDFQ2nFN0>VfJ@z#0dg94ff(DmS>fZ()Idj_nc;U0uly`_N=8#% zz5b1=%;#h=CNMJ&np91v`2jIX?( zd3eUk_~i%X9~c^tw+cokID)4SN~ZLoZlRYa%^d^k@K)hFg%2{{!c!mMrw)U)9|`^_ z;d8eUX~KTQD8GXX9B;PehRfQcTBFImfN_c}@JoZGQ+v|U#w4K(UbP!YHKhTa;_+iy zXp-_5<2gMEhj9NayWF<`0aFxF0KKSiDzT zSIh@TW*PR7wAcNoG^Q(tJ)$R)H3v9m&r?p#CsqSPre0ZWUc7x*( zW^psj0~4T9hNV87SqQHs?l$-QtM&WsC3of3&=?6CPNtXqPlzEeVi1|ai*RgUcRZXM zvncta!(*E9b#oHuXG|pgkA4YZAq{Q-Q7Et|mW7>B^u}HePh2E&v6+S%X3svNC(j1qihQ z1&OiwBoi;u@E$i1GmjJ}+!P0&qWr15eF?@-ba4H+eIa;$08@MKh%wI&jfLP~+$Rd# z9)V&L`0U-5yMatVZm<@q14Inw7SXvFhoeZBs}|tp_$Qe4aHg}lBwtK;_)h#$g6Y9? zO`|T#?)V)(FwyXO^P$3`*fF#==*)-r?-UNsWc)Vx(zLICo6irDb%?Je2r2fMivcbt z%q?RTMHiYj=B%<{*o^Ul8GKx`LxriD(xjcP$rrYv$S=uo)Tv*}}9zPGyeb!4X_ zp&o8j|FH`l{AIz$S6&;+i|7dv9$)Tn=86Wu4RJ|#=X(H#TM38h537%)gc)IQ+}YcL z&VxP0e*Q-;x!U|^byY}!xg{L;wJ6Tr=`y{p5lF5bCGD*ZI8e;0@TI2Z9@f`82`ud)~(G#8{dixmzUKYo^Ati(w-ex?CK z$|#duAj-lD-n~p586kMu{=MCe&o*FTNc#8C)kXdL9M`zPO$kQ!Mt3$fy0JT823l9F zMf}C3sRO>Fi2~TfL!2-8gjdY|71X%~QdC~x*IFoZs@1SurHPI20-!!wO^#K4$Sf7Jr+?-(m50 zS^R*-Z?O1H7L--`U$77~`8TZnTNZs>k(#&-X(}DX&wC36!ZLHk<3+2OEAGR8sW?@f zDVB@VsFjKn#hLPREoF6V8DZW@diMuC?2g?VFmKImT7s4DIrU)U>xd5T3 zSL6d*61T?JtswuXZ))l@`bTVXIMVkPeC#Xo7U*RWS%~lL2nyAjnCN@!p8g4+_@^kG zGCzZ~Ag+<<>ph3%!Y)1w=df3ApuL9=0AZ`&0)pI;?^{#mMNY}WI=PeSPc diff --git a/Robotics_API/__pycache__/Pose.cpython-312.pyc b/Robotics_API/__pycache__/Pose.cpython-312.pyc deleted file mode 100644 index d1a5f1cc41b9309719cc4a58074ce42b31868b4c..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 5528 zcmdTIU2Id=`P?5nj$<1q4lve0E=x-=3QoXufMJwCNXdZk6IfY7=HB?;B-gR8y>qWi zY{G_G9@tc=AZ-c~tJZ3jx+*+$(xmlmowf(2J%%gJKs6y`_A`!{HI{hPvE=wkC(@O8zSVdIB-6((pl<)&NNZTAW=Al&v1i0 zKp`V!#X%28c=9Sy#H&Q{%<%S#L2r?ddP_g|w@C+kwT#*?XI07m64I%hp~mt>oI9(z zc$`g0JG*x7+TOKidskfA@p7VTPa?ht>ADN%c#c+fK)T`tqK{-{E#qPlE$d?w1#QQU zox2Z?;bbh8%OYiuYNV)^H?^FeUL!QVDpbTNUh&-a&Qx^#+;_X#*;}b|ZvRa# zS}*?;Ow8}NVV@4>D`&KlD#?si8NvDEt! zXVod{7+j%Ix-f*%YFK(4^+y{m5tV2892HyKq{S62u4MT&L(>gY)>Ep*>v^lLqH)V(jLUh|;?7tNqcoRI z+M-!?vXR6Ku^MV_+ww5ISwcqDEk7ey@kTBGcjQb#J%R#7MN2f)%qR`wHGB;ki=zhK z32NlY_RjHKR_%oR(RoNU%oDPn?9Hf?+8do`aw9oYOBu=TQ+=IyO^~`QUoxqIohOr} zAVWFBwXmOoS>+0O+)OrXym|gs=$iO;aA9+F*0%|I-s|E|yf?+4c^`X8VBPhJ4|aZ- zct3HkA@=dU`FPK4yyuShZoL0~xN$moBY1OSF1+np_x;wk_qspqd%y1&$L3nQuJwEt zXo1zi_174gkva|gbB2yM3oq|hhLOqngxJy+dw8H+6t`|fX$PR<&_$lzG_k5 zM0!kZSIEN<2{*rU`A3(_z81O}P$@T6!T^nh3Fd>{K`^e8+yYM%5h(10(PiQ~_N8%` z72oRXXHW+C(&)V_K1EQ(+itZrfV_BFO4$BfC)KYw28K?$9jGY^mW6*`>GU!+*7)T! zX|k5yR;46a{%X*(Jj|CV^43eSR<+hR0k0Ajo(w%h{4$Gq)eT?m)&1K$q;_@!bReR( zuh_di>SfV_qHA0AEV!@$b>-+CEv@S%}&qvy3BW>kC+d{DM`fEQ<-j6g-@4K<@ra9l*IosMf z7wMXh?46D5{p6)PYs-}OtWRK40Ze%EM$ znI`x#QWU?F~rxB*+wEm844g`)1?Quthodc~Xe zu#J~Bu=8r};?~1G%;dOGrZ$UFnGsi;tH04(S2rga?Q!H4bCEa-WAl`h**%TVRT| zV}ovRHp*M|@a9ZGm6g*ipHax;3)CouS1yLK3G3+Ogr+OYvN@iGZ?`2>JZ5u+yCp*Q z=KE z?K$@1H>?1AH0(5>Jl4nTC@tDiIt-P8npCQP4^VvU+ah7AMlG^Bf+k>8@>c~q4P&DX zq9L&FBb7LPKhU_)6f1YWQjQ(`G;rtapNIcATpqeGdt$i!J-K{*q&%XORc&6Svnnkc z=B!#MzfmkJrE+Nsr2YYSi2nk?G5(Pa0C}80VUPNG7@g$LAh^s8AQ<2k1S+rLI+fRv z5&AQ1rayMih}$<`x`3V$ag$B!ZMhx|bV25;iJ0zZj>@jtSou*=EOZl;V#gXgnb@#Xt6u$i{_mP5toY;s$11 zUoQFpjtB+@aKHq7tQIq7a}uxz&-7Rv*QO&ER#mS$JUBcAv87JaRr(BdJk z&a-i;DeMV7ezDmTvKJzGw2mGIV0rXHHea+u%6cQE<%==+7Xkm=h?!K@4Gc;Y{GYJe yke#%G1&D;U90zDG&|&a4@Q<2@0>^QWyk1URY9d_ZFJ#9T-g6umS|r#qPW}n;w&lkF diff --git a/Robotics_API/__pycache__/Pose.cpython-38.pyc b/Robotics_API/__pycache__/Pose.cpython-38.pyc deleted file mode 100644 index d992812f90ae2eb3a2ebbebc590de7f3a3869370..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 3494 zcmbVP&2JmW72nxilFJn((XQjfwVO;@v@%hn|F>($Uxa0d4+_V6HjooSS=a-y4!kN>PEN3(Rmn-n@D5_r8W7 z&d$09e4qX2-Oj(~4C5P|On(+kZbOs5gpL_)gAry%q;0~l6;{(avV^GLgZxH1?Ofnxg16-N@3gvIWDmU5$0070?7BiXk%xK#~wc8~&%PP!y zMnL&F=CT}93PdzWVi`)km~7!v zh*D8MW;78YkZUQMnpJ?ZjXxU(C1~$MlmCJ)GX`Xz3=EG9$j27^n$Ymq&PoF_D|ajV zruf6a>^h9>z$mL8x&!lwd~U%>>u~1C(5Gv0s_ADl1J7((a4O5@2If9E0i4T#8pgo4 z7O#DW^2-sW1(aWA)$TmA2PP}A@^kAu_c&RdRrbkW33C9os{apS7+Oz+?S2eE_)(a1 z04wI7X(yt|(=HFPrdyER5gpkoW_YlG@M;9O(j^+F&6xQ@_5;N__wuzDL{%8?0E9CCcoY5p3BA0za;?XONW|sM zmtFv{>mioE7PhKqdKXZCNW>-vzo*B zbO4c4d^pDTJ~UZ|ZeVl`2r!cWu?Un7&kE0;1k*#F^+kLto)GfZgcuzUzXs6|z|SSP zLhC>5uE12)-~w-KreC8VCUiz6Kd0rM-c|0zGXqV9l046b7C; zpo|lx3zJD5nzrmJ_$rhLEJ>ea1~Uibz+h0ex`f%6;B|3ev$CF5PRyLqtjfxVrY^tW z=Z6*}N8~OPvLobD`(kFF5F`E?_|F3Wul{fTb5P!g{AbVLzYF~5cg(*Lv7Fi6605Uw z?EG_+KuP|=`uqqufWDx~{1IUn*lXuM}RK ziZIrZ&5I@Nkom=lx83O7kGGOxX3lFxWVh(1z_jtwYOZYM<{~}D*)c|8e(nkSS@9YH z(i0hH9I20hR$hRv16Vz&eGU&1W*pdmH$Sx>883*q3R~K@UYN>kE{mD*d@6IetC(=$ z5trWjGqS@BP=;KPzo{KdE7)9(lE9DTEf|c6Uy>(+0__hMO#BLt4JBACMwrh9rZUll zZWCG|iFg~AQLe`gQ&r(r7eMt%V&%wwif=rb*H7c6T>ZZAcEXsQkPtCO_%w753yTjO7s%nes;UFO`0I`C@Ot_+!K0ZtCb4pz_@{%d%iay_q? zFeunCu6bDAqtQ;q^H>sip7;qKd=opZBFZu)-$cl9f|6L}9b((g+fLOzFL1q45x<2W zRf_w)bZ-dL>oN$_y(T;k@y*)Igda-;TA*ol64po0x=T5$L#b~eb6v0PszYkle*qMV BagYE2 diff --git a/Robotics_API/__pycache__/Utils.cpython-38.pyc b/Robotics_API/__pycache__/Utils.cpython-38.pyc deleted file mode 100644 index 455f025e10f3ba6717a07f91fc8916af32b4036f..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 4765 zcmb_fTXWmS6~+P}2~m46NBME^;||{B1&FX(mSDmTSR;>mK{)P4|A7di?#lajp5wFa_tH3ON-?0RpUR}8f_@srkvZxK@j)X` z62CiQPc!i_j%586q-8Z&?mdcApH@9w@7UB=JS5f{-urly+aRfR#4-r-$>S*d- zx4P~zdtr-8#zgfSduV6&k#lT8NHu?rSyr7mx*tcoQl;GII|DzJDk4tPn6C{&skk3? zLz(b})>mKhSP7{b&AMK;;VVcN^Ar7ZY5eG)v=zh>21&}}Exr}TeyRuP#NVBq6GZ%v zdy9O3ksmDb!)NA*rk*KeS4oLYp2u-GUipq|gtmMeNED3G>Dw9#IAhuN0>UDZ*S(AU8` z@z7;7zJ82G3T(Y(jJ~${zJ$?gQ*#gB*V5ZiL6mZc9ZEPsJM?b4dU2GbN`u*`_Hb=; zJ4m>hiZ!DN09q>j&NhsQKR+l95G1!?h(1^5p4?Zx=Hb0{AY@6XU{~s+^dMnZ$qvBE z@B1p13DnQ~ae~eLNMNY1Vln9G6}{#|m|CSz{A&r9$Q^kF6coVAkJ&hV;GG_JPWd0q(9J4kYnU;+LIZ zE9h|Oz89nft_1g!oXi*FT2Yv(J4cS?``w|LT5^bG(T)^!`Z&yi+HnB+%+yz1nYIlh zs|1O$YBnKp(;rd1gLN97VJs07UVK-d%`so23OSTzo@hDDVk%LGW z_O#b~^&Otvg5Rqo{V+)3^{VFPt(h|a7$$ku&z5`7g-!(^O zH^VM4^;7iypRw`x5jLRD#5k3+$bdGqN(apq2Ig7&%#~)vIAP z*73$96eo5sLLs=)T*V&cdJX=o~n zWaTR0DiBP;WM8|@QX4-h&A23{mQ_uEnK+;z4pzrm?#NU;o ztz%AezZXt~2M;r*@oN8dgoerAj@gi9?8t@(v%BQQkf{lF%;4J&hi>K`xdOpsSkWy9 zt!i4yDo0g;4DTf?+LcnfTC}T}Q!BN-QhTb@u2Z{P8`d)CsFq;`vvzn!3H3A*YLfbc z*az@+AT^#a;$MF8fNvquF7R+yb(7UGW@$isqy?z8vC7Nf8Z9Jmh1^i+uwevd(#s-I zJK2rwXqAtBrDkI_Cbv9LJ+C$W+mkst+t1lS&JJ^X zXCLnYUTW_(yxd;fL~1~OAg{riR2-*yZ7Ya`ABMS0jl7~0+uXr$UiJHZ83`=(MmNvh z6bVZ1($ZE%UoUs*lY5|=n|!*&F+hpIw(wz`r@(iGq+}q~Ch50EnP6bJ%CI`DXtxGw7C8UBOf}-ZA@B5SDOf-qilMS@1O+O zL2kW+B47tq06;Mse5SQpPQ(kWZH(&=gAv;Pq~Q3_lpG0P8t-)Gm~()Z4*DT>St#;+ zR2gv?F7kemcD7NX_$m)T`@m8svL{ey_9mJyR$FOb}3ZU)SBk#_+l4!lW?eA-iY=uO~dI`arK z-5Me0@E_V|ya_HND4f;C1X9lE_su}jmS=8$0+|oagUo3~K060A#xtF8c?!+?Zf7-{ z*R>hz+AOc39HdgMG1IyL#gzCvt?!Sa7xil*pAh+o$fWuWBu=@HS$$3$`X;>}V^yK< zIrbKtW9lAy8f?A6)VtK0%WLhnh&%0eYd+}&{evcIk~HwcCNkzIq4=yQLDv|~ufZ2J z;UJ}Ku9+fZliXfKcjeQiKD9`TsOHpllg{pS8m9xNc2gR8nx7J6^=)(z_CD^RdJE+z c4mL@syn=&cm+ktzH|1UO=Di#Bx4pUl0B(T#R)Wn>4x17|9%rc-bOF({cs-GrP5i?L(5etx5$?zFS zGW;^s&&bbB)vrv<(+6qw1!{%p(htf{$}h=GE{=B$@YIiw&&u;SP>hL4*)M%G{gV^ diff --git a/Robotics_API/__pycache__/__init__.cpython-38.pyc b/Robotics_API/__pycache__/__init__.cpython-38.pyc deleted file mode 100644 index 99e2730476a785a20f083224c64ce8642e95d043..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 212 zcmWIL<>g`k0*9GC>E1y4F^GcAU0DDV=iM9BaqD; z%%I8gk`bsxlkt|YQ)+QZZem`1P-;IMq*+sR(3b5i^ij$xy@s zVuOfZCi)rqxvBb29uxU<(F!fIH8HJGp)ws3?SI}Hc_rP%G38f9jgu}asoEoOC> z8a_s@h>GyOWsRy>68^VVV_8(iGX7V@M`Bg1;mxX87nd-yCN{)n{I3i5sa4w?e%x14 z9H~LQH}L)E1<$*oI!r}Y_lIBHPkRFybq*EZx?khFH*ej(v3qm(7JnFrr_nKgq(q(H zi(|e|b27dkW}!R|MZHljW}L$y>#G{m-hQeA6{X3;mQ7>hi!86;zK=WmEr_ztSPuKI zS)jI|j-l=~tK+_5uWeDvnJ9l}pE+-=k)7MHqY@l3%knv{`)P6;(p175G!iAxUn^=%&2!#WRx)?ooi zK3%7s#&LQAC;%p~s~5CF%uZTyU%)~RsPZ_-6i*W)mpUj$6e|;_Iv5k9PbnpwSx5~|?wB;f z{$WcOJdz2e%%|3oD;&hk4gNie8O}V87b$PfA161@z`iG=Zlt2)5Kcr^;RI$gyGM3l z1ZTuV*aYb(h515qk7VZ~{$zf?xz#zGheUx07%j{J*xT!bYAW>9XtY%;{NE$fnktZ( z0%T#YX)M1&o+DGq?nM&;TO-KnwfHb$+JCsnXTILml%7ymIbJM&0d;?S^cyd&m|Rtauv4$>lv7$ z&b^`3!p>}}WF3Mms+wSL7?RC!~D{Rau8O{M$R;9djRu-s)->@-Xn&Hc1zKl7Q z8Qz=Wmu7gM_)cY1$(^%G4hd!*@tlrFb@(q6fk$Z|xG}0gT#)b=k00@N6i-snJ=w|j zrqQAo2_b6>p{(ukS(Hr#DJ!CaiUNAZVMXyBXS0jdC?D+cN1z0$LJd>D!Qx1!Nrr@o z!X%?GK@e%6z(K=(f^{1nsF~NO{^eqhW~Z7BG#hIB=oI$=H}NMmPumZk>5_@LuHekb zG*!CNjuH{Xv37~kB~x6rgV(wo^m<_;Am~jj*RDc=*Di@RN=9ex(x3J~jW)VEqcMPq zmrW7ilv16V$%mK8O9iBwri21lFEAw|Y+Jt1Jlkcf%*XEqTejur7@fo2G6tso)t~W@ zTQ4gkHnK;~$Q_kNQ}XXYj!`A8zxb6Wn+@!s8LTBEVqs^1#G#wnZu8XmIsp z+LIN?Ab%BAl2CD{XvZ>sW4hXVTbX4HnS+?>_hPKHHPuj}zHRefq*{kiIykBE6Leny z^LR#$BQu$VIgX@Jd+U9%075ix)}pxvj*6o!BO(ta1*qo8CHzUsERr%sZCTzV{#QhP zO@tHql*lJUs5Ozl0ntnn*Z|LxQCvGvd=W`224gBMdandxMS1l%P+YGt`2|M)FLKJW z$Whj7CRpy~z)lC{6=-pgM=F%fUb-W14mZ`@BkXi4gq-2IiNp`VW&#PcqzloQL(F?I z{&B#Vb5~e@0iD|)g3f!p{s4H2+;pMs0=kXV`w`Z@Imo6l)fF_u)Th>#8>^GtQ0TZA z_uo;pZxgvggg%uPU{F9~A%U!zeVT|n#eW2;aoE?{CR=Cn_ZX?MhczZYChn51G@BxA zHJf^+*=$Wd{_yBGn?WC+g%Z4r=J8Np&00}!P)84_q9Cp-iSH7s<&r)K44CytNLO{_ z&@ik&h{G;Y^fStgB{QU}Gh^~I5~5z*GTkIuOk^ YLEMtdT-#&IY{T}id7IuZ{7rA|f8Pf)Q2+n{ diff --git a/Robotics_API/__pycache__/utils.cpython-38.pyc b/Robotics_API/__pycache__/utils.cpython-38.pyc deleted file mode 100644 index 4681ae70b207ed1cf988193eb7986341a566c9da..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 3969 zcmb7H&2rnw5yk)@0g?K%?3K%@Y%*~+RbrJ)*-Bz>DJfUHj$Jv}8%MTN4%aFc%mFDl zAV3X28DxU;t)!fIH8HJGp)ws3?SI}Hc_rP%G38f9jgF|X^}7PGob zjS5Dt2~YUnutrrZiOM%tV_8(iGX7V@Ct_8s;mxX87gsQ{CN{)X{I3i5xmDX7e%e=2 z9H~LQH>gxz7Ci5U>M#{qy)yj#VcHwWsB@_J*25a#y?y8It=-$ZclhHtJdKX|6D8{W zK^*ganv?PUFbm~zDC&)JG2|4Uu3z5`yuY^Hz3M7V>#@< zVS(C)I)=J8td9GRy|qOtXQKSAedfHgMs{w)4lg)jmQ~@n9;V50C>1~I2P%{aHsmVh zkNR;ad60DCFykkYI^>PNz2d=XlyN!M3+9>yo&_?1145aZ$6M3OJkqj!kGJC#|8bNl zGfEr(Gc(l68_c6_UZOudw9T8DMDE{lSSB(VTeWEq-i`A-w#z^Cj2;v`%n?G z#k6djr|K}2Cs7vCs0>6DBokc=p^qy15e?^@K-$gfwmn{&zZqcbcF>O%?tNjIfv7&a|-7<#Zl6uc1{Pw@0j++`qx~|_VrR*rrjnvPy0dE3@IGCtWxq| z&C#xTQ!YdrzIlxReP>CW7VG_;ox`*n?hJxt=Zi2?PlKdcplfG8eUqxFmA!x~zerDa z2+bL=tXF8o-N{0l9oM#yS+xZ>O>g^xllKuWlc3&^rgbUau3E9PpdlJgSFv9tN$$ z0*-vTPCJd`^aMZvNMKhlXoZ-awBo*ig&aWTagZsVCQy(Lg|^-^j??cz8^qa;vxX-_ z)x@J_k%?|WC^*_D%Xjf^3s0F3Vy(N(XB$jj2S1lMC`J@36Q?>D6Qj>4A)HxA3{LKt zB*Ok~OBXy+2_?+u){!e5#LNx;HHjHcJPsErZ%!X4H_w2+C!=npqT>)wL{8xZWiz`+ zc3=c2#6;Kx=|_e6LUNB}=Og}Xe!scZIh={e@&LXNF0kBAp4AcR)De2A9Z^Q&@LQ_ zv^>>wYH`#ql$w|$Qg}W|6-On?RAH}4LFXTE$qirv$SY+%)VG@3SeiP$u{`S*%pjNP z(g|{Owg}0!V(L`(K32_4F~8#5zP-#=ZTVA-yZd56XB<|P-f=d&SdH?*9)AK#kSf$L)f+61WSV41 zh$u@k3KIm81_~TB+-F#~;e(oajq0B-=4f`R*+8?QwvSG64{#HIQuDR_=!Gtsm}?Jb zMy9FKUOP%e5XagjMwd)!)ec_ka?tCAiGZL#v0S?fWsz! zCSEp0fKy6!Y9=3EB`+0_YMKHHT)o1SjIeE0D$KWCw#q8_y<*F@{53}BaJP(sX?OJ} zJml7EZ^TCS$Qik#(x^Q03=&IuIrnmx`m2$j`{=Py;)L_9{n{BVjVhyR&T>0<@}<0z zS36~sKJQP`=cMD3REtfJ#!xAW7P<^R*yh7+esqExPe*t>LQe!3OkN(CIL89EGa-WM=s${Qf85qDQe5|Ht|0r z@^d1b$Y(@;L4;Zp`AZPZB!La^EE&bM1H~7Sv|=!((xUT9AXb#u$_9$-H6}mD$p1u6 zc@{a!ddmdM-5l8Ipu7St4)Tac+3dAD^5<|<&3(d7$0Otn&rKx04>l7>pdnp|#vEcU z#rTH-W6oV+{RMRHehfO7cKsgk6uId_+XZwRsml@86?2eHW2!y0!qlhMmK&>++)(Ja z828^&wC@tRM}$6<7GO|7Vwlsd3oX*(O_O@^=`ivBxzgKPB#p_L@zR zwwg`7(rmUSpMH3(G@C&mAB7UUi{|lAU(Z@mZ%{`MsG=aQD~YcXs^yYC2Mn0?CrDRy zBgJ@%9(J=nLn+kkO+;LcPDR$Ljeh?i?GI(tJyv|@L4)t#-TU;;{@wjO{wNO5qBs1p5-ol| zj`<<=$@pQIh4M`(T1B<&aRG;nFB+_H4^tJWC`}%?oef9dGtZgW->?gISgdqp+Vg4i3OC67kit<^RZ2V9 zusmCTsg^@LSARx;zBlALrhK=zdz|*d-C>aIejaA(Ns#y@x^@rKSE-8H*>h~==jqul zp*aJVi#3{Ye>xG)oRQTbjO&}M{=UlW_ccpy;=N5SOfPc6#;w748;W3Kfbmbp(G38N zDI+((!Lj_v_{;A-s684=`_JG{marX7yBM!ND#9OTvX~_}4-Q-a#Z1FH?AD2+% zvw1pc9H*xM0RRHO`awHH@1z|M1U%#bDUX9p@ic*fbRY`j9pgCp7OX*x%{cQM87d#I z1&efKOF_{oT=IM$-*)hpxe&&t&0My{BimzAvL*q@p(=HW7Tn3C6m2pZq`z zPKS=L4$@Bw^X13`8oL|310qt3aX) zkcG8ovHSuIN3N3J%Ps=8dL#!_{y(JsmAGp{$uM7Ju05D?q#4NKo?3 z&Y8zir!;Enj!3cdNvb%qP^Lm#={Y;R?@>0W()ThN2Htf-uV zpR;91Zj@bTWA9#P?qkP1P06}JZKIg~QP zy(Ma4=O*Q_Ex`Y;I(Kr6V8#j0>3FP;{;4DII1L2XPL+rY68_@JW8R74DFxk^-Rxi% zE&7lUvUU*4#sQy4*>oVWB66oFp{F;jOuG{{yPS>q;DA4dBuJHJ=*$fjM>0(^5F%2N zjKTy#q=pg)p8XW_dM>2y=dFHu*{5J<1sfJ@RG24cxQDn&KW(^$`S5vB(J}WNoEe#> zs&G0{B7!(BY*LDfPOF85&qX!p_rpYBpgZ;4vlUW%Vbf5rqHQj0dU!76#F{ya0Ze@K zMSxREnQo2`uhTA-kn(l%1g>77OST1J)N0H%ZMM#8_`P6tQ{F-A0`4f?%_^%u<0Us< zI%76A$JW>$SH{(`qmfw2tGScgR9lVR+(mtb6eq0j%$L@9Wn3Gt<}5dJD__ZL`D(YS z>GR%{KBv`|q&%#HG=WN)v`}I2VUv%V{Nxlj-cIm(f_exrn0`Fcahxws$~bCVJ)Ko! zB{FEg$}CB!_^d3%GJa<^+m)sCG+Hb`Obz-mX4;uqD3RZqydSCdF^mpQ8~hY?7r;E} z(coYvO_-BNYO{B)4ht}d`ptW^P`XialoXmrlE|T9$rU^#F^eRoXw+qs^q-UXgv2jM zP(dPpNrIF36-2=#ferAiXu}H&hA$&&O=C=@W!05nSeagHTS%@qnA}Ip|3pr89y!W* z%>>Ks9N6h1y#g&3c*G$#dufl|1>CIWE@7wZ5OPKrIubtwn<*qvj4nlE0Wnu%{NsSp z=dSer5;~uL2s&3*{Q>Zl+;nN}61w%Ms}c5U`XHA@se>|@YSc#ETc2`6Y2$L-cPZL; zN$ioJdS?j+B{Y@@WKFN*BkmOcF-A?ozR9-PCX>HKOM^XXF!?jmt`&~&i?r?g#hUN8 zr+@qKTJ!y2fIo!N_dOfM Date: Wed, 18 Dec 2024 01:12:22 +0800 Subject: [PATCH 03/24] update --- Examples/move_arm_to_home.py | 68 +++++++++--------- Install/basic_environment.yaml | 2 +- Install/pythonpath.sh | 11 +++ .../Bestman_Real_Xarm.cpython-38.pyc | Bin 0 -> 22984 bytes Robotics_API/__pycache__/Pose.cpython-38.pyc | Bin 0 -> 3496 bytes .../__pycache__/__init__.cpython-38.pyc | Bin 0 -> 212 bytes 6 files changed, 46 insertions(+), 35 deletions(-) create mode 100755 Install/pythonpath.sh create mode 100644 Robotics_API/__pycache__/Bestman_Real_Xarm.cpython-38.pyc create mode 100644 Robotics_API/__pycache__/Pose.cpython-38.pyc create mode 100644 Robotics_API/__pycache__/__init__.cpython-38.pyc diff --git a/Examples/move_arm_to_home.py b/Examples/move_arm_to_home.py index 715ca7a..773f038 100644 --- a/Examples/move_arm_to_home.py +++ b/Examples/move_arm_to_home.py @@ -1,53 +1,53 @@ -''' -Run this script using: +# !/usr/bin/env python +# -*- encoding: utf-8 -*- +""" +# @FileName : move_arm_to_home.py +# @Time : 2024-12-01 15:21:45 +# @Author : Yan +# @Email : yding25@binghamton.edu +# @Description : Move arm to default home position +# @Usage : python move_arm_to_home.py 192.168.1.208 20 +""" + +import argparse +import time +from Robotics_API import Bestman_Real_Flexiv +import flexivrdk -python3 move_arm_to_home.py 192.168.1.208 -''' - -import sys -import os -parent_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), '..')) -sys.path.append(os.path.join(parent_dir, 'RoboticsToolBox')) -# import pyRobotiqGripper -from Bestman_real_xarm6 import * -from time import time, sleep def main(): # Parse Arguments - argparser = argparse.ArgumentParser(description="Xarm6.") + argparser = argparse.ArgumentParser( + description="Move the robot arm to follow a trajectory." + ) # Required arguments argparser.add_argument("robot_ip", help="IP address of the robot server") - # argparser.add_argument("local_ip", help="IP address of this PC") - # argparser.add_argument("frequency", type=int, help="Command frequency, 1 to 200 [Hz]") - # Optional arguments - # argparser.add_argument("--hold", action="store_true", help="Robot holds current joint positions, otherwise do a sine-sweep") + argparser.add_argument("local_ip", help="IP address of this PC") + argparser.add_argument("frequency", type=int, help="Command frequency, 1 to 200 [Hz]") args = argparser.parse_args() + # Validate the frequency argument + frequency = args.frequency + assert 1 <= frequency <= 200, "Invalid input" + + # Initialize logging + log = flexivrdk.Log() + try: # Instantiate the robot interface - bestman = Bestman_Real_Xarm6(args.robot_ip, None, None) - - # Clear fault on the robot server if any - # if bestman.robot.isFault(): - # log.warn("Fault occurred on the robot server, trying to clear ...") - # bestman.robot.clearFault() - # time.sleep(2) - # if bestman.robot.isFault(): - # log.error("Fault cannot be cleared, exiting ...") - # return - # log.info("Fault on the robot server is cleared") - - # Enable the robot, ensuring the E-stop is released before enabling - # log.info("Enabling robot ...") - # bestman.robot.enable() + bestman = Bestman_Real_Flexiv(args.robot_ip, args.local_ip, args.frequency) + + # Initialize robot + if not bestman.initialize_robot(): + return # Exit if initialization fails # Go back to home pose bestman.go_home() - sleep(1) + time.sleep(10) except Exception as e: # Log any exceptions that occur - print(str(e)) + log.error(str(e)) if __name__ == "__main__": diff --git a/Install/basic_environment.yaml b/Install/basic_environment.yaml index 4b39528..83aed21 100644 --- a/Install/basic_environment.yaml +++ b/Install/basic_environment.yaml @@ -1,4 +1,4 @@ -name: flexiv +name: xarm channels: - defaults dependencies: diff --git a/Install/pythonpath.sh b/Install/pythonpath.sh new file mode 100755 index 0000000..430db39 --- /dev/null +++ b/Install/pythonpath.sh @@ -0,0 +1,11 @@ +#!/bin/bash + +# Add this project to the python search path +Install_dir=$(dirname "$(readlink -f "$0")") +project_dir=$(dirname "$Install_dir") +if [ -z "$PYTHONPATH" ] || ! echo "$PYTHONPATH" | grep -q "$project_dir"; then + PYTHONPATH="\$PYTHONPATH:$project_dir" +fi + +echo "export PYTHONPATH=$PYTHONPATH" >> ~/.bashrc + diff --git a/Robotics_API/__pycache__/Bestman_Real_Xarm.cpython-38.pyc b/Robotics_API/__pycache__/Bestman_Real_Xarm.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..c28273a256cd51d37d89c60b36e1e0c9a9f8b378 GIT binary patch literal 22984 zcmeHvTaX;rd0zM2XZC`{VgV2&O|?jn*r9-302By$%^L_{L5SoM0u~fFCNt@s(=$7Z zxuLrUz)tNdu|eBPs-Pr}BS(^D1GXzJYYTN+ngQk#43-X_Ix*jZ8CJ%E@)2kuT-(EYm17r%F@uEZdlF z&Xi{4I@g$O?kVjtjSGg#tHS4oD%?qw=2YT}QJm`i*TPHIJFE4Evt~CPOMh0ZcOB1f z+O4wd*p2dS+ikwS)Y)du^?Fo`byuvD$4`Fs*ol+JPMo$*o?PMo*I4gN*WYZr0X)_# zZQ8f%)+^R0cFSUg^G&~G-FDMnvbXJ4?VTE{ELGY~G}~77R_)}ecQ){~ zX*d0LYspbvfppICDsH{w*V`>Uq-)o%v4^XkT?=O%%zJJHjjVOxOoSE-U>iE1~$&%rj?Q^ zp^~2)*tnER<2$V~DvR%o`i5+RS<0#@HH{}ZHKS(nomYF*9KH)`ubRjAl-j2j@I9^e zs{{C+Q3usC_?}gV)U){Bqn=aG<9km1f_eepd)1eeh3|QFS-q%U`rIt-Q&-es^)jv& z)GO);-tAY5s)+9c>Q(g>d>>TnYFQmejc3#ebrMewsn^t3@%^kirC!JPbLzDE8or-b zzo=I5{R`@s)EoGILA|No!uOZdRrQWK0|=J-Se;epaP?*NO?5%NhpQLWMfG*mdP!YV z@2d~+MYb>J#;mx`ZdMs4oK%Oa1u}=PncHt`X<#|A<8`a8u7%m4@wkmfyDO zE#GmgcEz!7)%{J&-*hZ)&g+)j-e~*viiMU-1>MT`-TFq?cf1w3CnTx0Vy#Ebu&Y*k zW6P=d0A2U%cB9^N)Bt$LZQrabJh^zuvXycj&s&P1RbA&s*J)L@HN;B0<-6^M6+O19 zAOa3ix*fD@v}-Gv6uxSX8+MP$;epje55Qbc+A3$GTdM)-ExVI63Ar_ z_={QRJyL~w0t~6U8`N4G_Yn{IV;u`w88*5@UT(4iHa0-OyU>Z)fisf#*8cHPZN&mq3Vd9SYtENAHZy%on>Vitwz+Ul}T`&$BygO*@W_B{@ZK1qh zkNLO|a=1^z)oD zK?$?He;=XO#RwtwX50p%Yg7_+aGeHrQelmoopuXb3(Sn#o^fk2w2x4~T<=(mp6?d* zjt!7HM2JBjgH2}Z0ZuPg8*SSMfU{4F?rSa?Ztu|2(()P4bNpoyDP>I*T~}3m`^jvr zD&&yWYWqOlZcD9LtLqOlXjozE@RPs=G2{yVyd@Mhto>)N?3%Q#KJ8s=;0<9~>WWP}N>MhVltKzuqwN%(@gV~h* z?T+&x(N8QLFQw(l1H+xc`=7qOyxDF#%aB#e7r=_T8_Q&2A3|)Bdo5qq{KzYV!^j&7 z)QGu3p7slGH0qwu-FxdCXG2{flSr5avtShdM{m5*>r3S_nN+#F^hTpyu^Zl7xCmkN z4xhi>0bv0nUbpOuU%%;CGGOnh1um!;)#@Exvd(TgmFq%|gJmaw(T&O=*6UumX?xd~ zdZ&mF0duvkgRjVPSrrON%j;Gu;8xXcqp=<5o=d$2?r<4X!=zOiY;DQ!HjB@@r0;%O zHolY%*hN1d;Z{GRSzmLpH+3Y1x==( zwKqI@k}H>qR^@U(#l`4noZDD1ufJE6p0>YAR6FmwZPz7Qm1e7MyICeuO1u7)?nx%x z-#fNK{j{uBeCKt@v#5BIA zlLyRPGLy*sRW3JcCd|1n*=`GuV%o{cCF_dg`=RL7 zy=dtVT_fJtA{2eKT^w-y*Oshr+R%nDuWrkSrXDFV)`kPJhISxLV$-|q_+7U(B6Bv{ z?FJ-`+jXpZOyRIF*T*rBqt+@I8x$0D>E5b)F=eCol5j4WICP6(#j%*t=$4k2TuO-E z%i+xRtViZ6i@>^+EDwp&y+fDDuQ{+*`&N8$mezZ(#@_kyca~j+Hfo(XZh2UhmhzTX zdl)|WjncYms89|@xMHmpXWf11(tU-+910LScDGe_$c9P_{cmiG=-xS zx)ePq(Yq4qGFLl_>a8qdsF236*&`iR9lx>}pn4P93e3gQaLcFeD6#&tqHDgm{Q>Q3Kvl37g{#dOor;5SOGI|Z?<75aUI6qpIp&endLjSU@U9!&swavZo>Rj z#n)D>itPg9>+qI@)rLCxY6#?}1GS;2^^!<*vvbin( zr)AGaa8zpiCTe+NsqV>{6E!9xKf4@4NE!;%c?BWg!^MvIX?8cUo7_$9rgt;D+1;EE ztq8Vcb|<%!uBBk3=6CY2U*HH-$-9Y9le>l8som)vTE3~B!p_vrbS(q&PuXj61cohk z({cU4IwX~A9%x&%53FClebm}M3gzFraSMOAyMfyqz0b%d!k3?k@^Zf0fJlRoX*h9v zauS8Gn-$#w+zF>GF$_KHSOjsby8#M}sSAF$(*U%^XryF0M=fDF7`%O}Xx)NF05S<` z6e+C9<0F!!4YN4j%tj;Di_Z*v(Q<7C0*fh3{p9WK-s%H7=bW1BI37^Nyj+5P0+ZlI z*Y-;}+bcU*AYSM~ z6!;lJDf?{=`w#Kbn?_+2=FOB@c$9($-;12Gkqrr_Y`obQB3>qrc)pf2X`)_D+?98e zySbg*y~KUv)6{N$2<@hK@0!}&f0;(i;y!FA1xHKKTmyro+N4FiU+KW4w^gKIcI<85mv1@s+NNKc3hyiJ+UVf5j$m&K zfIkLEtwkkzUz#A|MV|=j4k?fva5U&waibN;`%oaobOcMqb#gc1r~LE|rg8^n-)?dz z$;tG76yYzJkXwg<(R@T~cWZ+V0hnTOa|QZT3^m7C7g0l_5PZ61w`@#yMk}cUD-XW8 zKs60yzwYE{z|p7(=msLj;abrHYC?HycALVMn+T8a8X7_uaOqpMHL!s?SIlUUM#G|% zy78fXns1D+m4DUWV~%PCm`eKWdqbI_Q}^1iGq{3&-&FfU8f zV0N-0FxqNpA882iPmqrr8mRui^_U&fY?;domyx5O<1e23Xtnq=Zx)2V_J5889N%~D zExw59M|MUkOOD}=AV-j$BUxFt2q-Mpm`z}_! zpLbf_CIcPNfN<}=$cHHoFrXXv6FxEPq^Q>QQ4-lNYGptqtN^!Mwf$#k;?Wo}vN;&e z33K+*bn;nTh1b2s3A}G2s4?P+m>@Z~4>a);V11g&+5jp;G))X|j^t#17Xk)BC;ntYgA8^xFtV1m;DEe38@QXBM(Rv8Mza zF6H_Iv!AbfWv%=RyAaQeaQ6pV%_+rXo%q>c2xxVGCLnJ8v=rWIYiX1nJj3M;-S}E^ z>WWj`7V>)|J8Lc^dS979|Bq|?*Vrc6s*!E_qo>-Yz+F%u@QI(}Ry`4Tv|Vl)%QUi0 zu8n8Gmbe?ZF6|BWWdL(@GoBw`&_U&c(S{%{f(@AzdV5m)wF-^=@Wbm!njgoF)=r}! ziE%H%5Va18Y$Zud2t8+1`fdseH;wzO{f8J)v_*99ksH%kw=EArzIqjz4)kb%XT&M$ zC>2N#`3e$}=xlC#^-A1|AD6yY0EC=c`>X^%kIDlkM%=158g!b}tJ|p1s4gK3!!LI| zN0l`uYjrdlDE&Gs{~KI6%BdcsF(88sy2fzu434&PU9}MYJ|-iQE3A;CpEYmVm5MxT z-lQvCE+uY`=xCL3hGP*gkazTI>gTN9?)tg~JOy1(F0aGmUh|?EYo!pIWvpvwYV|+A z{UFs(%V6BUjpuOHb2#o>xDH*V;!@K7{5>P{DrPmOnT|7ze!)ktSOB1Gbd8zyvmc(h zR{r?>2m0d7*|ViRoIx4#7jec`-P;_cgJ+sRrZSL;24uXC!=wQkdB!}{`9#JHJg|=@ zvZu#EsR&ym6nv;KN8Yb3+%Sh@%Xp626Jny#sf{O4TVbAMcc|393nv6KBQp05_iKKZ z>aWg~*n)FHC3nOvPD1hXP;N?n}d-K0}OC*GPKBU)sAY45-NvJf(0~wGOVW?T8$`Z7=Q70 z?<=c}&06h>2xDczHqmm7{KIjNGM*|1Izoc$u7gIs16QEo1v?jdagblNR-Bgbyn7mz zw7$y7QQy2FmUk&z_oOfP1mAD6c$dW>7?Ria_uetgaY-QVbrua4O%$aJ9N+b}DkWi9 zmO>_g@r+XS^ASB(;c#o;8lvnsN!iZ>oH1XJ$Xzah@8p2OGs)THM49&_s6&bf^=UFe z(HEz2K|0?OO;N0nKZqFSuoV(2Z%|m{=sT_n$J>wV(qqn-m|pmWph;o;1U|0PJR;i- za(_{yg?R_@0hcH>y7c?wiUAdGX;I~x(>kNkH_^bG*lcJ~setoO7WM16(2L5@z(5J7 z;w_4J`ZycZ248E21^cZL4NF&+JgH-TfawWJYjx??KM}4f1D3}lb3zqyL z@j1i?v8X~G5c)jR;(T>4px!7#EW~%#ajH_YLwq1%7=>BT;GF#H9r*&qKWshbv8CHh z(Bcv5UoP2NKJJ^%++>ONSmi$`HTjqj5+3 zRLVz7K8o^MsU)^SsU!~!H~CkH_Tu}RqK_DvEo#{vBd4 z6)r>g;WcSXDB?X_Xhx}VTO}3(><_N!wb0MvRFDNNAjYb4$PLclEr1dVV3Sk5A4e$k zG-MF6i6@dkt&KcC;jN38z|4YF+)!JT7l8nb^F?Zol|~oU5TZcld)Sq5HytC;QJxsV z6cIl~W)ZkJY_>q{6!j~pgKYBFK$p{4#WmLUc7$5)O+M?QST7d7Ac{p1*-wkzCFW0H z_h`q8`wrg1*?I(DrF^swTCQ&6q0}D5uKl8h1o#j&`hJ93Ip#U?pGK|(T&(!_-zG}k ziJ*pj@kFM`dSAy4Ocmq4IJg*9ViZZ4X@jiuG{o~8K&D-!qBHd|vhWl4m|$+|M98f) zTBLwhI0PaVKBK6)-an5JOHQBY5cSZ8k7_|obFxG8A9jV35KZ9i3MceV$~6k^wk?i=j1TScwX!X!^v zXv zPapWkx1r14F_FvvtdGnLIUkU`my+k{t;`*y740T|!>CHm#^*WK=1BmAW?<7wsAd5{ z7G!+vAW*|Rjzd738}FfB`d$ukR+Fa*v?o=itMsO3qkHkM%CnrGzgO5nmMb$v?o9y| z3p)jo`;O7S=>R|`bSJeg==0OESx){10syUQ;Kcvu;f{0)H zh+og5kh2T(yCyIJIHMEP^f?9Lfe^+PYUwak8nG4UwmZcKgYXFB7w#SuKO#9$1)y2i zTbZC($_%J#43v{e#|YL$n;6kBVD=tnPcN}}8AU(Q>Xgzw$Av9w@!^XsuAwN+V^iTI zQ|0KmfG4REr7203z>>N3+wMoG3aFF>Zk^5c@)*I8-CA0RC)FdCCJ`&Q#M(MiU6%bQ z%?-3I4ZWWWx4bkT+i#p{kiI`4gaQ+K9OO*5Tm7k6B`tkqg(a@LZ^F9$2^X6)Hm2q& zYaXT2^N>HON14=|c+VqC$b&G*sum0!d&+L*w(`73N|_u@yqgjhgriIh=74Dw zZ09MN#$=icxQC`1%~4{cJ#H3+`xP+^Qr`e{^m{+=DcVx5P6Kg(jUAun=P~5+ZqO>PeE`t6_Q^7p2 zpH68Z$W0z1dSM2m8sNSGfrnme@6Snt+O(Jhtu2^yi8=VRQd;}_%S?76GJHIY;v$DgVRx-wRrLUq9vL5qHV*8M}bSk=+rCM5vfqe zkW7m+tT-N3?+{d&FM+HHeHgWi(`Yrz4jh%@5{|HQoP0v9Sd8Z|wMBcM`D$P+gaTLZ zL(thU5ecjqDop?vdn*$pu0ICiS}4gx=bsrGDB`R%g<{Y~H+~)#ylIZyyPgOd^9-h*TKd zNs0|oL(n7L`_pJ|uHZOOWi#+{3lGpIWa)IaZ{lbOjs{7z4d*SeEpBjR$knH31flqp z$7SRcWQ=Q0WJ*yQ0esmZ$RMpO-aArd5gBaB@xY_ZuUSLLjGa)62|Cj3$g$UL-a7ynimo~IB24}luUAl+#;sstX&-%I;ZyPx;JJ9j13yDQ#k!>w90u;_ z-tR>KOGS-&pzsj4o{=LH!&|s|b5EhWcBOe;g6O9Ft0Vh5*2vqEJ z4JU!il%_(g2AomMJru|K zupjvyjkh^-iWMVONc4_Qz}v?*6}5cGTqT@{mA!x)&U}l>IVR!*WMa_e7z+q8UsLvR zRF4({dE{DG0rO@)OIMP?f-MT^evmZ1B1jroMCEoBs8EA#$0IW_Gbws_)H<d?kpFVf|3nAf{&C@|z z7Jw(d5Fifa{7e8f+>?U^fk_} zZZ_Hwix)LqJ#+Pg^`My!dxRh_eRSpG`o)jdtjp)WarOL_b?f}u_dXh${rHfum=WZt zJ&oyT$T#AnQfNkN&C~(#XM}Cg+T8Gq?gV0kWcCbTcG}8B=qY&kB zh-lDFG%)=9%f1PyDvaUbU*Vu!7$=$!XGap&1QlkD{sRzaKckDlP-ynhfoF5XAtl5 zH;2;Tq8a*l7%KgWTsK{CD`HcpkbqCp-N04GnK8m(1n zz*ZSdW*ELqhn{&3_6qZSLU9tV2_4SFUHk^a=uk(-3e_AIK>&-&mfh(z>J`x!skT!C z8fVMn9OW1mqlx3hJZ#P%X3!HD9``W|OEpxBi!rFPD(JmlO!t%6MeUzw0#iR5q(zmc z>@G|(IqB|%$3sdB)CWe9Hz0z3T6R`o_0uxhQMtJ{Y+7prHs||kq$$is=?X^T(L{}J ztntG~7_o#chO&}~?)>8c8!s+4t8lQ$DZ8{Xe+LP9zn%CFbEp&czs4vcesBS%y|(U$ z@f%FXp^XO=&{#qKDASCgdmu#7GIo;m>2MpUxrCkFa6;iYguW5>4&KYYNBBM1+h7bb z$dwSGf-w){D7uWM9kh}O4O+?V2xd=c#hkPiS|u<^kQm;!_<)Ne_Ks#wu#fbKS%`rq zWQGrU5z`&`8v>()32+es_JM?@NbhMNXYI#K@m!>A$2~)>d>3>TdZAUG2FPH(UIvAx!$9z~ zNXK%#dk%E`2rg57!21#0HIF9h{ga>!>HRd}=u8*MMbMO7k<3bJQ@xn(UHAx`ZA?xv zv=L%WIAblR!l$D|T)zzE+5N}pXzfAL{Vu)-tAZn=)A0dMG==t(U?c8G?)Ix(75u?~ znaI(`8Vp6fsRu-SjOkrkbXV#A6+R|hEFE#NFUPqU7bQk~H$pxzP*?viWfM0-E&(p` zH;jTT^yjMWy59#Pg?vH($`bT-BI1LL4%SRy1?~^fXp9dGVQ;_(ew`RENya!v|AjnY z5`@1N4|wk^mAJ`FAUI?BbWA{p#e@mCKjfr;m&GK<$GnK&A#(p-jNB)BE;-1d7XM51 z{$iZoWdZ(%;3X;S37P@?75|@U215iNx%mWn-Fp+rdjBw92Xy_z$I|trvyBr$6ip$n zVtS3AVoJ)SqueWf!MdoMv%`1(gl=Ll(@wbT6~G%Hm@d{Ou(7GZue~MTLc8 z(PF`1u6u)p&*Bz~9*Z3ozry0zS^Nfzzt7?>i|?@bO&0%@#qY5AJr?&_{6336U@=IV ziCnNQ{oIU`$&U=@%+z%5bnZwlQ^@D$b9-}pa=F}eZYH;o%j4g4ZVs+${qHA*=Yy-C z7K*t7>K-cW&mBbD7jm<>+n>u7_7~t#kk6ySLXPSfKaUFg2G`Pmaj*MFc;Rv!k_=x; z!>59*p>Od46f67~tyigcwwL&;eTZW$NiHbFThpbBLPsxujr9r)PcBJVRR3aMULMGK wT&hH+X{uIBw_KRd2r5uRk|9i5n||&MjR0?P2%?~-3OtyL-?zd)T}~(eCx{o()c^nh literal 0 HcmV?d00001 diff --git a/Robotics_API/__pycache__/Pose.cpython-38.pyc b/Robotics_API/__pycache__/Pose.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..470f2d72e7a160e26c96d89222d44b6bf16fdf73 GIT binary patch literal 3496 zcmbVP&2JmW72nxilFJn((XQjviJMLupfU@Yk|nn_2*I@*+ksnDk?r&YRE*7vGm@5C z?rvs=sI*`%m9GYxLr+3b>F8_zfV6)`G1s1S?#&nXy&<`z6ctFizzpZ(&71dr?`!z` z*;(I$ul{AY^N%^p`ZrFdKL;lFpvj*>$E>!+2(x3-w&B-_ou1n+5$hw1xvccUVx>d7 zU1s((t5MGX?teh*uZI zJFV3_NO!-lc2hCpfOH3YxcsmeMscx<%VDy!*N<-9TKVO@9o%eo(jHPi;Ibp4OhswZ zG8b)pEV+LQ3R&w7qP?f73URZ8E7M=0{B2m}R+|6-pm^J1HgjHBZFi`4yToQ$g?TRs zC_l%1HUpyys{su2+C`1?4Id@FPdjlaB^5jqT=GOk$qo(a>I0eq5$&XjjHFpiw{a;# zsc4umnu-X>jg(Eps)MqPKNkiiXdgn8-$19V0XZZCD&*eB!*2TnT2GbffgT}v`UKdUS}v+cksrRE0qAvgh?Q$P)4;9E-` zeT(wz5v2o^U#n_&p1A{?m008H*3I)v7uHc+* z$7!g}SxN2k3A-pko5?Kf@S!#EGW@2@M9rOxRQ0o%FJ5$Wu~$fN#xL;3X@39nQoQ)F zqMb0ITb%L;4hs6PA43lj5pzjztWqBH9(a&!ECPm~_d~@+g3fj0_VgA?LRa@!sThR> z?S)E2`x7l2_NfC7CeY>>uQhUJ+ekdV>ySdzJ z(T^vNOrs?0E86RWX+ZFC6ppS0Ydx8WxpYRM^~NRbqVcphHnArAnjC17Yd?}vB2}1l zxVDo_S4I}sCAk}BT$87IW?Q7aV5pj|gfc)t^vo36x@7F8otP)uH{=`<)&3vCxX&M= zLJe2DlE>QuGpKkAI!(kYOI(N1>y@S5w8xhqCoO#h7U^#-{hCYlyD$m<6pG%`({w9U zQAY;%Hy$q`Qjin17X%TwdJrHl`BUgD-zRnFD#5<~#wPw7+o=_!I;q<>vESsghQs)D z2V$rAaE$FkXtE65!0K8MVkG}<5h^=e6}~+Qr>9)?MRG2p5cJlB7+nv)1~DMO&pU91 zL15VZ2&TFQ7X)KN{Q?ECp|dLaB_sFruJ$Ls*)dc|2_KHp-HaX^Fc=V%y5@%Zzzh@8 zq9VtT>blkm={n58^!yU45$u*>_rYZM*gXLhRB1ZtCb}-yvMoLB@+{S(xCout*oo4KRQJUO2~lVZBi{-YG8Q)m7un z#^F8i{GnG@VKfW}aSezXwy~pj@yheooJV_2Df=rpAd$6I@kp6?6@g9s$xb z8D|`+&wy55fo=z|dS3eio+8XTa=)}ccb{3Wh`@vT?3wwudhW;~zD9R3<69C*fM zcmIg&Fawk!7v%35$1(~w*W$Dj#_}!<#>B75bMZdVet*Hl%|eW3F~W>4FqMgA=r*Ah zl8B$+GRpNy!`4-J*L9%&q>1)qKf_m^%9QL+Yc=t0n_#bFie*I#UhGg*at!}3R!XDlN;sc;xZ+N<7Fg4j+ zyWKQO#0}UH^U&$K#5|5pm72%2)HU;RT9NQZNOp#)!!fC&UC*GX1#I&277NJ7AUT2a z4y-+aCMk55>k*r5yeT{5U!TkkS6ox({E0g7Za1zBnW_ONhhq;bXafH>f*_Sm!w#IsXN%rE*^Y literal 0 HcmV?d00001 diff --git a/Robotics_API/__pycache__/__init__.cpython-38.pyc b/Robotics_API/__pycache__/__init__.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..87cf5317a428e352f8b8992014ab08b039a71aff GIT binary patch literal 212 zcmWIL<>g`kg5-&b>Fz-KF^GcAU0DDV=iM9BaqD; z%%I8gk`bsxlkt|IQ)+QZZem`1P-MsqAip@(Pm`$#WMC09kXXr3!~$Z2 zh+k&<8Tq-X`jv@!`tF$}8Kp`3AVYkChJf_z2jwT_mt-av$2$gi>c_`t=4F<|$LkeT W-r}&y%}*)KNwovnT?{gthY Date: Wed, 18 Dec 2024 19:58:53 +0800 Subject: [PATCH 04/24] Fix nested git repository or add submodule --- Examples/close_gripper.py | 52 - Examples/close_gripper_robotiq.py | 42 + Examples/display_states.py | 91 +- Examples/move_arm_follow_joint_angles.py | 90 -- Examples/move_arm_to_follow_trajectory.py | 108 -- Examples/move_arm_to_home.py | 25 +- Examples/move_arm_to_joint_angles.py | 63 - Examples/move_arm_to_joint_values.py | 44 + Examples/move_eef_to_pose.py | 45 + Examples/open_gripper.py | 50 - Examples/open_gripper_robotiq.py | 42 + Examples/open_gripper_width.py | 50 - Examples/teach_by_demonstration.py | 192 --- Examples/test.py | 68 + Examples/xarm_command_log.log | 1124 +++++++++++++++++ Install/xArm-Python-SDK | 1 + Robotics_API/Bestman_Real_Xarm.py | 686 ---------- Robotics_API/Bestman_Real_Xarm6.py | 808 ++++++++++++ Robotics_API/Utils.py | 166 +-- Robotics_API/__init__.py | 2 +- .../Bestman_Real_Xarm6.cpython-38.pyc | Bin 0 -> 23465 bytes .../__pycache__/__init__.cpython-38.pyc | Bin 212 -> 213 bytes 22 files changed, 2213 insertions(+), 1536 deletions(-) delete mode 100644 Examples/close_gripper.py create mode 100644 Examples/close_gripper_robotiq.py delete mode 100644 Examples/move_arm_follow_joint_angles.py delete mode 100644 Examples/move_arm_to_follow_trajectory.py delete mode 100644 Examples/move_arm_to_joint_angles.py create mode 100644 Examples/move_arm_to_joint_values.py create mode 100644 Examples/move_eef_to_pose.py delete mode 100644 Examples/open_gripper.py create mode 100644 Examples/open_gripper_robotiq.py delete mode 100644 Examples/open_gripper_width.py delete mode 100644 Examples/teach_by_demonstration.py create mode 100644 Examples/test.py create mode 100644 Examples/xarm_command_log.log create mode 160000 Install/xArm-Python-SDK delete mode 100644 Robotics_API/Bestman_Real_Xarm.py create mode 100644 Robotics_API/Bestman_Real_Xarm6.py create mode 100644 Robotics_API/__pycache__/Bestman_Real_Xarm6.cpython-38.pyc diff --git a/Examples/close_gripper.py b/Examples/close_gripper.py deleted file mode 100644 index 64d38df..0000000 --- a/Examples/close_gripper.py +++ /dev/null @@ -1,52 +0,0 @@ -''' -Run this script using: - -python3 close_gripper.py 192.168.1.208 -''' - -import sys -import os -parent_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), '..')) -sys.path.append(os.path.join(parent_dir, 'RoboticsToolBox')) -# import pyRobotiqGripper -from Bestman_real_xarm6 import * -from time import time, sleep -import numpy as np -def main(): - # Parse Arguments - argparser = argparse.ArgumentParser(description="Xarm6.") - # Required arguments - argparser.add_argument("robot_ip", help="IP address of the robot server") - # argparser.add_argument("local_ip", help="IP address of this PC") - # argparser.add_argument("frequency", type=int, help="Command frequency, 1 to 200 [Hz]") - # Optional arguments - # argparser.add_argument("--hold", action="store_true", help="Robot holds current joint positions, otherwise do a sine-sweep") - args = argparser.parse_args() - - try: - # Instantiate the robot interface - bestman = Bestman_Real_Xarm6(args.robot_ip, None, None) - - # Clear fault on the robot server if any - # if bestman.robot.isFault(): - # log.warn("Fault occurred on the robot server, trying to clear ...") - # bestman.robot.clearFault() - # time.sleep(2) - # if bestman.robot.isFault(): - # log.error("Fault cannot be cleared, exiting ...") - # return - # log.info("Fault on the robot server is cleared") - - # Close gripper - bestman.find_gripper() - bestman.close_gripper() - print(bestman.get_current_end_effector_pose()) - - - except Exception as e: - # Log any exceptions that occur - print(str(e)) - - -if __name__ == "__main__": - main() diff --git a/Examples/close_gripper_robotiq.py b/Examples/close_gripper_robotiq.py new file mode 100644 index 0000000..eadebd4 --- /dev/null +++ b/Examples/close_gripper_robotiq.py @@ -0,0 +1,42 @@ +# !/usr/bin/env python +# -*- encoding: utf-8 -*- +""" +# @FileName : open_gripper_robotiq.py +# @Time : 2024-12-01 15:21:45 +# @Author : Zhaxi & Yan +# @Email : zhaxizhuoma.ayang@gmail.com & yding25@binghamton.edu +# @Description : Open robotiq gripper +# @Usage : python close_gripper_robotiq.py 192.168.1.208 +""" + +import argparse +import time +from Robotics_API import Bestman_Real_Xarm6 + +def main(): + # Parse Arguments + argparser = argparse.ArgumentParser( + description="Move the robot arm to follow a trajectory." + ) + # Required arguments + argparser.add_argument("robot_ip", help="IP address of the robot server") + args = argparser.parse_args() + + try: + # Instantiate the robot interface + bestman = Bestman_Real_Xarm6(args.robot_ip) + + # Initialize robot + if not bestman.initialize_robot(): + return # Exit if initialization fails + + bestman.close_gripper_robotiq() + time.sleep(1) + + except Exception as e: + # Log any exceptions that occur + print(str(e)) + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/Examples/display_states.py b/Examples/display_states.py index 02f6b27..0bc6075 100644 --- a/Examples/display_states.py +++ b/Examples/display_states.py @@ -1,72 +1,41 @@ -''' -Run this script using: -python /home/$(whoami)/BestMan_Flexiv/Examples/display_states.py 192.168.2.100 192.168.2.108 -''' - -import sys -import os -parent_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), '..')) -sys.path.append(os.path.join(parent_dir, 'RoboticsToolBox')) -from Bestman_sim_flexiv import * - -def print_robot_states(bestman): - """ - Print robot states data @ 1Hz. - """ - while True: - # Get the latest robot states - bestman.update_robot_states() - robot_states = bestman.robot_states - - # Print all robot states, round all float values to 2 decimals - print("{") - print("q: ", ['%.2f' % i for i in robot_states.q]) - print("theta: ", ['%.2f' % i for i in robot_states.theta]) - print("dq: ", ['%.2f' % i for i in robot_states.dq]) - print("dtheta: ", ['%.2f' % i for i in robot_states.dtheta]) - print("tau: ", ['%.2f' % i for i in robot_states.tau]) - print("tau_des: ", ['%.2f' % i for i in robot_states.tauDes]) - print("tau_dot: ", ['%.2f' % i for i in robot_states.tauDot]) - print("tau_ext: ", ['%.2f' % i for i in robot_states.tauExt]) - print("tcp_pose(quaternion): ", ['%.2f' % i for i in robot_states.tcpPose]) - print("tcp_pose_d: ", ['%.2f' % i for i in robot_states.tcpPoseDes]) - print("tcp_velocity: ", ['%.2f' % i for i in robot_states.tcpVel]) - print("camera_pose: ", ['%.2f' % i for i in robot_states.camPose]) - print("flange_pose: ", ['%.2f' % i for i in robot_states.flangePose]) - print("FT_sensor_raw_reading: ", ['%.2f' % i for i in robot_states.ftSensorRaw]) - print("F_ext_tcp_frame: ", ['%.2f' % i for i in robot_states.extWrenchInTcp]) - print("F_ext_base_frame: ", ['%.2f' % i for i in robot_states.extWrenchInBase]) - print("}" + '\n') - time.sleep(1) +# !/usr/bin/env python +# -*- encoding: utf-8 -*- +""" +# @FileName : move_arm_to_home.py +# @Time : 2024-12-01 15:21:45 +# @Author : Zhaxi & Yan +# @Email : zhaxizhuoma.ayang@gmail.com & yding25@binghamton.edu +# @Description : Display arm state +# @Usage : python display_states.py 192.168.1.208 +""" + +import argparse +import time +from Robotics_API import Bestman_Real_Xarm6, Pose def main(): - # Parse arguments - argparser = argparse.ArgumentParser() + # Parse Arguments + argparser = argparse.ArgumentParser( + description="Move the robot arm to follow a trajectory." + ) + # Required arguments argparser.add_argument("robot_ip", help="IP address of the robot server") - argparser.add_argument("local_ip", help="IP address of this PC") args = argparser.parse_args() - # Instantiate Bestman_Real_Flexiv - bestman = Bestman_Real_Flexiv(args.robot_ip, args.local_ip, frequency=1) - - # Print description - print( - "This tutorial does the very first thing: check connection with the robot server " - "and print received robot states." - ) - try: - # Initialize and clear faults - bestman.clear_fault() + # Instantiate the robot interface + bestman = Bestman_Real_Xarm6(args.robot_ip) - # Print States - print_thread = threading.Thread(target=print_robot_states, args=[bestman]) - print_thread.start() - print_thread.join() + # Initialize robot + if not bestman.initialize_robot(): + return # Exit if initialization fails + + bestman.update_robot_states() except Exception as e: - # Print exception error message - bestman.log.error(str(e)) + # Log any exceptions that occur + print(str(e)) + if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/Examples/move_arm_follow_joint_angles.py b/Examples/move_arm_follow_joint_angles.py deleted file mode 100644 index a791581..0000000 --- a/Examples/move_arm_follow_joint_angles.py +++ /dev/null @@ -1,90 +0,0 @@ -''' -Run this script using: - -python move_arm_follow_joint_angles.py 192.168.2.100 192.168.2.108 20 -''' - -import sys -import os -parent_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), '..')) -sys.path.append(os.path.join(parent_dir, 'RoboticsToolBox')) -import pyRobotiqGripper -from Bestman_sim_flexiv import * - -def main(): - # Parse Arguments - argparser = argparse.ArgumentParser(description="Move the robot arm to follow a trajectory.") - # Required arguments - argparser.add_argument("robot_ip", help="IP address of the robot server") - argparser.add_argument("local_ip", help="IP address of this PC") - argparser.add_argument("frequency", type=int, help="Command frequency, 1 to 200 [Hz]") - # Optional arguments - argparser.add_argument("--hold", action="store_true", help="Robot holds current joint positions, otherwise do a sine-sweep") - args = argparser.parse_args() - - # Validate the frequency argument - frequency = args.frequency - assert 1 <= frequency <= 200, "Invalid input" - - log = flexivrdk.Log() - - try: - # Instantiate the robot interface - bestman = Bestman_Real_Flexiv(args.robot_ip, args.local_ip, args.frequency) - - # Clear fault on the robot server if any - if bestman.robot.isFault(): - log.warn("Fault occurred on the robot server, trying to clear ...") - bestman.robot.clearFault() - time.sleep(2) - if bestman.robot.isFault(): - log.error("Fault cannot be cleared, exiting ...") - return - log.info("Fault on the robot server is cleared") - - # Enable the robot, ensuring the E-stop is released before enabling - log.info("Enabling robot ...") - bestman.robot.enable() - - # Wait for the robot to become operational - seconds_waited = 0 - while not bestman.robot.isOperational(): - time.sleep(1) - seconds_waited += 1 - if seconds_waited == 10: - log.warn( - "Still waiting for robot to become operational. Please " - "check that the robot 1) has no fault, 2) is booted " - "into Auto mode" - ) - - log.info("Robot is now operational") - - # Get and log current joint values and bounds - joint_angles = bestman.get_current_joint_angles() - log.info(f"Current joint values: {joint_angles}") - - joint_bounds = bestman.get_joint_bounds() - log.info(f"Current joint bounds: {joint_bounds}") - - # Define the target trajectory - target_trajectory = [ - [0.4, -0.5, 0, 1.3, 0, 0.5, 0.2], - [0.5, -0.55, 0.1, 1.3, 0, 0.5, 0.2], - [0.55, -0.6, 0.2, 1.3, 0, 0.5, 0.2], - [0.6, -0.7, 0, 1.3, 0, 0.5, 0.2], - [0.7, -0.7, 0, 1.3, 0, 0.5, 0.2], - [0.8, -0.7, 0, 1.3, 0, 0.5, 0.2] - ] - - # Move the arm to follow the target trajectory - bestman.move_arm_follow_joint_angles(target_trajectory) - time.sleep(1) - - except Exception as e: - # Log any exceptions that occur - log.error(str(e)) - - -if __name__ == "__main__": - main() diff --git a/Examples/move_arm_to_follow_trajectory.py b/Examples/move_arm_to_follow_trajectory.py deleted file mode 100644 index 9ae9b95..0000000 --- a/Examples/move_arm_to_follow_trajectory.py +++ /dev/null @@ -1,108 +0,0 @@ -''' -Run this script using: - -python move_arm_to_follow_trajectory.py 192.168.2.100 192.168.2.108 20 -''' - -import sys -import os -parent_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), '..')) -sys.path.append(os.path.join(parent_dir, 'RoboticsToolBox')) -# import pyRobotiqGripper -from Bestman_sim_flexiv import * - -def main(): - # Parse Arguments - argparser = argparse.ArgumentParser(description="Move the robot arm to follow a trajectory.") - # Required arguments - argparser.add_argument("robot_ip", help="IP address of the robot server") - argparser.add_argument("local_ip", help="IP address of this PC") - argparser.add_argument("frequency", type=int, help="Command frequency, 1 to 200 [Hz]") - # Optional arguments - argparser.add_argument("--hold", action="store_true", help="Robot holds current joint positions, otherwise do a sine-sweep") - args = argparser.parse_args() - - # Validate the frequency argument - frequency = args.frequency - assert 1 <= frequency <= 200, "Invalid input" - - log = flexivrdk.Log() - - try: - # Instantiate the robot interface - bestman = Bestman_Real_Flexiv(args.robot_ip, args.local_ip, args.frequency) - - # Clear fault on the robot server if any - if bestman.robot.isFault(): - log.warn("Fault occurred on the robot server, trying to clear ...") - bestman.robot.clearFault() - time.sleep(2) - if bestman.robot.isFault(): - log.error("Fault cannot be cleared, exiting ...") - return - log.info("Fault on the robot server is cleared") - - # Enable the robot, ensuring the E-stop is released before enabling - log.info("Enabling robot ...") - bestman.robot.enable() - - # Wait for the robot to become operational - seconds_waited = 0 - while not bestman.robot.isOperational(): - time.sleep(1) - seconds_waited += 1 - if seconds_waited == 10: - log.warn( - "Still waiting for robot to become operational. Please " - "check that the robot 1) has no fault, 2) is booted " - "into Auto mode" - ) - - log.info("Robot is now operational") - - # Get and log current joint values and bounds - joint_angles = bestman.get_current_joint_angles() - log.info(f"Current joint angles: {joint_angles}") - - joint_bounds = bestman.get_joint_bounds() - log.info(f"Current joint bounds: {joint_bounds}") - - target_trajectory = [ - [0.5729730725288391, -0.05068958178162575, 0.19427451491355896, 0.2969912886619568, -0.6381924748420715, 0.6559267044067383, -0.27251917123794556], - [0.5860132575035095, -0.03786855563521385, 0.23408854007720947, 0.33398041129112244, -0.6333060264587402, 0.6339964866638184, -0.29228243231773376], - [0.5932722687721252, -0.00427872221916914, 0.2615146338939667, -0.36070218682289124, 0.6458043456077576, -0.6061388254165649, 0.2922779619693756], - [0.5901758074760437, 0.06993594765663147, 0.2914247214794159, -0.3982785642147064, 0.6554641127586365, -0.5877817869186401, 0.25739768147468567], - [0.5728914141654968, 0.16257865726947784, 0.3004828095436096, -0.4288429319858551, 0.6845492124557495, -0.5475476384162903, 0.21835222840309143], - [0.5349975824356079, 0.2612200081348419, 0.29423990845680237, -0.4434780478477478, 0.7227771282196045, -0.5031689405441284, 0.1665576845407486], - [0.502289354801178, 0.32907742261886597, 0.24723584949970245, -0.3830461800098419, 0.7727231979370117, -0.4946635961532593, 0.1071559339761734], - [0.49048033356666565, 0.33556684851646423, 0.1762292981147766, -0.33498552441596985, 0.7840493321418762, -0.5154750943183899, 0.0856550931930542], - [0.49339544773101807, 0.334873765707016, 0.12699848413467407, -0.3229883313179016, 0.7895045876502991, -0.5138758420944214, 0.09106409549713135], - [0.4951143264770508, 0.2605552673339844, 0.10749507695436478, -0.29603084921836853, 0.7468422651290894, -0.5826343894004822, 0.1230020746588707], - [0.5105777978897095, 0.17725078761577606, 0.0976296067237854, -0.291313499212265, 0.7007455229759216, -0.6215708255767822, 0.19427280128002167], - [0.5435065627098083, 0.06313088536262512, 0.11161068826913834, -0.297743558883667, 0.6461387276649475, -0.6455997824668884, 0.2775867283344269], - [0.5594496726989746, -0.03314505144953728, 0.1478213518857956, 0.3072815537452698, -0.579055905342102, 0.6743392944335938, -0.33990997076034546], - [0.5756168365478516, -0.06275233626365662, 0.2009924054145813, 0.34431394934654236, -0.5344999432563782, 0.6804036498069763, -0.36442917585372925], - [0.5784454345703125, -0.12293380498886108, 0.2624003291130066, 0.3557124137878418, -0.48897919058799744, 0.6842485666275024, -0.4076419472694397], - [0.5636951923370361, -0.21520644426345825, 0.3116268813610077, 0.3457571268081665, -0.44096311926841736, 0.6897113919258118, -0.45858660340309143], - [0.5404208302497864, -0.31854310631752014, 0.24892762303352356, 0.2763579487800598, -0.5303931832313538, 0.6992200016975403, -0.391664057970047], - [0.5138706564903259, -0.29581376910209656, 0.1351354867219925, 0.24635033309459686, -0.6483463048934937, 0.6676037907600403, -0.2706727385520935], - [0.5513743758201599, -0.2239120602607727, 0.08924050629138947, -0.3200412094593048, 0.6891423463821411, -0.6101202964782715, 0.2245209813117981], - [0.5742518901824951, -0.1305057555437088, 0.09171643108129501, -0.3716944754123688, 0.69478440284729, -0.5891148447990417, 0.17905732989311218], - [0.5915540456771851, -0.039267927408218384, 0.10550501197576523, -0.4135972261428833, 0.6998876333236694, -0.5652957558631897, 0.1397687941789627], - [0.6060796976089478, 0.025441067293286324, 0.11670827120542526, -0.42734822630882263, 0.7028806209564209, -0.5518678426742554, 0.1370190531015396], - [0.6179891228675842, 0.0633757933974266, 0.13044734299182892, -0.41930848360061646, 0.6576049327850342, -0.593339741230011, 0.19920873641967773], - [0.6067739725112915, 0.06295245885848999, 0.18496927618980408, 0.40669456124305725, -0.5851887464523315, 0.6297315955162048, -0.3091791570186615], - [0.6095121502876282, 0.056209687143564224, 0.23338516056537628, 0.41905108094215393, -0.5604040026664734, 0.6283918023109436, -0.33980485796928406], - [0.6121770143508911, 0.04117409512400627, 0.2725994288921356, 0.4189033806324005, -0.5498623847961426, 0.6234208345413208, -0.36540088057518005] - ] - - bestman.move_end_effector_follow_trajectory(target_trajectory, max_linear_vel=0.1, max_angular_vel=0.5) - time.sleep(1) - - except Exception as e: - # Log any exceptions that occur - log.error(str(e)) - - -if __name__ == "__main__": - main() diff --git a/Examples/move_arm_to_home.py b/Examples/move_arm_to_home.py index 773f038..c736c76 100644 --- a/Examples/move_arm_to_home.py +++ b/Examples/move_arm_to_home.py @@ -3,17 +3,15 @@ """ # @FileName : move_arm_to_home.py # @Time : 2024-12-01 15:21:45 -# @Author : Yan -# @Email : yding25@binghamton.edu +# @Author : Zhaxi & Yan +# @Email : zhaxizhuoma.ayang@gmail.com & yding25@binghamton.edu # @Description : Move arm to default home position -# @Usage : python move_arm_to_home.py 192.168.1.208 20 +# @Usage : python move_arm_to_home.py 192.168.1.208 """ import argparse import time -from Robotics_API import Bestman_Real_Flexiv -import flexivrdk - +from Robotics_API import Bestman_Real_Xarm6 def main(): # Parse Arguments @@ -22,20 +20,11 @@ def main(): ) # Required arguments argparser.add_argument("robot_ip", help="IP address of the robot server") - argparser.add_argument("local_ip", help="IP address of this PC") - argparser.add_argument("frequency", type=int, help="Command frequency, 1 to 200 [Hz]") args = argparser.parse_args() - # Validate the frequency argument - frequency = args.frequency - assert 1 <= frequency <= 200, "Invalid input" - - # Initialize logging - log = flexivrdk.Log() - try: # Instantiate the robot interface - bestman = Bestman_Real_Flexiv(args.robot_ip, args.local_ip, args.frequency) + bestman = Bestman_Real_Xarm6(args.robot_ip) # Initialize robot if not bestman.initialize_robot(): @@ -43,11 +32,11 @@ def main(): # Go back to home pose bestman.go_home() - time.sleep(10) + time.sleep(1) except Exception as e: # Log any exceptions that occur - log.error(str(e)) + print(str(e)) if __name__ == "__main__": diff --git a/Examples/move_arm_to_joint_angles.py b/Examples/move_arm_to_joint_angles.py deleted file mode 100644 index eac8fe5..0000000 --- a/Examples/move_arm_to_joint_angles.py +++ /dev/null @@ -1,63 +0,0 @@ -''' -Run this script using: - -python3 move_arm_to_joint_angles.py 192.168.1.208 -''' - -import sys -import os -parent_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), '..')) -sys.path.append(os.path.join(parent_dir, 'RoboticsToolBox')) -# import pyRobotiqGripper -from Bestman_real_xarm6 import * -from time import time, sleep -import numpy as np -def main(): - # Parse Arguments - argparser = argparse.ArgumentParser(description="Xarm6.") - # Required arguments - argparser.add_argument("robot_ip", help="IP address of the robot server") - # argparser.add_argument("local_ip", help="IP address of this PC") - # argparser.add_argument("frequency", type=int, help="Command frequency, 1 to 200 [Hz]") - # Optional arguments - # argparser.add_argument("--hold", action="store_true", help="Robot holds current joint positions, otherwise do a sine-sweep") - args = argparser.parse_args() - - try: - # Instantiate the robot interface - bestman = Bestman_Real_Xarm6(args.robot_ip, None, None) - - # Clear fault on the robot server if any - # if bestman.robot.isFault(): - # log.warn("Fault occurred on the robot server, trying to clear ...") - # bestman.robot.clearFault() - # time.sleep(2) - # if bestman.robot.isFault(): - # log.error("Fault cannot be cleared, exiting ...") - # return - # log.info("Fault on the robot server is cleared") - - # Enable the robot, ensuring the E-stop is released before enabling - # log.info("Enabling robot ...") - # bestman.robot.enable() - bestman.go_home() - - _joint_angles = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] - - # Go back to home pose - for i in range(10): - - _joint_angles = np.add(_joint_angles, [-0.05, -0.05, -0.05, -0.05, -0.05, -0.05]) - print(_joint_angles) - bestman.move_arm_to_joint_angles(joint_angles=_joint_angles, wait_for_finish=False) - sleep(1) - bestman.go_home() - - - except Exception as e: - # Log any exceptions that occur - print(str(e)) - - -if __name__ == "__main__": - main() diff --git a/Examples/move_arm_to_joint_values.py b/Examples/move_arm_to_joint_values.py new file mode 100644 index 0000000..204b5b1 --- /dev/null +++ b/Examples/move_arm_to_joint_values.py @@ -0,0 +1,44 @@ +# !/usr/bin/env python +# -*- encoding: utf-8 -*- +""" +# @FileName : move_arm_to_home.py +# @Time : 2024-12-01 15:21:45 +# @Author : Zhaxi & Yan +# @Email : zhaxizhuoma.ayang@gmail.com & yding25@binghamton.edu +# @Description : Move arm to Move arm to targeted joint values +# @Usage : python move_arm_to_joint_values.py 192.168.1.208 +""" + +import argparse +import time +from Robotics_API import Bestman_Real_Xarm6 + +def main(): + # Parse Arguments + argparser = argparse.ArgumentParser( + description="Move the robot arm to follow a trajectory." + ) + # Required arguments + argparser.add_argument("robot_ip", help="IP address of the robot server") + args = argparser.parse_args() + + try: + # Instantiate the robot interface + bestman = Bestman_Real_Xarm6(args.robot_ip) + + # Initialize robot + if not bestman.initialize_robot(): + return # Exit if initialization fails + + import math + target_joint = [16.8, -24.1, -24.7, 11.2, -77.7, -5.1] + target_joint = [math.radians(target_joint[n]) for n in range(6)] + bestman.move_arm_to_joint_values(target_joint) + + except Exception as e: + # Log any exceptions that occur + print(str(e)) + + +if __name__ == "__main__": + main() diff --git a/Examples/move_eef_to_pose.py b/Examples/move_eef_to_pose.py new file mode 100644 index 0000000..8d3d947 --- /dev/null +++ b/Examples/move_eef_to_pose.py @@ -0,0 +1,45 @@ +# !/usr/bin/env python +# -*- encoding: utf-8 -*- +""" +# @FileName : move_arm_to_home.py +# @Time : 2024-12-01 15:21:45 +# @Author : Zhaxi & Yan +# @Email : zhaxizhuoma.ayang@gmail.com & yding25@binghamton.edu +# @Description : Move end effector to targeted pose +# @Usage : python move_eef_to_pose.py 192.168.1.208 +""" + +import argparse +import time +from Robotics_API import Bestman_Real_Xarm6, Pose + +def main(): + # Parse Arguments + argparser = argparse.ArgumentParser( + description="Move the robot arm to follow a trajectory." + ) + # Required arguments + argparser.add_argument("robot_ip", help="IP address of the robot server") + args = argparser.parse_args() + + try: + # Instantiate the robot interface + bestman = Bestman_Real_Xarm6(args.robot_ip) + + # Initialize robot + if not bestman.initialize_robot(): + return # Exit if initialization fails + + # Define the target trajectory + target_pose = Pose([0.420434235, -0.00581615, 0.459876038], [0.7071068218077394, -0.00032550013355177364, 0.7071065907288765, 0.0003255002399235735]) # 四元数 + + # Move the arm to follow the target trajectory + bestman.move_eef_to_goal_pose(target_pose) + + except Exception as e: + # Log any exceptions that occur + print(str(e)) + + +if __name__ == "__main__": + main() diff --git a/Examples/open_gripper.py b/Examples/open_gripper.py deleted file mode 100644 index 7310c20..0000000 --- a/Examples/open_gripper.py +++ /dev/null @@ -1,50 +0,0 @@ -''' -Run this script using: - -python3 open_gripper.py 192.168.1.208 -''' - -import sys -import os -parent_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), '..')) -sys.path.append(os.path.join(parent_dir, 'RoboticsToolBox')) -# import pyRobotiqGripper -from Bestman_real_xarm6 import * -from time import time, sleep -import numpy as np -def main(): - # Parse Arguments - argparser = argparse.ArgumentParser(description="Xarm6.") - # Required arguments - argparser.add_argument("robot_ip", help="IP address of the robot server") - # argparser.add_argument("local_ip", help="IP address of this PC") - # argparser.add_argument("frequency", type=int, help="Command frequency, 1 to 200 [Hz]") - # Optional arguments - # argparser.add_argument("--hold", action="store_true", help="Robot holds current joint positions, otherwise do a sine-sweep") - args = argparser.parse_args() - - try: - # Instantiate the robot interface - bestman = Bestman_Real_Xarm6(args.robot_ip, None, None) - - # Clear fault on the robot server if any - # if bestman.robot.isFault(): - # log.warn("Fault occurred on the robot server, trying to clear ...") - # bestman.robot.clearFault() - # time.sleep(2) - # if bestman.robot.isFault(): - # log.error("Fault cannot be cleared, exiting ...") - # return - # log.info("Fault on the robot server is cleared") - - # open gripper - bestman.open_gripper() - - - except Exception as e: - # Log any exceptions that occur - print(str(e)) - - -if __name__ == "__main__": - main() diff --git a/Examples/open_gripper_robotiq.py b/Examples/open_gripper_robotiq.py new file mode 100644 index 0000000..e4a4985 --- /dev/null +++ b/Examples/open_gripper_robotiq.py @@ -0,0 +1,42 @@ +# !/usr/bin/env python +# -*- encoding: utf-8 -*- +""" +# @FileName : open_gripper_robotiq.py +# @Time : 2024-12-01 15:21:45 +# @Author : Zhaxi & Yan +# @Email : zhaxizhuoma.ayang@gmail.com & yding25@binghamton.edu +# @Description : Open robotiq gripper +# @Usage : python open_gripper_robotiq.py 192.168.1.208 +""" + +import argparse +import time +from Robotics_API import Bestman_Real_Xarm6 + +def main(): + # Parse Arguments + argparser = argparse.ArgumentParser( + description="Move the robot arm to follow a trajectory." + ) + # Required arguments + argparser.add_argument("robot_ip", help="IP address of the robot server") + args = argparser.parse_args() + + try: + # Instantiate the robot interface + bestman = Bestman_Real_Xarm6(args.robot_ip) + + # Initialize robot + if not bestman.initialize_robot(): + return # Exit if initialization fails + + bestman.open_gripper_robotiq() + time.sleep(1) + + except Exception as e: + # Log any exceptions that occur + print(str(e)) + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/Examples/open_gripper_width.py b/Examples/open_gripper_width.py deleted file mode 100644 index 31963a4..0000000 --- a/Examples/open_gripper_width.py +++ /dev/null @@ -1,50 +0,0 @@ -''' -Run this script using: - -python3 open_gripper_width.py 192.168.1.208 -''' - -import sys -import os -parent_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), '..')) -sys.path.append(os.path.join(parent_dir, 'RoboticsToolBox')) -# import pyRobotiqGripper -from Bestman_real_xarm6 import * -from time import time, sleep -import numpy as np -def main(): - # Parse Arguments - argparser = argparse.ArgumentParser(description="Xarm6.") - # Required arguments - argparser.add_argument("robot_ip", help="IP address of the robot server") - # argparser.add_argument("local_ip", help="IP address of this PC") - # argparser.add_argument("frequency", type=int, help="Command frequency, 1 to 200 [Hz]") - # Optional arguments - # argparser.add_argument("--hold", action="store_true", help="Robot holds current joint positions, otherwise do a sine-sweep") - args = argparser.parse_args() - - try: - # Instantiate the robot interface - bestman = Bestman_Real_Xarm6(args.robot_ip, None, None) - - # Clear fault on the robot server if any - # if bestman.robot.isFault(): - # log.warn("Fault occurred on the robot server, trying to clear ...") - # bestman.robot.clearFault() - # time.sleep(2) - # if bestman.robot.isFault(): - # log.error("Fault cannot be cleared, exiting ...") - # return - # log.info("Fault on the robot server is cleared") - - # open gripper - bestman.gripper_goto(value=300, speed=8000, force=None) - - - except Exception as e: - # Log any exceptions that occur - print(str(e)) - - -if __name__ == "__main__": - main() diff --git a/Examples/teach_by_demonstration.py b/Examples/teach_by_demonstration.py deleted file mode 100644 index fed44e0..0000000 --- a/Examples/teach_by_demonstration.py +++ /dev/null @@ -1,192 +0,0 @@ -#!/usr/bin/env python - -"""intermediate4_teach_by_demonstration.py - -This tutorial shows a demo implementation for teach by demonstration: free-drive the robot and -record a series of Cartesian poses, which are then reproduced by the robot. - -Run this script using: -python intermediate4_teach_by_demonstration.py 192.168.2.100 192.168.2.108 -""" - -__copyright__ = "Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved." -__author__ = "Flexiv" - -import time -import argparse -import sys -import os -parent_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), '..')) -sys.path.append(os.path.join(parent_dir, 'RoboticsToolBox')) -from utility import quat2eulerZYX -from utility import list2str -from utility import parse_pt_states -from Bestman_sim_flexiv import * - -# Maximum contact wrench [fx, fy, fz, mx, my, mz] [N][Nm] -MAX_CONTACT_WRENCH = [50.0, 50.0, 50.0, 15.0, 15.0, 15.0] - -def print_description(): - """ - Print tutorial description. - - """ - print( - "This tutorial shows a demo implementation for teach by demonstration: free-drive the " - "robot and record a series of Cartesian poses, which are then reproduced by the robot." - ) - print() - - -def main(): - # Program Setup - # ============================================================================================== - # Parse arguments - argparser = argparse.ArgumentParser() - argparser.add_argument("robot_ip", help="IP address of the robot server") - argparser.add_argument("local_ip", help="IP address of this PC") - args = argparser.parse_args() - - # Define alias - log = flexivrdk.Log() - mode = flexivrdk.Mode - - # Print description - log.info("Tutorial description:") - print_description() - - try: - # RDK Initialization - # ========================================================================================== - # Instantiate robot interface - robot = flexivrdk.Robot(args.robot_ip, args.local_ip) - - # Clear fault on robot server if any - if robot.isFault(): - log.warn("Fault occurred on robot server, trying to clear ...") - # Try to clear the fault - robot.clearFault() - time.sleep(2) - # Check again - if robot.isFault(): - log.error("Fault cannot be cleared, exiting ...") - return - log.info("Fault on robot server is cleared") - - # Enable the robot, make sure the E-stop is released before enabling - log.info("Enabling robot ...") - robot.enable() - - # Wait for the robot to become operational - while not robot.isOperational(): - time.sleep(1) - - log.info("Robot is now operational") - - # Teach By Demonstration - # ========================================================================================== - # Recorded robot poses - saved_poses = [] - - # Robot states data - robot_states = flexivrdk.RobotStates() - - # Acceptable user inputs - log.info("Accepted key inputs:") - print("[n] - start new teaching process") - print("[r] - record current robot pose") - print("[e] - finish recording and start execution") - - # User input polling - input_buffer = "" - while True: - input_buffer = str(input()) - # Start new teaching process - if input_buffer == "n": - # Clear storage - saved_poses.clear() - - # Put robot to plan execution mode - robot.setMode(mode.NRT_PLAN_EXECUTION) - - # Robot run free drive - robot.executePlan("PLAN-FreeDriveAuto") - - log.info("New teaching process started") - log.warn( - "Hold down the enabling button on the motion bar to activate free drive" - ) - # Save current robot pose - elif input_buffer == "r": - if not robot.isBusy(): - log.warn("Please start a new teaching process first") - continue - - robot.getRobotStates(robot_states) - saved_poses.append(robot_states.tcpPose) - log.info("New pose saved: " + str(robot_states.tcpPose)) - log.info("Number of saved poses: " + str(len(saved_poses))) - # Reproduce recorded poses - elif input_buffer == "e": - if len(saved_poses) == 0: - log.warn("No pose is saved yet") - continue - - # Put robot to primitive execution mode - robot.setMode(mode.NRT_PRIMITIVE_EXECUTION) - - for i in range(len(saved_poses)): - log.info( - "Executing pose " + str(i + 1) + "/" + str(len(saved_poses)) - ) - - target_pos = [ - saved_poses[i][0], - saved_poses[i][1], - saved_poses[i][2], - ] - # Convert quaternion to Euler ZYX required by MoveCompliance primitive - target_quat = [ - saved_poses[i][3], - saved_poses[i][4], - saved_poses[i][5], - saved_poses[i][6], - ] - - target_euler_deg = quat2eulerZYX(target_quat, degree=True) - robot.executePrimitive( - "MoveCompliance(target=" - + list2str(target_pos) - + list2str(target_euler_deg) - + "WORLD WORLD_ORIGIN, maxVel=0.3, enableMaxContactWrench=1, maxContactWrench=" - + list2str(MAX_CONTACT_WRENCH) - + ")" - ) - - # Wait for robot to reach target location by checking for "reachedTarget = 1" - # in the list of current primitive states - while ( - parse_pt_states(robot.getPrimitiveStates(), "reachedTarget") - != "1" - ): - time.sleep(1) - - log.info( - "All saved poses are executed, enter 'n' to start a new " - "teaching process, 'r' to record more poses, 'e' to repeat " - "execution" - ) - - # Put robot back to free drive - robot.setMode(mode.NRT_PLAN_EXECUTION) - robot.executePlan("PLAN-FreeDriveAuto") - else: - log.warn("Invalid input") - - except Exception as e: - # Print exception error message - log.error(str(e)) - - -if __name__ == "__main__": - main() diff --git a/Examples/test.py b/Examples/test.py new file mode 100644 index 0000000..237fc37 --- /dev/null +++ b/Examples/test.py @@ -0,0 +1,68 @@ +# !/usr/bin/env python +# -*- encoding: utf-8 -*- +""" +# @FileName : move_arm_to_home.py +# @Time : 2024-12-01 15:21:45 +# @Author : Zhaxi & Yan +# @Email : zhaxizhuoma.ayang@gmail.com & yding25@binghamton.edu +# @Description : Move arm to default home position +# @Usage : python test.py 192.168.1.208 +""" + +import argparse +import time +from Robotics_API import Bestman_Real_Xarm6 +from scipy.spatial.transform import Rotation as R +import numpy as np +import math + +def main(): + # Parse Arguments + argparser = argparse.ArgumentParser( + description="Move the robot arm to follow a trajectory." + ) + # Required arguments + argparser.add_argument("robot_ip", help="IP address of the robot server") + args = argparser.parse_args() + + try: + # Instantiate the robot interface + bestman = Bestman_Real_Xarm6(args.robot_ip) + + # Initialize robot + if not bestman.initialize_robot(): + return # Exit if initialization fails + + bestman.update_robot_states() + # Test + gt_pose = bestman.get_current_eef_pose() + gt_joint = bestman.get_current_joint_values() + print(f'gt position:{gt_pose.position}, gt orientation:{gt_pose.orientation}') + print(f'gt joint:{gt_joint}') + # joint_values = [-1.6, -27.2, -2.2, -1.3, -60.6, 0.2] + # joint_values_rad = [v / 180 * math.pi for v in joint_values] + # joint_values_rad = [-0.12792803756892681, -0.47472870349884033, -0.038403209298849106, -0.022691410034894943, -1.0576682090759277, 0.0034917236771434546] + # bestman.move_arm_to_joint_values(joint_values_rad, wait_for_finish=False) + + # joint_values_rad = [-0.22792803756892681, -0.47472870349884033, -0.038403209298849106, -0.022691410034894943, -1.0576682090759277, 0.0034917236771434546] + # bestman.move_arm_to_joint_values(joint_values_rad, wait_for_finish=False) + + # pose.position[2] += 0.1 + # print(f'test position:{pose.position}, test orientation:{pose.orientation}') + # bestman.move_eef_to_goal_pose(pose) + + trans_joint = bestman.cartesian_to_joints(gt_pose) + print(f'trans_joint:{trans_joint}') + bestman.move_arm_to_joint_values(trans_joint) + + trans_pose = bestman.joints_to_cartesian(gt_joint) + print(f'trans_pose:{trans_pose}') + bestman.move_eef_to_goal_pose(trans_pose) + + except Exception as e: + # Log any exceptions that occur + print(str(e)) + + +if __name__ == "__main__": + main() diff --git a/Examples/xarm_command_log.log b/Examples/xarm_command_log.log new file mode 100644 index 0000000..26ae865 --- /dev/null +++ b/Examples/xarm_command_log.log @@ -0,0 +1,1124 @@ +2024-12-18 10:21:28,018 - INFO - Checking for faults on the robot... +2024-12-18 10:21:31,021 - INFO - Successfully initialize the robot +2024-12-18 10:21:40,888 - INFO - Checking for faults on the robot... +2024-12-18 10:21:43,892 - INFO - Successfully initialize the robot +2024-12-18 10:57:45,836 - INFO - Checking for faults on the robot... +2024-12-18 10:57:48,840 - INFO - Successfully initialize the robot +2024-12-18 10:58:51,895 - INFO - Checking for faults on the robot... +2024-12-18 10:58:54,898 - INFO - Successfully initialize the robot +2024-12-18 10:59:08,786 - INFO - Checking for faults on the robot... +2024-12-18 10:59:11,790 - INFO - Successfully initialize the robot +2024-12-18 10:59:11,793 - INFO - Robot states updated. +2024-12-18 10:59:41,411 - INFO - Checking for faults on the robot... +2024-12-18 10:59:44,415 - INFO - Successfully initialize the robot +2024-12-18 10:59:44,416 - INFO - Robot states updated: {'joint_angles': [-0.027926120907068253, -0.4747267961502075, -0.03839937224984169, -0.022685658186674118, -1.0576759576797485, 0.0034840537700802088], 'joint_velocities': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 'tcp_pose': [420.434235, -5.81615, 359.876038, 3.140672, -1.570796, 0.0]} +2024-12-18 11:11:04,631 - INFO - Checking for faults on the robot... +2024-12-18 11:11:07,635 - INFO - Successfully initialize the robot +2024-12-18 11:11:07,638 - INFO - Robot states updated: {'joint_angles': [-0.027922285720705986, -0.47472870349884033, -0.0384012907743454, -0.022689493373036385, -1.0576720237731934, 0.003482136409729719], 'joint_velocities': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 'tcp_pose': [420.434235, -5.81615, 359.876038, 3.140672, -1.570796, 0.0]} +2024-12-18 11:11:15,198 - INFO - Checking for faults on the robot... +2024-12-18 11:11:18,202 - INFO - Successfully initialize the robot +2024-12-18 11:12:21,981 - INFO - Checking for faults on the robot... +2024-12-18 11:12:24,984 - INFO - Successfully initialize the robot +2024-12-18 11:14:48,311 - INFO - Checking for faults on the robot... +2024-12-18 11:14:51,314 - INFO - Successfully initialize the robot +2024-12-18 11:17:17,238 - INFO - Checking for faults on the robot... +2024-12-18 11:17:20,241 - INFO - Successfully initialize the robot +2024-12-18 11:20:44,817 - INFO - Checking for faults on the robot... +2024-12-18 11:20:47,821 - INFO - Successfully initialize the robot +2024-12-18 11:21:40,372 - INFO - Checking for faults on the robot... +2024-12-18 11:21:43,375 - INFO - Successfully initialize the robot +2024-12-18 11:23:34,190 - INFO - Checking for faults on the robot... +2024-12-18 11:23:37,194 - INFO - Successfully initialize the robot +2024-12-18 11:23:37,196 - INFO - x:396.4, y:-5.5, z:360.0, roll:90.0, pitch:0.0, yaw:-90.0 +2024-12-18 11:28:44,131 - INFO - Checking for faults on the robot... +2024-12-18 11:28:47,135 - INFO - Successfully initialize the robot +2024-12-18 11:29:29,591 - INFO - Checking for faults on the robot... +2024-12-18 11:29:32,593 - INFO - Successfully initialize the robot +2024-12-18 11:29:32,595 - INFO - x:396.4, y:-5.5, z:360.0, roll:-180.0, pitch:-90.0, yaw:0.0 +2024-12-18 11:34:57,158 - INFO - Checking for faults on the robot... +2024-12-18 11:35:00,162 - INFO - Successfully initialize the robot +2024-12-18 11:35:00,166 - INFO - x:396.4, y:-5.5, z:360.0, roll:-180.0, pitch:-90.0, yaw:0.0 +2024-12-18 11:36:29,865 - INFO - Checking for faults on the robot... +2024-12-18 11:36:32,869 - INFO - Successfully initialize the robot +2024-12-18 11:36:32,873 - INFO - x:396.4, y:-5.5, z:360.0, roll:-180.0, pitch:-90.0, yaw:0.0 +2024-12-18 11:41:01,984 - INFO - Checking for faults on the robot... +2024-12-18 11:41:04,987 - INFO - Successfully initialize the robot +2024-12-18 11:41:04,991 - INFO - x:396.4, y:-5.5, z:360.0, roll:-180.0, pitch:-90.0, yaw:0.0 +2024-12-18 11:41:20,576 - INFO - Checking for faults on the robot... +2024-12-18 11:41:23,579 - INFO - Successfully initialize the robot +2024-12-18 11:41:23,583 - INFO - x:396.4, y:-5.5, z:360.0, roll:180.0, pitch:-90.0, yaw:0.0 +2024-12-18 11:42:39,940 - INFO - Checking for faults on the robot... +2024-12-18 11:42:42,943 - INFO - Successfully initialize the robot +2024-12-18 11:42:42,944 - INFO - x:396.4, y:-5.5, z:360.0, roll:90.0, pitch:0.0, yaw:-90.0 +2024-12-18 11:45:45,965 - INFO - Checking for faults on the robot... +2024-12-18 11:45:48,968 - INFO - Successfully initialize the robot +2024-12-18 11:45:48,969 - INFO - x:396.4, y:-5.5, z:360.0, roll:0.0, pitch:0.0, yaw:0.0 +2024-12-18 11:47:12,772 - INFO - Checking for faults on the robot... +2024-12-18 11:47:15,775 - INFO - Successfully initialize the robot +2024-12-18 11:47:15,779 - INFO - x:396.4, y:-5.5, z:360.0, roll:0.0, pitch:-90.0, yaw:0.0 +2024-12-18 11:49:38,744 - INFO - Checking for faults on the robot... +2024-12-18 11:49:41,747 - INFO - Successfully initialize the robot +2024-12-18 11:49:41,750 - INFO - x:396.4, y:-5.5, z:360.0, roll:0.0, pitch:0.0, yaw:0.0 +2024-12-18 11:49:59,651 - INFO - Checking for faults on the robot... +2024-12-18 11:50:02,654 - INFO - Successfully initialize the robot +2024-12-18 11:50:02,657 - INFO - x:396.4, y:-5.5, z:360.0, roll:180.0, pitch:0.0, yaw:0.0 +2024-12-18 11:50:22,326 - INFO - Checking for faults on the robot... +2024-12-18 11:50:25,328 - INFO - Successfully initialize the robot +2024-12-18 11:50:25,331 - INFO - x:396.4, y:-5.5, z:360.0, roll:0.0, pitch:0.0, yaw:180.0 +2024-12-18 11:51:16,922 - INFO - Checking for faults on the robot... +2024-12-18 11:51:19,924 - INFO - Successfully initialize the robot +2024-12-18 11:51:19,927 - INFO - x:396.4, y:-5.5, z:360.0, roll:-178.4289429205916, pitch:1.614731541901501, yaw:-1.5710570794084195 +2024-12-18 11:52:19,863 - INFO - Checking for faults on the robot... +2024-12-18 11:52:22,866 - INFO - Successfully initialize the robot +2024-12-18 11:52:22,869 - INFO - x:396.4, y:-5.5, z:360.0, roll:-178.42847802292002, pitch:1.5803633679586777, yaw:-1.5715219770799764 +2024-12-18 11:54:12,711 - INFO - Checking for faults on the robot... +2024-12-18 11:54:15,714 - INFO - Successfully initialize the robot +2024-12-18 11:54:15,717 - INFO - x:396.4, y:-5.5, z:360.0, roll:90.0, pitch:0.0, yaw:-90.0 +2024-12-18 11:57:56,821 - INFO - Checking for faults on the robot... +2024-12-18 11:57:59,824 - INFO - Successfully initialize the robot +2024-12-18 11:57:59,825 - INFO - x:396.4, y:-5.5, z:360.0, roll:90.0, pitch:0.0, yaw:-90.0 +2024-12-18 11:58:30,983 - INFO - Checking for faults on the robot... +2024-12-18 11:58:33,986 - INFO - Successfully initialize the robot +2024-12-18 11:58:33,988 - INFO - x:396.4, y:-5.5, z:360.0, roll:90.0, pitch:0.0, yaw:-90.0 +2024-12-18 11:59:04,562 - INFO - Checking for faults on the robot... +2024-12-18 11:59:07,565 - INFO - Successfully initialize the robot +2024-12-18 11:59:07,566 - INFO - x:396.4, y:-5.5, z:360.0, roll:179.9, pitch:-90.0, yaw:0.0 +2024-12-18 11:59:22,540 - INFO - Checking for faults on the robot... +2024-12-18 11:59:25,543 - INFO - Successfully initialize the robot +2024-12-18 11:59:25,544 - INFO - x:396.4, y:-5.5, z:360.0, roll:179.9, pitch:-90.0, yaw:0.0 +2024-12-18 12:00:29,627 - INFO - Checking for faults on the robot... +2024-12-18 12:00:32,630 - INFO - Successfully initialize the robot +2024-12-18 12:00:32,631 - INFO - x:420.0, y:-5.5, z:360.0, roll:179.9, pitch:-90.0, yaw:0.0 +2024-12-18 12:00:56,124 - INFO - Checking for faults on the robot... +2024-12-18 12:00:59,125 - INFO - Successfully initialize the robot +2024-12-18 12:00:59,126 - INFO - x:420.0, y:-5.5, z:360.0, roll:179.9, pitch:-90.0, yaw:0.0 +2024-12-18 12:00:59,127 - INFO - Robot moved to home position +2024-12-18 12:01:43,640 - INFO - Checking for faults on the robot... +2024-12-18 12:01:46,643 - INFO - Successfully initialize the robot +2024-12-18 12:01:46,644 - INFO - x:420.0, y:-5.5, z:360.0, roll:179.9, pitch:-90.0, yaw:0.0 +2024-12-18 12:02:33,026 - INFO - Checking for faults on the robot... +2024-12-18 12:02:36,030 - INFO - Successfully initialize the robot +2024-12-18 12:03:06,355 - INFO - Checking for faults on the robot... +2024-12-18 12:03:09,359 - INFO - Successfully initialize the robot +2024-12-18 12:03:09,360 - INFO - x:420.0, y:-5.5, z:360.0, roll:179.9, pitch:-90.0, yaw:0.0 +2024-12-18 12:03:16,307 - INFO - Checking for faults on the robot... +2024-12-18 12:03:19,310 - INFO - Successfully initialize the robot +2024-12-18 12:03:19,311 - INFO - x:420.0, y:-5.5, z:360.0, roll:179.9, pitch:-90.0, yaw:0.0 +2024-12-18 12:03:27,613 - INFO - Checking for faults on the robot... +2024-12-18 12:03:30,617 - INFO - Successfully initialize the robot +2024-12-18 12:03:30,618 - INFO - x:420.0, y:-5.5, z:360.0, roll:179.9, pitch:-90.0, yaw:0.0 +2024-12-18 12:03:39,292 - INFO - Checking for faults on the robot... +2024-12-18 12:03:42,296 - INFO - Successfully initialize the robot +2024-12-18 12:03:42,296 - INFO - x:420.0, y:-5.5, z:360.0, roll:179.9, pitch:-90.0, yaw:0.0 +2024-12-18 12:03:45,992 - INFO - Checking for faults on the robot... +2024-12-18 12:03:48,995 - INFO - Successfully initialize the robot +2024-12-18 12:03:48,996 - INFO - x:420.0, y:-5.5, z:360.0, roll:179.9, pitch:-90.0, yaw:0.0 +2024-12-18 12:03:48,996 - INFO - Robot moved to home position +2024-12-18 12:07:10,398 - INFO - Checking for faults on the robot... +2024-12-18 12:07:13,401 - INFO - Successfully initialize the robot +2024-12-18 12:07:13,401 - INFO - Link 1: world_joint +2024-12-18 12:07:13,401 - INFO - Link 2: joint1 +2024-12-18 12:07:13,401 - INFO - Link 3: joint2 +2024-12-18 12:07:13,401 - INFO - Link 4: joint3 +2024-12-18 12:07:13,401 - INFO - Link 5: joint4 +2024-12-18 12:07:13,401 - INFO - Link 6: joint5 +2024-12-18 12:07:13,401 - INFO - Link 7: joint6 +2024-12-18 12:08:58,020 - INFO - Checking for faults on the robot... +2024-12-18 12:09:01,023 - INFO - Successfully initialize the robot +2024-12-18 12:10:20,385 - INFO - Checking for faults on the robot... +2024-12-18 12:10:23,388 - INFO - Successfully initialize the robot +2024-12-18 12:10:23,389 - INFO - Robot states updated: {'joint_angles': [-0.022722089663147926, -0.47703543305397034, -0.036874979734420776, -0.01575014740228653, -1.0572023391723633, 7.669904152862728e-05], 'joint_velocities': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 'tcp_pose': [420.0, -5.500007, 360.0, 3.139847, -1.570796, 0.0]} +2024-12-18 12:10:23,389 - ERROR - Failed to retrieve DOF: 'dict' object has no attribute 'q' +2024-12-18 12:11:08,557 - INFO - Checking for faults on the robot... +2024-12-18 12:11:11,560 - INFO - Successfully initialize the robot +2024-12-18 12:11:11,561 - INFO - Robot states updated: {'joint_angles': [-0.02271825633943081, -0.47703543305397034, -0.036874979734420776, -0.015753982588648796, -1.057206153869629, 7.861651829443872e-05], 'joint_velocities': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 'tcp_pose': [420.0, -5.500007, 360.0, 3.139847, -1.570796, 0.0]} +2024-12-18 12:11:32,931 - INFO - Checking for faults on the robot... +2024-12-18 12:11:35,935 - INFO - Successfully initialize the robot +2024-12-18 12:12:01,559 - INFO - Checking for faults on the robot... +2024-12-18 12:12:04,562 - INFO - Successfully initialize the robot +2024-12-18 12:19:18,638 - INFO - Checking for faults on the robot... +2024-12-18 12:19:21,641 - INFO - Successfully initialize the robot +2024-12-18 12:31:09,340 - INFO - Checking for faults on the robot... +2024-12-18 12:31:12,344 - INFO - Successfully initialize the robot +2024-12-18 12:31:27,842 - INFO - Checking for faults on the robot... +2024-12-18 12:31:30,846 - INFO - Successfully initialize the robot +2024-12-18 12:31:57,790 - INFO - Checking for faults on the robot... +2024-12-18 12:32:00,793 - INFO - Successfully initialize the robot +2024-12-18 12:32:00,794 - INFO - Link 1: world_joint +2024-12-18 12:32:00,794 - INFO - Link 2: joint1 +2024-12-18 12:32:00,794 - INFO - Link 3: joint2 +2024-12-18 12:32:00,794 - INFO - Link 4: joint3 +2024-12-18 12:32:00,795 - INFO - Link 5: joint4 +2024-12-18 12:32:00,795 - INFO - Link 6: joint5 +2024-12-18 12:32:00,795 - INFO - Link 7: joint6 +2024-12-18 12:32:10,772 - INFO - Checking for faults on the robot... +2024-12-18 12:32:13,774 - INFO - Successfully initialize the robot +2024-12-18 12:32:26,923 - INFO - Checking for faults on the robot... +2024-12-18 12:32:29,926 - INFO - Successfully initialize the robot +2024-12-18 12:32:29,929 - INFO - Robot states updated: {'joint_angles': [-0.02792995609343052, -0.47473061084747314, -0.03839937224984169, -0.02269524522125721, -1.0576720237731934, 0.0034878887236118317], 'joint_velocities': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 'tcp_pose': [420.434235, -5.81615, 359.876038, 3.140672, -1.570796, 0.0]} +2024-12-18 12:32:29,930 - ERROR - Failed to retrieve DOF: 'dict' object has no attribute 'q' +2024-12-18 12:33:22,978 - INFO - Checking for faults on the robot... +2024-12-18 12:33:25,982 - INFO - Successfully initialize the robot +2024-12-18 12:33:25,982 - INFO - Robot DOF: 6 +2024-12-18 12:33:36,446 - INFO - Checking for faults on the robot... +2024-12-18 12:33:39,450 - INFO - Successfully initialize the robot +2024-12-18 12:33:39,450 - INFO - TCP link retrieved: joint5 +2024-12-18 12:35:03,307 - INFO - Checking for faults on the robot... +2024-12-18 12:35:06,311 - INFO - Successfully initialize the robot +2024-12-18 12:35:15,775 - INFO - Checking for faults on the robot... +2024-12-18 12:35:18,778 - INFO - Successfully initialize the robot +2024-12-18 12:35:18,779 - INFO - TCP link retrieved: joint6 +2024-12-18 12:36:45,902 - INFO - Checking for faults on the robot... +2024-12-18 12:36:48,906 - INFO - Successfully initialize the robot +2024-12-18 12:38:38,619 - INFO - Checking for faults on the robot... +2024-12-18 12:38:41,622 - INFO - Successfully initialize the robot +2024-12-18 12:38:41,624 - INFO - Current joint values: [-0.02792995609343052, -0.4747267961502075, -0.03839937224984169, -0.02269524522125721, -1.0576759576797485, 0.0034917236771434546] +2024-12-18 12:41:02,526 - INFO - Checking for faults on the robot... +2024-12-18 12:41:05,529 - INFO - Successfully initialize the robot +2024-12-18 12:41:05,530 - INFO - Current joint values: [-0.02792803756892681, -0.47472870349884033, -0.03839937224984169, -0.02269524522125721, -1.0576720237731934, 0.0034917236771434546] +2024-12-18 12:42:22,550 - INFO - Checking for faults on the robot... +2024-12-18 12:42:25,554 - INFO - Successfully initialize the robot +2024-12-18 12:42:25,555 - INFO - Current joint velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] +2024-12-18 12:47:09,221 - INFO - Checking for faults on the robot... +2024-12-18 12:47:12,224 - INFO - Successfully initialize the robot +2024-12-18 12:47:12,225 - INFO - Current end effector pose: [0.420434235, -0.00581615, 0.359876038, 3.140672, -1.570796, 0.0] +2024-12-18 12:48:49,846 - INFO - Checking for faults on the robot... +2024-12-18 12:48:52,849 - INFO - Successfully initialize the robot +2024-12-18 12:48:52,850 - INFO - Current end effector pose: +2024-12-18 12:52:36,977 - INFO - Checking for faults on the robot... +2024-12-18 12:52:39,980 - INFO - Successfully initialize the robot +2024-12-18 12:54:48,198 - INFO - Checking for faults on the robot... +2024-12-18 12:54:51,202 - INFO - Successfully initialize the robot +2024-12-18 12:56:16,603 - INFO - Checking for faults on the robot... +2024-12-18 12:56:19,607 - INFO - Successfully initialize the robot +2024-12-18 12:56:40,999 - INFO - Checking for faults on the robot... +2024-12-18 12:56:44,003 - INFO - Successfully initialize the robot +2024-12-18 12:56:44,005 - INFO - Current joint values: [-0.02792803756892681, -0.47472870349884033, -0.038403209298849106, -0.022691410034894943, -1.0576682090759277, 0.0034917236771434546] +2024-12-18 12:57:48,238 - INFO - Checking for faults on the robot... +2024-12-18 12:57:51,242 - INFO - Successfully initialize the robot +2024-12-18 12:57:51,243 - INFO - Current joint values: [-0.02792803756892681, -0.47472870349884033, -0.03839937224984169, -0.022689493373036385, -1.0576720237731934, 0.0034878887236118317] +2024-12-18 12:58:02,951 - INFO - Checking for faults on the robot... +2024-12-18 12:58:05,955 - INFO - Successfully initialize the robot +2024-12-18 12:58:05,956 - INFO - Current joint values: [-0.027931872755289078, -0.47472870349884033, -0.03840704262256622, -0.02269524522125721, -1.0576682090759277, 0.0034917236771434546] +2024-12-18 12:58:35,739 - INFO - Checking for faults on the robot... +2024-12-18 12:58:38,740 - INFO - Successfully initialize the robot +2024-12-18 12:58:38,741 - INFO - Current joint values: [-0.1279320865869522, -0.4747248589992523, -0.03840704262256622, -0.02269524522125721, -1.057666301727295, 0.0034878887236118317] +2024-12-18 13:00:44,350 - INFO - Checking for faults on the robot... +2024-12-18 13:00:47,354 - INFO - Successfully initialize the robot +2024-12-18 13:00:47,355 - INFO - Current joint values: [-0.02792995609343052, -0.47472870349884033, -0.03840896114706993, -0.022691410034894943, -1.057666301727295, 0.0034917236771434546] +2024-12-18 13:02:31,278 - INFO - Checking for faults on the robot... +2024-12-18 13:02:34,281 - INFO - Successfully initialize the robot +2024-12-18 13:02:34,282 - INFO - Current joint values: [-0.02792995609343052, -0.4747248589992523, -0.03840704262256622, -0.022691410034894943, -1.057666301727295, 0.0034917236771434546] +2024-12-18 13:02:34,297 - INFO - Moving arm to joint values: [-0.02792803756892681, -0.47472870349884033, -0.038403209298849106, -0.022691410034894943, -1.0576682090759277, 0.0034917236771434546] +2024-12-18 13:02:34,297 - INFO - Motion complete. +2024-12-18 13:03:24,602 - INFO - Checking for faults on the robot... +2024-12-18 13:03:27,606 - INFO - Successfully initialize the robot +2024-12-18 13:03:27,607 - INFO - Current joint values: [-0.027926120907068253, -0.4747248589992523, -0.03840896114706993, -0.022691410034894943, -1.057666301727295, 0.0034917236771434546] +2024-12-18 13:03:27,628 - INFO - Moving arm to joint values: [-0.02792803756892681, -0.47472870349884033, -0.038403209298849106, -0.022691410034894943, -1.0576682090759277, 0.0034917236771434546] +2024-12-18 13:06:00,687 - INFO - Checking for faults on the robot... +2024-12-18 13:06:03,691 - INFO - Successfully initialize the robot +2024-12-18 13:06:03,692 - INFO - Current joint values: [-0.02792995609343052, -0.47472870349884033, -0.03840896114706993, -0.022691410034894943, -1.057666301727295, 0.0034917236771434546] +2024-12-18 13:06:43,781 - INFO - Checking for faults on the robot... +2024-12-18 13:06:46,785 - INFO - Successfully initialize the robot +2024-12-18 13:06:46,787 - INFO - Current joint values: [-0.22792653739452362, -0.4747267961502075, -0.03840704262256622, -0.022691410034894943, -1.057666301727295, 0.003485971363261342] +2024-12-18 13:07:12,502 - INFO - Checking for faults on the robot... +2024-12-18 13:07:15,505 - INFO - Successfully initialize the robot +2024-12-18 13:07:15,506 - INFO - Current joint values: [-0.22793228924274445, -0.4747267961502075, -0.03840896114706993, -0.02269524522125721, -1.057666301727295, 0.0034878887236118317] +2024-12-18 13:07:30,905 - INFO - Checking for faults on the robot... +2024-12-18 13:07:33,909 - INFO - Successfully initialize the robot +2024-12-18 13:07:33,910 - INFO - Current joint values: [-0.22793036699295044, -0.47472870349884033, -0.03840704262256622, -0.02269524522125721, -1.0576682090759277, 0.0034917236771434546] +2024-12-18 13:59:04,860 - INFO - Checking for faults on the robot... +2024-12-18 13:59:07,864 - INFO - Successfully initialize the robot +2024-12-18 13:59:07,865 - INFO - Current joint values: [-0.4279269576072693, -0.47472870349884033, -0.03840896114706993, -0.02269524522125721, -1.057666301727295, 0.0034878887236118317] +2024-12-18 13:59:30,813 - INFO - Checking for faults on the robot... +2024-12-18 13:59:33,817 - INFO - Successfully initialize the robot +2024-12-18 13:59:33,818 - INFO - Current joint values: [-0.4279327094554901, -0.47472870349884033, -0.038403209298849106, -0.022691410034894943, -1.0576682090759277, 0.0034878887236118317] +2024-12-18 14:01:49,643 - INFO - Checking for faults on the robot... +2024-12-18 14:01:52,647 - INFO - Successfully initialize the robot +2024-12-18 14:01:52,649 - INFO - Current joint values: [-0.12792633473873138, -0.47472870349884033, -0.038403209298849106, -0.02269524522125721, -1.057666301727295, 0.003485971363261342] +2024-12-18 14:05:11,257 - INFO - Checking for faults on the robot... +2024-12-18 14:05:14,261 - INFO - Successfully initialize the robot +2024-12-18 14:05:14,263 - INFO - Current joint values: [-0.1279301643371582, -0.47472870349884033, -0.038403209298849106, -0.02269524522125721, -1.0576682090759277, 0.003485971363261342] +2024-12-18 14:06:30,357 - INFO - Checking for faults on the robot... +2024-12-18 14:06:33,361 - INFO - Successfully initialize the robot +2024-12-18 14:06:33,362 - INFO - Current joint values: [-0.027926120907068253, -0.47472870349884033, -0.03839745745062828, -0.022691410034894943, -1.0576701164245605, 0.0034878887236118317] +2024-12-18 14:10:53,013 - INFO - Checking for faults on the robot... +2024-12-18 14:10:56,017 - INFO - Successfully initialize the robot +2024-12-18 14:10:56,017 - INFO - Current joint values: [-0.1279301643371582, -0.47472870349884033, -0.038405127823352814, -0.022691410034894943, -1.0576682090759277, 0.003485971363261342] +2024-12-18 14:11:58,497 - INFO - Checking for faults on the robot... +2024-12-18 14:12:01,501 - INFO - Successfully initialize the robot +2024-12-18 14:12:01,501 - INFO - Current joint values: [-0.1279301643371582, -0.47473061084747314, -0.038405127823352814, -0.02269524522125721, -1.0576682090759277, 0.0034878887236118317] +2024-12-18 14:12:01,515 - INFO - Moving arm to joint values: [-0.12792803756892682, -0.47472870349884033, -0.038403209298849106, -0.022691410034894943, -1.0576682090759277, 0.0034917236771434546] +2024-12-18 14:12:01,516 - INFO - Moving arm to joint values: [-0.22792803756892682, -0.47472870349884033, -0.038403209298849106, -0.022691410034894943, -1.0576682090759277, 0.0034917236771434546] +2024-12-18 14:39:51,033 - INFO - Checking for faults on the robot... +2024-12-18 14:39:54,036 - INFO - Successfully initialize the robot +2024-12-18 14:39:54,037 - INFO - Current joint values: [-0.22793036699295044, -0.4747267961502075, -0.03840896114706993, -0.02269524522125721, -1.0576682090759277, 0.0034878887236118317] +2024-12-18 14:40:09,463 - INFO - Checking for faults on the robot... +2024-12-18 14:40:12,466 - INFO - Successfully initialize the robot +2024-12-18 14:40:12,467 - INFO - Current joint values: [-0.12792441248893738, -0.4747267961502075, -0.03840704262256622, -0.02269524522125721, -1.0576682090759277, 0.0034878887236118317] +2024-12-18 14:40:12,485 - INFO - Moving arm to joint values: [-0.12792803756892682, -0.47472870349884033, -0.038403209298849106, -0.022691410034894943, -1.0576682090759277, 0.0034917236771434546] +2024-12-18 14:40:12,488 - INFO - Moving arm to joint values: [-0.22792803756892682, -0.47472870349884033, -0.038403209298849106, -0.022691410034894943, -1.0576682090759277, 0.0034917236771434546] +2024-12-18 14:40:39,115 - INFO - Checking for faults on the robot... +2024-12-18 14:40:42,118 - INFO - Successfully initialize the robot +2024-12-18 14:40:42,119 - INFO - Current joint values: [-0.22792845964431763, -0.47472870349884033, -0.03840704262256622, -0.022691410034894943, -1.0576682090759277, 0.0034878887236118317] +2024-12-18 14:45:31,107 - INFO - Checking for faults on the robot... +2024-12-18 14:45:34,110 - INFO - Successfully initialize the robot +2024-12-18 14:45:34,111 - INFO - Current joint values: [-0.1279282420873642, -0.4747267961502075, -0.03840896114706993, -0.022691410034894943, -1.057666301727295, 0.0034878887236118317] +2024-12-18 14:45:34,129 - INFO - Moving arm to joint values: [-0.12792803756892682, -0.47472870349884033, -0.038403209298849106, -0.022691410034894943, -1.0576682090759277, 0.0034917236771434546] +2024-12-18 14:45:34,132 - INFO - Moving arm to joint values: [-0.22792803756892682, -0.47472870349884033, -0.038403209298849106, -0.022691410034894943, -1.0576682090759277, 0.0034917236771434546] +2024-12-18 14:55:05,039 - INFO - Checking for faults on the robot... +2024-12-18 14:55:08,042 - INFO - Successfully initialize the robot +2024-12-18 14:55:08,044 - INFO - Current end effector pose: +2024-12-18 14:55:54,576 - INFO - Checking for faults on the robot... +2024-12-18 14:55:57,580 - INFO - Successfully initialize the robot +2024-12-18 14:55:57,581 - INFO - Current end effector pose: +2024-12-18 14:56:56,664 - INFO - Checking for faults on the robot... +2024-12-18 14:56:59,667 - INFO - Successfully initialize the robot +2024-12-18 14:56:59,669 - INFO - Current end effector pose: +2024-12-18 15:01:35,440 - INFO - Checking for faults on the robot... +2024-12-18 15:01:38,443 - INFO - Successfully initialize the robot +2024-12-18 15:01:38,445 - INFO - Current end effector pose: +2024-12-18 15:01:38,453 - ERROR - Failed to move end effector to goal pose: index 3 is out of bounds for axis 0 with size 3 +2024-12-18 15:01:55,409 - INFO - Checking for faults on the robot... +2024-12-18 15:01:58,413 - INFO - Successfully initialize the robot +2024-12-18 15:01:58,415 - INFO - Current end effector pose: +2024-12-18 15:01:58,417 - INFO - Moved end effector to pose: +2024-12-18 15:02:20,114 - INFO - Checking for faults on the robot... +2024-12-18 15:02:23,118 - INFO - Successfully initialize the robot +2024-12-18 15:02:23,120 - INFO - Current end effector pose: +2024-12-18 15:02:23,122 - INFO - Moved end effector to pose: +2024-12-18 15:02:35,737 - INFO - Checking for faults on the robot... +2024-12-18 15:02:38,740 - INFO - Successfully initialize the robot +2024-12-18 15:02:38,742 - INFO - Current end effector pose: +2024-12-18 15:02:38,744 - INFO - Moved end effector to pose: +2024-12-18 15:02:48,428 - INFO - Checking for faults on the robot... +2024-12-18 15:02:51,432 - INFO - Successfully initialize the robot +2024-12-18 15:02:51,435 - INFO - Current end effector pose: +2024-12-18 15:02:51,436 - INFO - Moved end effector to pose: +2024-12-18 15:04:23,813 - INFO - Checking for faults on the robot... +2024-12-18 15:04:26,817 - INFO - Successfully initialize the robot +2024-12-18 15:04:26,819 - INFO - Current end effector pose: +2024-12-18 15:04:26,821 - INFO - Moved end effector to pose: +2024-12-18 15:05:03,880 - INFO - Checking for faults on the robot... +2024-12-18 15:05:06,883 - INFO - Successfully initialize the robot +2024-12-18 15:05:06,885 - INFO - Current end effector pose: +2024-12-18 15:05:06,887 - INFO - Moved end effector to pose: +2024-12-18 15:05:20,891 - INFO - Checking for faults on the robot... +2024-12-18 15:05:23,895 - INFO - Successfully initialize the robot +2024-12-18 15:05:23,898 - INFO - Current end effector pose: +2024-12-18 15:05:23,900 - INFO - Moved end effector to pose: +2024-12-18 15:06:39,124 - INFO - Checking for faults on the robot... +2024-12-18 15:06:42,127 - INFO - Successfully initialize the robot +2024-12-18 15:06:42,129 - INFO - Current end effector pose: +2024-12-18 15:06:42,131 - INFO - Moved end effector to pose: +2024-12-18 15:07:12,280 - INFO - Checking for faults on the robot... +2024-12-18 15:07:15,284 - INFO - Successfully initialize the robot +2024-12-18 15:07:15,286 - INFO - Current end effector pose: +2024-12-18 15:07:15,287 - INFO - Moved end effector to pose: [0.41095602400000003, -0.089484787, 0.7598677060000001], [0.7035419144838481, -0.07091498202064685, 0.7035416845699785, 0.07091500519530484] +2024-12-18 15:10:30,859 - INFO - Checking for faults on the robot... +2024-12-18 15:10:33,863 - INFO - Successfully initialize the robot +2024-12-18 15:10:33,865 - INFO - Current end effector pose: +2024-12-18 15:10:33,866 - INFO - Moved end effector to pose: [0.420434235, -0.00581615, 0.759876038], [0.7071068218077394, -0.00032550013355177364, 0.7071065907288765, 0.0003255002399235735] +2024-12-18 15:11:02,831 - INFO - Checking for faults on the robot... +2024-12-18 15:11:05,835 - INFO - Successfully initialize the robot +2024-12-18 15:11:05,837 - INFO - Current end effector pose: +2024-12-18 15:11:05,839 - INFO - Moved end effector to pose: [0.420434235, -0.005816157, 0.5598760380000001], [0.7071068218077394, -0.00032550013355177364, 0.7071065907288765, 0.0003255002399235735] +2024-12-18 15:11:35,530 - INFO - Checking for faults on the robot... +2024-12-18 15:11:38,533 - INFO - Successfully initialize the robot +2024-12-18 15:11:38,535 - INFO - Current end effector pose: +2024-12-18 15:11:38,548 - INFO - Moved end effector to pose: [0.420432465, -0.006418663, 0.35980011000000006], [0.7071051997683803, -0.0015491469907030697, 0.7071049686900474, 0.001549147496956483] +2024-12-18 15:12:25,733 - INFO - Checking for faults on the robot... +2024-12-18 15:12:28,736 - INFO - Successfully initialize the robot +2024-12-18 15:12:28,738 - INFO - Current end effector pose: +2024-12-18 15:12:28,739 - INFO - Moved end effector to pose: [0.420432465, -0.006418671, 0.45980011], [0.7071051997683803, -0.0015491469907030697, 0.7071049686900474, 0.001549147496956483] +2024-12-18 15:12:48,752 - INFO - Checking for faults on the robot... +2024-12-18 15:12:51,756 - INFO - Successfully initialize the robot +2024-12-18 15:12:51,758 - INFO - Current end effector pose: +2024-12-18 15:12:51,759 - INFO - Moved end effector to pose: [0.420432465, -0.006418671, 0.45980011], [0.7071051997683803, -0.0015491469907030697, 0.7071049686900474, 0.001549147496956483] +2024-12-18 15:13:00,445 - INFO - Checking for faults on the robot... +2024-12-18 15:13:03,449 - INFO - Successfully initialize the robot +2024-12-18 15:13:03,451 - INFO - Current end effector pose: +2024-12-18 15:13:03,815 - INFO - Moved end effector to pose: [0.420432465, -0.006418671, 0.45980011], [0.7071051997683803, -0.0015491469907030697, 0.7071049686900474, 0.001549147496956483] +2024-12-18 15:13:25,774 - INFO - Checking for faults on the robot... +2024-12-18 15:13:28,777 - INFO - Successfully initialize the robot +2024-12-18 15:13:28,779 - INFO - Current end effector pose: +2024-12-18 15:13:28,795 - INFO - Moved end effector to pose: [0.420432465, -0.00641867, 0.559800079], [0.7071051997683803, -0.0015491469907030697, 0.7071049686900474, 0.001549147496956483] +2024-12-18 15:13:49,828 - INFO - Checking for faults on the robot... +2024-12-18 15:13:52,832 - INFO - Successfully initialize the robot +2024-12-18 15:13:52,834 - INFO - Current end effector pose: +2024-12-18 15:13:52,850 - INFO - Moved end effector to pose: [0.420432465, -0.00641867, 0.559800079], [0.7071051997683803, -0.0015491469907030697, 0.7071049686900474, 0.001549147496956483] +2024-12-18 15:14:07,443 - INFO - Checking for faults on the robot... +2024-12-18 15:14:10,446 - INFO - Successfully initialize the robot +2024-12-18 15:14:10,448 - INFO - Current end effector pose: +2024-12-18 15:14:10,449 - INFO - Moved end effector to pose: [0.42028936800000005, -0.012681033, 0.660812195], [-0.7049980142066017, 0.015339326301879369, -0.7089204044956668, -0.013204738659008564] +2024-12-18 15:20:57,806 - INFO - Checking for faults on the robot... +2024-12-18 15:21:00,810 - INFO - Successfully initialize the robot +2024-12-18 15:21:00,812 - INFO - Current end effector pose: +2024-12-18 15:21:00,813 - INFO - Current joint values: [-0.040353283286094666, -0.8794407844543457, -0.7431389093399048, -0.06232563778758049, 0.045714545994997025, 0.06825639307498932] +2024-12-18 15:21:00,814 - INFO - Converted joint values [-0.040353283286094666, -0.8794407844543457, -0.7431389093399048, -0.06232563778758049, 0.045714545994997025, 0.06825639307498932] to Cartesian Pose. +2024-12-18 15:21:57,496 - INFO - Checking for faults on the robot... +2024-12-18 15:22:00,499 - INFO - Successfully initialize the robot +2024-12-18 15:22:00,501 - INFO - Current end effector pose: +2024-12-18 15:22:00,502 - INFO - Current joint values: [-0.040353283286094666, -0.8794465065002441, -0.7431408166885376, -0.06232372298836708, 0.04571837931871414, 0.06825447827577591] +2024-12-18 15:22:00,503 - INFO - Converted joint values [-0.040353283286094666, -0.8794465065002441, -0.7431408166885376, -0.06232372298836708, 0.04571837931871414, 0.06825447827577591] to Cartesian Pose. +2024-12-18 15:25:35,901 - INFO - Checking for faults on the robot... +2024-12-18 15:25:38,904 - INFO - Successfully initialize the robot +2024-12-18 15:25:38,906 - INFO - Current end effector pose: +2024-12-18 15:25:38,907 - INFO - Current joint values: [-0.040355198085308075, -0.8794426918029785, -0.7431408166885376, -0.06232563778758049, 0.04571837931871414, 0.0682525560259819] +2024-12-18 15:25:38,908 - INFO - Converted joint values [-0.040355198085308075, -0.8794426918029785, -0.7431408166885376, -0.06232563778758049, 0.04571837931871414, 0.0682525560259819] to Cartesian Pose. +2024-12-18 15:26:18,610 - INFO - Checking for faults on the robot... +2024-12-18 15:26:21,614 - INFO - Successfully initialize the robot +2024-12-18 15:26:21,616 - INFO - Current end effector pose: +2024-12-18 15:26:21,617 - INFO - Current joint values: [-0.040355198085308075, -0.8794426918029785, -0.7431427240371704, -0.0623275563120842, 0.04571837931871414, 0.0682525560259819] +2024-12-18 15:26:21,619 - INFO - Converted joint values [-0.040355198085308075, -0.8794426918029785, -0.7431427240371704, -0.0623275563120842, 0.04571837931871414, 0.0682525560259819] to Cartesian Pose [ 0.24948594 -0.00561569 0.66118192] and [ 0.70471241 -0.01734275 0.70915838 0.01319249]. +2024-12-18 15:27:35,231 - INFO - Checking for faults on the robot... +2024-12-18 15:27:38,235 - INFO - Successfully initialize the robot +2024-12-18 15:27:38,237 - INFO - Current end effector pose: +2024-12-18 15:27:38,238 - INFO - Current joint values: [-0.027922285720705986, -0.4747248589992523, -0.038393620401620865, -0.022691410034894943, -1.0576778650283813, 0.003482136409729719] +2024-12-18 15:27:38,239 - INFO - Converted joint values [-0.027922285720705986, -0.4747248589992523, -0.038393620401620865, -0.022691410034894943, -1.0576778650283813, 0.003482136409729719] to Cartesian Pose [ 0.2501445 -0.00422106 0.36013102] and [ 7.07145719e-01 -1.75818220e-04 7.07045707e-01 5.59184046e-03]. +2024-12-18 15:38:34,821 - INFO - Checking for faults on the robot... +2024-12-18 15:38:37,825 - INFO - Successfully initialize the robot +2024-12-18 15:38:37,827 - INFO - Current end effector pose: +2024-12-18 15:38:37,828 - INFO - Current joint values: [-0.02792995609343052, -0.47473254799842834, -0.03839745745062828, -0.022691410034894943, -1.057666301727295, 0.0034878887236118317] +2024-12-18 15:38:37,830 - INFO - Converted joint values [-0.02792995609343052, -0.47473254799842834, -0.03839745745062828, -0.022691410034894943, -1.057666301727295, 0.0034878887236118317] to Cartesian Pose [ 0.25014534 -0.00422299 0.3601331 ] and [ 7.07145732e-01 -1.80529106e-04 7.07045687e-01 5.59264452e-03]. +2024-12-18 15:38:37,884 - ERROR - Error converting Cartesian to joint values: 'Bestman_Real_Xarm6' object has no attribute '_validate_joint_limits' +2024-12-18 15:39:28,165 - INFO - Checking for faults on the robot... +2024-12-18 15:39:31,169 - INFO - Successfully initialize the robot +2024-12-18 15:39:31,171 - INFO - Current end effector pose: +2024-12-18 15:39:31,172 - INFO - Current joint values: [-0.02792995609343052, -0.47473254799842834, -0.03839553892612457, -0.02269524522125721, -1.057666301727295, 0.0034878887236118317] +2024-12-18 15:39:31,174 - INFO - Converted joint values [-0.02792995609343052, -0.47473254799842834, -0.03839553892612457, -0.02269524522125721, -1.057666301727295, 0.0034878887236118317] to Cartesian Pose [ 0.25014499 -0.00422252 0.36013246] and [ 7.07146424e-01 -1.78695836e-04 7.07045000e-01 5.59210996e-03]. +2024-12-18 15:39:31,202 - INFO - Converted Cartesian position [0.420434235, -0.00581615, 0.359876038] to joint values [ 0. 0. -0.00379779 0.11826075 -0.63562283 -0.00222824 + -1.05343637 0. ]. +2024-12-18 15:43:24,534 - INFO - Checking for faults on the robot... +2024-12-18 15:43:27,537 - INFO - Successfully initialize the robot +2024-12-18 15:43:27,539 - INFO - Current end effector pose: +2024-12-18 15:43:27,540 - INFO - Current joint values: [-0.02792803756892681, -0.47473254799842834, -0.038393620401620865, -0.022691410034894943, -1.0576701164245605, 0.003489806316792965] +2024-12-18 15:43:27,542 - INFO - Converted joint values [-0.02792803756892681, -0.47473254799842834, -0.038393620401620865, -0.022691410034894943, -1.0576701164245605, 0.003489806316792965] to Cartesian Pose [ 0.25014435 -0.00422249 0.3601322 ] and [ 7.07145739e-01 -1.80540572e-04 7.07045691e-01 5.59124647e-03]. +2024-12-18 15:43:27,559 - INFO - Converted Cartesian position [0.420434235, -0.00581615, 0.359876038] to joint values [ 0. 0. -0.00379779 0.11826075 -0.63562283 -0.00222824 + -1.05343637 0. ]. +2024-12-18 15:48:21,264 - INFO - Checking for faults on the robot... +2024-12-18 15:48:24,267 - INFO - Successfully initialize the robot +2024-12-18 15:48:24,269 - INFO - Current end effector pose: +2024-12-18 15:48:24,270 - INFO - Current joint values: [-0.02792995609343052, -0.47473254799842834, -0.03839745745062828, -0.02269524522125721, -1.0576682090759277, 0.0034917236771434546] +2024-12-18 15:48:24,272 - INFO - Converted joint values [-0.02792995609343052, -0.47473254799842834, -0.03839745745062828, -0.02269524522125721, -1.0576682090759277, 0.0034917236771434546] to Cartesian Pose [ 0.2501452 -0.00422252 0.36013328] and [ 7.07145071e-01 -1.80030507e-04 7.07046363e-01 5.59077080e-03]. +2024-12-18 15:49:10,269 - INFO - Checking for faults on the robot... +2024-12-18 15:49:13,272 - INFO - Successfully initialize the robot +2024-12-18 15:49:13,274 - INFO - Current end effector pose: +2024-12-18 15:49:13,275 - INFO - Current joint values: [-0.02792803756892681, -0.47473254799842834, -0.038393620401620865, -0.02269332855939865, -1.0576682090759277, 0.003489806316792965] +2024-12-18 15:49:13,277 - INFO - Converted joint values [-0.02792803756892681, -0.47473254799842834, -0.038393620401620865, -0.02269332855939865, -1.0576682090759277, 0.003489806316792965] to Cartesian Pose [ 0.2501445 -0.00422226 0.36013201] and [ 7.07146421e-01 -1.79624382e-04 7.07045012e-01 5.59099063e-03]. +2024-12-18 15:49:31,135 - INFO - Checking for faults on the robot... +2024-12-18 15:49:34,139 - INFO - Successfully initialize the robot +2024-12-18 15:49:34,141 - INFO - Current end effector pose: +2024-12-18 15:49:34,142 - INFO - Current joint values: [-0.02792995609343052, -0.47473445534706116, -0.03839745745062828, -0.02269524522125721, -1.0576682090759277, 0.0034917236771434546] +2024-12-18 15:49:34,144 - INFO - Converted joint values [-0.02792995609343052, -0.47473445534706116, -0.03839745745062828, -0.02269524522125721, -1.0576682090759277, 0.0034917236771434546] to Cartesian Pose [ 0.25014502 -0.00422252 0.36013376] and [ 7.07144397e-01 -1.80017004e-04 7.07047037e-01 5.59078946e-03]. +2024-12-18 15:57:31,038 - INFO - Checking for faults on the robot... +2024-12-18 15:57:34,040 - INFO - Successfully initialize the robot +2024-12-18 15:57:34,042 - INFO - Current end effector pose: +2024-12-18 15:57:34,043 - INFO - Current joint values: [-0.02792995609343052, -0.47473254799842834, -0.03839745745062828, -0.02269524522125721, -1.0576682090759277, 0.003489806316792965] +2024-12-18 15:57:34,046 - INFO - Converted joint values [-0.02792995609343052, -0.47473254799842834, -0.03839745745062828, -0.02269524522125721, -1.0576682090759277, 0.003489806316792965] to Cartesian Pose [ 0.2501452 -0.00422252 0.36013328] and [ 7.07145071e-01 -1.79352581e-04 7.07046358e-01 5.59144864e-03]. +2024-12-18 16:09:33,916 - INFO - Checking for faults on the robot... +2024-12-18 16:09:36,920 - INFO - Successfully initialize the robot +2024-12-18 16:09:36,922 - INFO - Current end effector pose: +2024-12-18 16:09:36,923 - INFO - Current joint values: [-0.02792995609343052, -0.47473445534706116, -0.03839745745062828, -0.02269524522125721, -1.0576701164245605, 0.003489806316792965] +2024-12-18 16:09:36,925 - INFO - Converted joint values [-0.02792995609343052, -0.47473445534706116, -0.03839745745062828, -0.02269524522125721, -1.0576701164245605, 0.003489806316792965] to Cartesian Pose [ 0.25014488 -0.00422252 0.36013394] and [ 7.07143723e-01 -1.79331395e-04 7.07047706e-01 5.59146511e-03]. +2024-12-18 16:14:19,449 - INFO - Checking for faults on the robot... +2024-12-18 16:14:22,453 - INFO - Successfully initialize the robot +2024-12-18 16:14:22,454 - INFO - Current end effector pose: +2024-12-18 16:14:22,454 - INFO - Current joint values: [-0.02792995609343052, -0.47473445534706116, -0.03839745745062828, -0.022691410034894943, -1.057666301727295, 0.003489806316792965] +2024-12-18 16:14:22,454 - INFO - Converted joint values [-0.02792995609343052, -0.47473445534706116, -0.03839745745062828, -0.022691410034894943, -1.057666301727295, 0.003489806316792965] to Cartesian Pose [ 0.25014516 -0.00422299 0.36013358] and [ 7.07145057e-01 -1.81193613e-04 7.07046367e-01 5.59198527e-03]. +2024-12-18 16:15:05,156 - INFO - Checking for faults on the robot... +2024-12-18 16:15:08,159 - INFO - Successfully initialize the robot +2024-12-18 16:15:08,161 - INFO - Current end effector pose: +2024-12-18 16:15:08,162 - INFO - Current joint values: [-0.02792995609343052, -0.47473445534706116, -0.038393620401620865, -0.02269524522125721, -1.0576682090759277, 0.003489806316792965] +2024-12-18 16:15:08,164 - INFO - Converted joint values [-0.02792995609343052, -0.47473445534706116, -0.038393620401620865, -0.02269524522125721, -1.0576682090759277, 0.003489806316792965] to Cartesian Pose [ 0.25014431 -0.0042225 0.36013248] and [ 7.07145753e-01 -1.79366242e-04 7.07045676e-01 5.59142976e-03]. +2024-12-18 16:18:14,396 - INFO - Checking for faults on the robot... +2024-12-18 16:18:17,399 - INFO - Successfully initialize the robot +2024-12-18 16:18:17,400 - INFO - Current end effector pose: +2024-12-18 16:18:17,400 - INFO - Current joint values: [-0.02792803756892681, -0.47473254799842834, -0.038393620401620865, -0.02269524522125721, -1.0576701164245605, 0.003489806316792965] +2024-12-18 16:18:17,400 - INFO - Converted joint values [-0.02792803756892681, -0.47473254799842834, -0.038393620401620865, -0.02269524522125721, -1.0576701164245605, 0.003489806316792965] to Cartesian Pose [ 0.25014436 -0.00422202 0.36013219] and [ 7.07145753e-01 -1.78693723e-04 7.07045681e-01 5.59073067e-03]. +2024-12-18 16:20:30,031 - INFO - Checking for faults on the robot... +2024-12-18 16:20:33,035 - INFO - Successfully initialize the robot +2024-12-18 16:20:33,037 - INFO - Current end effector pose: +2024-12-18 16:20:33,038 - INFO - Current joint values: [-0.02792995609343052, -0.47473445534706116, -0.03839745745062828, -0.022691410034894943, -1.0576701164245605, 0.003489806316792965] +2024-12-18 16:20:33,040 - INFO - Converted joint values [-0.02792995609343052, -0.47473445534706116, -0.03839745745062828, -0.022691410034894943, -1.0576701164245605, 0.003489806316792965] to Cartesian Pose [ 0.25014487 -0.00422298 0.36013395] and [ 7.07143709e-01 -1.81178245e-04 7.07047715e-01 5.59198090e-03]. +2024-12-18 16:21:02,151 - INFO - Checking for faults on the robot... +2024-12-18 16:21:05,155 - INFO - Successfully initialize the robot +2024-12-18 16:21:05,157 - INFO - Current end effector pose: +2024-12-18 16:21:05,158 - INFO - Current joint values: [-0.02792803756892681, -0.47473254799842834, -0.03839745745062828, -0.02269524522125721, -1.0576701164245605, 0.003489806316792965] +2024-12-18 16:21:05,160 - INFO - Converted joint values [-0.02792803756892681, -0.47473254799842834, -0.03839745745062828, -0.02269524522125721, -1.0576701164245605, 0.003489806316792965] to Cartesian Pose [ 0.25014506 -0.00422204 0.36013347] and [ 7.07144397e-01 -1.78666561e-04 7.07047038e-01 5.59076821e-03]. +2024-12-18 16:23:58,176 - INFO - Checking for faults on the robot... +2024-12-18 16:24:01,180 - INFO - Successfully initialize the robot +2024-12-18 16:24:01,182 - INFO - Current end effector pose: +2024-12-18 16:24:01,183 - INFO - Current joint values: [-0.02792995609343052, -0.47473254799842834, -0.03839745745062828, -0.02269524522125721, -1.0576682090759277, 0.003489806316792965] +2024-12-18 16:24:01,185 - INFO - Converted joint values [-0.02792995609343052, -0.47473254799842834, -0.03839745745062828, -0.02269524522125721, -1.0576682090759277, 0.003489806316792965] to Cartesian Pose [ 0.2501452 -0.00422252 0.36013328] and [ 7.07145071e-01 -1.79352581e-04 7.07046358e-01 5.59144864e-03]. +2024-12-18 16:25:25,381 - INFO - Checking for faults on the robot... +2024-12-18 16:25:28,384 - INFO - Successfully initialize the robot +2024-12-18 16:25:28,386 - INFO - Current end effector pose: +2024-12-18 16:25:28,387 - INFO - Current joint values: [-0.3434295356273651, -0.8785529732704163, -0.3339322805404663, -0.7777052521705627, -0.4970596432685852, 0.7144553661346436] +2024-12-18 16:25:28,389 - INFO - Converted joint values [-0.3434295356273651, -0.8785529732704163, -0.3339322805404663, -0.7777052521705627, -0.4970596432685852, 0.7144553661346436] to Cartesian Pose [ 0.25007871 -0.00518193 0.50048514] and [7.03587891e-01 9.64928697e-05 7.10606195e-01 1.70444139e-03]. +2024-12-18 16:28:19,565 - INFO - Checking for faults on the robot... +2024-12-18 16:28:22,569 - INFO - Successfully initialize the robot +2024-12-18 16:28:22,571 - INFO - Current end effector pose: +2024-12-18 16:28:22,572 - INFO - Current joint values: [-0.3434295356273651, -0.8785548806190491, -0.3339361250400543, -0.7777071595191956, -0.4970596432685852, 0.7144553661346436] +2024-12-18 16:28:22,574 - INFO - Converted joint values [-0.3434295356273651, -0.8785548806190491, -0.3339361250400543, -0.7777071595191956, -0.4970596432685852, 0.7144553661346436] to Cartesian Pose [ 0.25007825 -0.00518161 0.50048707] and [7.03586178e-01 9.80098062e-05 7.10607889e-01 1.70547698e-03]. +2024-12-18 16:29:51,847 - INFO - Checking for faults on the robot... +2024-12-18 16:29:54,851 - INFO - Successfully initialize the robot +2024-12-18 16:29:54,853 - INFO - Current end effector pose: +2024-12-18 16:29:54,854 - INFO - Current joint values: [-0.3434333801269531, -0.8785567879676819, -0.3339322805404663, -0.7777109742164612, -0.49706345796585083, 0.7144534587860107] +2024-12-18 16:29:54,856 - INFO - Converted joint values [-0.3434333801269531, -0.8785567879676819, -0.3339322805404663, -0.7777109742164612, -0.49706345796585083, 0.7144534587860107] to Cartesian Pose [ 0.25007756 -0.00518186 0.50048589] and [7.03586229e-01 9.96554519e-05 7.10607834e-01 1.70711965e-03]. +2024-12-18 16:29:55,215 - ERROR - Failed to move arm to joint values: [ 0. -4.00149035 -0.19108453 -0.79580863 -0.32920702 -0.41989968 + -0.48213191 0.37667688]. Error code: -8 +2024-12-18 16:30:30,315 - INFO - Checking for faults on the robot... +2024-12-18 16:30:33,317 - INFO - Successfully initialize the robot +2024-12-18 16:30:33,319 - INFO - Current end effector pose: +2024-12-18 16:30:33,320 - INFO - Current joint values: [-0.3434295356273651, -0.8785548806190491, -0.3339322805404663, -0.7777109742164612, -0.49706345796585083, 0.7144553661346436] +2024-12-18 16:30:33,322 - INFO - Converted joint values [-0.3434295356273651, -0.8785548806190491, -0.3339322805404663, -0.7777109742164612, -0.49706345796585083, 0.7144553661346436] to Cartesian Pose [ 0.250078 -0.00518105 0.50048544] and [7.03586868e-01 1.00110267e-04 7.10607207e-01 1.70484998e-03]. +2024-12-18 16:30:33,392 - INFO - Moving arm to joint values: [-0.19108453 -0.79580863 -0.32920702 -0.41989968 -0.48213191 0.37667688] +2024-12-18 16:30:40,356 - INFO - Checking for faults on the robot... +2024-12-18 16:30:43,359 - INFO - Successfully initialize the robot +2024-12-18 16:30:43,361 - INFO - Current end effector pose: +2024-12-18 16:30:43,362 - INFO - Current joint values: [-0.19108223915100098, -0.7958043217658997, -0.3292037844657898, -0.41989463567733765, -0.4821263253688812, 0.3766689896583557] +2024-12-18 16:30:43,363 - INFO - Converted joint values [-0.19108223915100098, -0.7958043217658997, -0.3292037844657898, -0.41989463567733765, -0.4821263253688812, 0.3766689896583557] to Cartesian Pose [ 0.27043563 -0.00567838 0.49721064] and [ 7.07110973e-01 -3.25483434e-04 7.07102439e-01 3.26887858e-04]. +2024-12-18 16:30:43,415 - INFO - Moving arm to joint values: [-0.11595228 -0.70094913 -0.36571698 -0.24654865 -0.51095967 0.2169669 ] +2024-12-18 16:30:47,320 - INFO - Checking for faults on the robot... +2024-12-18 16:30:50,323 - INFO - Successfully initialize the robot +2024-12-18 16:30:50,325 - INFO - Current end effector pose: +2024-12-18 16:30:50,326 - INFO - Current joint values: [-0.11594785749912262, -0.7009525299072266, -0.365714430809021, -0.24654905498027802, -0.5109613537788391, 0.21696048974990845] +2024-12-18 16:30:50,328 - INFO - Converted joint values [-0.11594785749912262, -0.7009525299072266, -0.365714430809021, -0.24654905498027802, -0.5109613537788391, 0.21696048974990845] to Cartesian Pose [ 0.29081091 -0.00592617 0.49499266] and [ 0.70926564 0.00074095 0.70493846 -0.00186094]. +2024-12-18 16:30:50,385 - INFO - Moving arm to joint values: [-0.09228574 -0.60722254 -0.41695144 -0.19691551 -0.54535215 0.17136337] +2024-12-18 16:31:44,365 - INFO - Checking for faults on the robot... +2024-12-18 16:31:47,369 - INFO - Successfully initialize the robot +2024-12-18 16:31:47,371 - INFO - Current end effector pose: +2024-12-18 16:31:47,372 - INFO - Current joint values: [-0.09229003638029099, -0.6072282195091248, -0.41695132851600647, -0.1969171166419983, -0.5453512668609619, 0.17136099934577942] +2024-12-18 16:31:47,374 - INFO - Converted joint values [-0.09229003638029099, -0.6072282195091248, -0.41695132851600647, -0.1969171166419983, -0.5453512668609619, 0.17136099934577942] to Cartesian Pose [ 0.31117608 -0.0061466 0.4933169 ] and [ 0.71057051 0.00211385 0.70360843 -0.00450106]. +2024-12-18 16:31:47,428 - INFO - Moving arm to joint values: [-0.08646262 -0.5351637 -0.46150452 -0.1895752 -0.56940426 0.16462447] +2024-12-18 16:31:58,814 - INFO - Checking for faults on the robot... +2024-12-18 16:32:01,818 - INFO - Successfully initialize the robot +2024-12-18 16:32:01,820 - INFO - Current end effector pose: +2024-12-18 16:32:01,821 - INFO - Current joint values: [-0.08646474778652191, -0.5351617932319641, -0.4615115523338318, -0.18957893550395966, -0.5694059729576111, 0.16461722552776337] +2024-12-18 16:32:01,823 - INFO - Converted joint values [-0.08646474778652191, -0.5351617932319641, -0.4615115523338318, -0.18957893550395966, -0.5694059729576111, 0.16461722552776337] to Cartesian Pose [ 0.32750492 -0.00638833 0.49186177] and [ 0.71161951 0.00341994 0.70251796 -0.00738136]. +2024-12-18 16:32:01,886 - INFO - Moving arm to joint values: [-0.11727354 -0.57439067 -0.43518294 -0.26255545 -0.56093625 0.22992395] +2024-12-18 16:32:05,003 - INFO - Checking for faults on the robot... +2024-12-18 16:32:08,006 - INFO - Successfully initialize the robot +2024-12-18 16:32:08,008 - INFO - Current end effector pose: +2024-12-18 16:32:08,009 - INFO - Current joint values: [-0.1172804981470108, -0.5743914246559143, -0.435186505317688, -0.26255616545677185, -0.5609422326087952, 0.22992071509361267] +2024-12-18 16:32:08,011 - INFO - Converted joint values [-0.1172804981470108, -0.5743914246559143, -0.435186505317688, -0.26255616545677185, -0.5609422326087952, 0.22992071509361267] to Cartesian Pose [ 0.31780266 -0.00713857 0.49084825] and [ 0.71262516 0.00459916 0.70145311 -0.01038105]. +2024-12-18 16:32:08,068 - INFO - Moving arm to joint values: [-0.15492367 -0.61383727 -0.41136466 -0.35116185 -0.55333455 0.30944015] +2024-12-18 16:32:15,447 - INFO - Checking for faults on the robot... +2024-12-18 16:32:18,448 - INFO - Successfully initialize the robot +2024-12-18 16:32:18,450 - INFO - Current end effector pose: +2024-12-18 16:32:18,451 - INFO - Current joint values: [-0.15493014454841614, -0.6138396859169006, -0.41136571764945984, -0.3511684834957123, -0.5533298850059509, 0.30944034457206726] +2024-12-18 16:32:18,453 - INFO - Converted joint values [-0.15493014454841614, -0.6138396859169006, -0.41136571764945984, -0.3511684834957123, -0.5533298850059509, 0.30944034457206726] to Cartesian Pose [ 0.30807579 -0.00785491 0.48961191] and [ 0.71400726 0.00570048 0.69999012 -0.01322784]. +2024-12-18 16:32:18,508 - INFO - Moving arm to joint values: [-0.14425605 -0.56811725 -0.43563316 -0.33252851 -0.56697177 0.29251705] +2024-12-18 16:32:24,363 - INFO - Checking for faults on the robot... +2024-12-18 16:32:27,366 - INFO - Successfully initialize the robot +2024-12-18 16:32:27,368 - INFO - Current end effector pose: +2024-12-18 16:32:27,369 - INFO - Current joint values: [-0.1442517191171646, -0.5681136250495911, -0.43563327193260193, -0.332524836063385, -0.5669784545898438, 0.2925109565258026] +2024-12-18 16:32:27,371 - INFO - Converted joint values [-0.1442517191171646, -0.5681136250495911, -0.43563327193260193, -0.332524836063385, -0.5669784545898438, 0.2925109565258026] to Cartesian Pose [ 0.31830267 -0.00790493 0.48761692] and [ 0.71582114 0.00664203 0.69807135 -0.01588601]. +2024-12-18 16:32:27,424 - INFO - Moving arm to joint values: [-0.1461244 -0.54412963 -0.44837743 -0.3419138 -0.57480053 0.30096639] +2024-12-18 16:32:33,877 - INFO - Checking for faults on the robot... +2024-12-18 16:32:36,881 - INFO - Successfully initialize the robot +2024-12-18 16:32:36,883 - INFO - Current end effector pose: +2024-12-18 16:32:36,884 - INFO - Current joint values: [-0.14612509310245514, -0.5441317558288574, -0.4483787417411804, -0.341922402381897, -0.5748094320297241, 0.3009631931781769] +2024-12-18 16:32:36,886 - INFO - Converted joint values [-0.14612509310245514, -0.5441317558288574, -0.4483787417411804, -0.341922402381897, -0.5748094320297241, 0.3009631931781769] to Cartesian Pose [ 0.32350733 -0.00812022 0.48581259] and [ 0.71753681 0.00755203 0.69623051 -0.01862699]. +2024-12-18 16:32:36,948 - INFO - Moving arm to joint values: [-0.15797634 -0.54111601 -0.44860192 -0.37300491 -0.57753913 0.32885722] +2024-12-18 16:32:46,193 - INFO - Checking for faults on the robot... +2024-12-18 16:32:49,197 - INFO - Successfully initialize the robot +2024-12-18 16:32:49,199 - INFO - Current end effector pose: +2024-12-18 16:32:49,200 - INFO - Current joint values: [-0.15798276662826538, -0.5411117076873779, -0.4486011862754822, -0.3730066120624542, -0.5775476098060608, 0.3288567066192627] +2024-12-18 16:32:49,201 - INFO - Converted joint values [-0.15798276662826538, -0.5411117076873779, -0.4486011862754822, -0.3730066120624542, -0.5775476098060608, 0.3288567066192627] to Cartesian Pose [ 0.32369599 -0.00852857 0.48413646] and [ 0.7192974 0.00838029 0.69432259 -0.02138135]. +2024-12-18 16:32:49,256 - INFO - Moving arm to joint values: [-0.16911621 -0.53815818 -0.44864048 -0.40244552 -0.58010083 0.35528901] +2024-12-18 16:33:04,626 - INFO - Checking for faults on the robot... +2024-12-18 16:33:07,630 - INFO - Successfully initialize the robot +2024-12-18 16:33:07,631 - INFO - Current end effector pose: +2024-12-18 16:33:07,631 - INFO - Current joint values: [-0.16911371052265167, -0.5381645560264587, -0.44864335656166077, -0.40245136618614197, -0.5801016688346863, 0.35528528690338135] +2024-12-18 16:33:07,632 - INFO - Converted joint values [-0.16911371052265167, -0.5381645560264587, -0.44864335656166077, -0.40245136618614197, -0.5801016688346863, 0.35528528690338135] to Cartesian Pose [ 0.32385438 -0.0089123 0.48236601] and [ 0.72119884 0.00912526 0.69224923 -0.02408253]. +2024-12-18 16:33:07,683 - INFO - Moving arm to joint values: [-0.16928958 -0.5169392 -0.45921082 -0.40812183 -0.58592702 0.36059332] +2024-12-18 16:33:15,338 - INFO - Checking for faults on the robot... +2024-12-18 16:33:18,340 - INFO - Successfully initialize the robot +2024-12-18 16:33:18,342 - INFO - Current end effector pose: +2024-12-18 16:33:18,343 - INFO - Current joint values: [-0.16929012537002563, -0.516936182975769, -0.4592105746269226, -0.4081290066242218, -0.585932731628418, 0.36059093475341797] +2024-12-18 16:33:18,345 - INFO - Converted joint values [-0.16929012537002563, -0.516936182975769, -0.4592105746269226, -0.4081290066242218, -0.585932731628418, 0.36059093475341797] to Cartesian Pose [ 0.32847731 -0.00905009 0.48028277] and [ 0.72323905 0.00976548 0.69001115 -0.0267306 ]. +2024-12-18 16:33:18,400 - INFO - Moving arm to joint values: [-0.17476017 -0.50605171 -0.46373149 -0.42520966 -0.58952449 0.3760937 ] +2024-12-18 16:33:24,726 - INFO - Checking for faults on the robot... +2024-12-18 16:33:27,730 - INFO - Successfully initialize the robot +2024-12-18 16:33:27,732 - INFO - Current end effector pose: +2024-12-18 16:33:27,733 - INFO - Current joint values: [-0.1747664362192154, -0.5060487389564514, -0.463731974363327, -0.42521756887435913, -0.5895337462425232, 0.3760937452316284] +2024-12-18 16:33:27,734 - INFO - Converted joint values [-0.1747664362192154, -0.5060487389564514, -0.463731974363327, -0.42521756887435913, -0.5895337462425232, 0.3760937452316284] to Cartesian Pose [ 0.33058143 -0.00931579 0.47829782] and [ 0.72529393 0.01034688 0.68773421 -0.02938234]. +2024-12-18 16:33:27,813 - INFO - Moving arm to joint values: [-0.18196304 -0.49928816 -0.46568393 -0.44594024 -0.59216837 0.39483737] +2024-12-18 16:34:13,434 - INFO - Checking for faults on the robot... +2024-12-18 16:34:16,438 - INFO - Successfully initialize the robot +2024-12-18 16:34:16,439 - INFO - Current end effector pose: +2024-12-18 16:34:16,440 - INFO - Current joint values: [-0.18196846544742584, -0.49928581714630127, -0.4656839668750763, -0.44594356417655945, -0.5921702980995178, 0.39483514428138733] +2024-12-18 16:34:16,441 - INFO - Converted joint values [-0.18196846544742584, -0.49928581714630127, -0.4656839668750763, -0.44594356417655945, -0.5921702980995178, 0.39483514428138733] to Cartesian Pose [ 0.33166406 -0.00962803 0.47630497] and [ 0.72742454 0.0108522 0.68535496 -0.03200545]. +2024-12-18 16:35:51,584 - INFO - Checking for faults on the robot... +2024-12-18 16:35:54,587 - INFO - Successfully initialize the robot +2024-12-18 16:35:54,589 - INFO - Current end effector pose: +2024-12-18 16:35:54,590 - INFO - Current joint values: [-0.18196655809879303, -0.49928581714630127, -0.4656878113746643, -0.4459473788738251, -0.5921741127967834, 0.39483514428138733] +2024-12-18 16:35:54,592 - INFO - Converted joint values [-0.18196655809879303, -0.49928581714630127, -0.4656878113746643, -0.4459473788738251, -0.5921741127967834, 0.39483514428138733] to Cartesian Pose [ 0.33166402 -0.00962691 0.47630677] and [ 0.72742225 0.01085547 0.68535732 -0.03200601]. +2024-12-18 16:35:54,940 - INFO - Moving arm to joint values: [-0.15140485 -0.41769749 -0.5152589 -0.3872068 -0.60674441 0.3440546 ] +2024-12-18 16:36:12,100 - INFO - Checking for faults on the robot... +2024-12-18 16:36:15,104 - INFO - Successfully initialize the robot +2024-12-18 16:36:15,106 - INFO - Current end effector pose: +2024-12-18 16:36:15,106 - INFO - Current joint values: [-0.027920367196202278, -0.47473636269569397, -0.03839937224984169, -0.02268374152481556, -1.0576778650283813, 0.003482136409729719] +2024-12-18 16:36:15,106 - INFO - Converted joint values [-0.027920367196202278, -0.47473636269569397, -0.03839937224984169, -0.02268374152481556, -1.0576778650283813, 0.003482136409729719] to Cartesian Pose [ 0.25014449 -0.00422151 0.36013582] and [ 7.07139593e-01 -1.78710583e-04 7.07051830e-01 5.59236235e-03]. +2024-12-18 16:36:15,159 - INFO - Moving arm to joint values: [-0.04150686 -0.37307685 -0.10900079 -0.04580389 -1.08914953 0.02123348] +2024-12-18 16:36:28,995 - INFO - Checking for faults on the robot... +2024-12-18 16:36:31,998 - INFO - Successfully initialize the robot +2024-12-18 16:36:31,999 - INFO - Current end effector pose: +2024-12-18 16:36:32,000 - INFO - Current joint values: [-0.041505686938762665, -0.37307947874069214, -0.1090046763420105, -0.04580466449260712, -1.0891493558883667, 0.021228376775979996] +2024-12-18 16:36:32,001 - INFO - Converted joint values [-0.041505686938762665, -0.37307947874069214, -0.1090046763420105, -0.04580466449260712, -1.0891493558883667, 0.021228376775979996] to Cartesian Pose [ 0.27043482 -0.00567766 0.35987804] and [ 7.07104589e-01 -3.22816393e-04 7.07108824e-01 3.26870088e-04]. +2024-12-18 16:36:32,054 - INFO - Moving arm to joint values: [-0.05037627 -0.28696404 -0.17889024 -0.0631509 -1.10506496 0.03647826] +2024-12-18 16:36:37,808 - INFO - Checking for faults on the robot... +2024-12-18 16:36:40,811 - INFO - Successfully initialize the robot +2024-12-18 16:36:40,813 - INFO - Current end effector pose: +2024-12-18 16:36:40,814 - INFO - Current joint values: [-0.05038359761238098, -0.28696754574775696, -0.1788966804742813, -0.06315974146127701, -1.1050740480422974, 0.03647806495428085] +2024-12-18 16:36:40,816 - INFO - Converted joint values [-0.05038359761238098, -0.28696754574775696, -0.1788966804742813, -0.06315974146127701, -1.1050740480422974, 0.03647806495428085] to Cartesian Pose [ 0.29066552 -0.00702262 0.35972585] and [ 0.70733923 -0.0007357 0.70685617 -0.00500344]. +2024-12-18 16:36:40,878 - INFO - Moving arm to joint values: [-0.05653082 -0.21081827 -0.24860129 -0.07734193 -1.11098133 0.05078134] +2024-12-18 16:36:43,490 - INFO - Checking for faults on the robot... +2024-12-18 16:36:46,493 - INFO - Successfully initialize the robot +2024-12-18 16:36:46,494 - INFO - Current end effector pose: +2024-12-18 16:36:46,494 - INFO - Current joint values: [-0.05652719363570213, -0.21082456409931183, -0.2485988438129425, -0.07733947783708572, -1.1109778881072998, 0.050778597593307495] +2024-12-18 16:36:46,494 - INFO - Converted joint values [-0.05652719363570213, -0.21082456409931183, -0.2485988438129425, -0.07733947783708572, -1.1109778881072998, 0.050778597593307495] to Cartesian Pose [ 0.31085532 -0.00825336 0.35957856] and [ 0.70766509 -0.00132531 0.70647113 -0.01033982]. +2024-12-18 16:36:46,542 - INFO - Moving arm to joint values: [-0.06093932 -0.14172297 -0.31845313 -0.08960802 -1.109535 0.06478463] +2024-12-18 16:36:48,879 - INFO - Checking for faults on the robot... +2024-12-18 16:36:51,882 - INFO - Successfully initialize the robot +2024-12-18 16:36:51,884 - INFO - Current end effector pose: +2024-12-18 16:36:51,885 - INFO - Current joint values: [-0.06093738600611687, -0.14172640442848206, -0.31845441460609436, -0.08960748463869095, -1.1095378398895264, 0.06478192657232285] +2024-12-18 16:36:51,887 - INFO - Converted joint values [-0.06093738600611687, -0.14172640442848206, -0.31845441460609436, -0.08960748463869095, -1.1095378398895264, 0.06478192657232285] to Cartesian Pose [ 0.3310143 -0.0093756 0.35943286] and [ 0.70804854 -0.00203371 0.70598757 -0.01564233]. +2024-12-18 16:36:51,941 - INFO - Moving arm to joint values: [-0.06415481 -0.07771941 -0.3887083 -0.10063938 -1.10246046 0.07878964] +2024-12-18 16:36:57,867 - INFO - Checking for faults on the robot... +2024-12-18 16:37:00,871 - INFO - Successfully initialize the robot +2024-12-18 16:37:00,873 - INFO - Current end effector pose: +2024-12-18 16:37:00,874 - INFO - Current joint values: [-0.06415874511003494, -0.07771913707256317, -0.38871073722839355, -0.10064639896154404, -1.102460503578186, 0.0787871703505516] +2024-12-18 16:37:00,876 - INFO - Converted joint values [-0.06415874511003494, -0.07771913707256317, -0.38871073722839355, -0.10064639896154404, -1.102460503578186, 0.0787871703505516] to Cartesian Pose [ 0.35114812 -0.01039105 0.35927266] and [ 0.70849179 -0.00282239 0.70540428 -0.02088591]. +2024-12-18 16:37:00,942 - INFO - Moving arm to joint values: [-0.06651505 -0.01744301 -0.45958402 -0.11086321 -1.09092479 0.09294348] +2024-12-18 16:41:29,206 - INFO - Checking for faults on the robot... +2024-12-18 16:41:32,210 - INFO - Successfully initialize the robot +2024-12-18 16:41:32,212 - INFO - Current end effector pose: +2024-12-18 16:41:32,213 - INFO - Current joint values: [-0.043637920171022415, -0.28797996044158936, -0.21119080483913422, -3.834952167380834e-06, 0.4991611838340759, 0.04014044255018234] +2024-12-18 16:41:32,214 - INFO - Converted joint values [-0.043637920171022415, -0.28797996044158936, -0.21119080483913422, -3.834952167380834e-06, 0.4991611838340759, 0.04014044255018234] to Cartesian Pose [ 0.27822749 -0.01214917 0.19437073] and [ 9.99122847e-01 -4.18752499e-02 4.78268035e-06 4.59945902e-06]. +2024-12-18 16:41:32,267 - INFO - Moving arm to joint values: [-0.05277871 -0.18336451 -0.19883586 -0.01302102 0.38214986 0.04091448] +2024-12-18 16:41:36,971 - INFO - Checking for faults on the robot... +2024-12-18 16:41:39,975 - INFO - Successfully initialize the robot +2024-12-18 16:41:39,977 - INFO - Current end effector pose: +2024-12-18 16:41:39,978 - INFO - Current joint values: [-0.052786197513341904, -0.1833624690771103, -0.1988326758146286, -0.013029249384999275, 0.38213953375816345, 0.040913183242082596] +2024-12-18 16:41:39,980 - INFO - Converted joint values [-0.052786197513341904, -0.1833624690771103, -0.1988326758146286, -0.013029249384999275, 0.38213953375816345, 0.040913183242082596] to Cartesian Pose [ 0.27601605 -0.01413562 0.17060509] and [ 9.99164632e-01 -4.07935847e-02 1.33361110e-05 2.43329214e-03]. +2024-12-18 16:41:40,047 - INFO - Moving arm to joint values: [-0.06469863 -0.06751967 -0.19611212 -0.03225192 0.26365645 0.04633606] +2024-12-18 16:42:11,943 - INFO - Checking for faults on the robot... +2024-12-18 16:42:14,947 - INFO - Successfully initialize the robot +2024-12-18 16:42:14,950 - INFO - Current end effector pose: +2024-12-18 16:42:14,951 - INFO - Current joint values: [0.8804762363433838, -0.4831387400627136, -0.43810874223709106, 1.4105087518692017, -0.8393310308456421, -0.9861693978309631] +2024-12-18 16:42:14,953 - INFO - Converted joint values [0.8804762363433838, -0.4831387400627136, -0.43810874223709106, 1.4105087518692017, -0.8393310308456421, -0.9861693978309631] to Cartesian Pose [0.2548577 0.11798433 0.40900307] and [0.80912723 0.00425489 0.58760083 0.00450348]. +2024-12-18 16:42:15,324 - INFO - Moving arm to joint values: [ 0.79357938 -0.42553265 -0.43929476 1.26982862 -0.79572055 -0.86372619] +2024-12-18 16:42:19,369 - INFO - Checking for faults on the robot... +2024-12-18 16:42:22,373 - INFO - Successfully initialize the robot +2024-12-18 16:42:22,375 - INFO - Current end effector pose: +2024-12-18 16:42:22,376 - INFO - Current joint values: [0.793576180934906, -0.42553776502609253, -0.43929949402809143, 1.2698273658752441, -0.7957180142402649, -0.8637290000915527] +2024-12-18 16:42:22,378 - INFO - Converted joint values [0.793576180934906, -0.42553776502609253, -0.43929949402809143, 1.2698273658752441, -0.7957180142402649, -0.8637290000915527] to Cartesian Pose [0.27701032 0.11478789 0.40380408] and [ 8.03998745e-01 -2.80538309e-04 5.94630817e-01 3.61946020e-04]. +2024-12-18 16:42:22,444 - INFO - Moving arm to joint values: [ 0.70922947 -0.36312311 -0.44843231 1.12093982 -0.76635632 -0.73652297] +2024-12-18 16:45:18,128 - INFO - Checking for faults on the robot... +2024-12-18 16:45:21,131 - INFO - Successfully initialize the robot +2024-12-18 16:45:21,133 - INFO - Current end effector pose: +2024-12-18 16:45:21,134 - INFO - Current joint values: [-0.02792803756892681, -0.47473445534706116, -0.038393620401620865, -0.02269524522125721, -1.0576759576797485, 0.003489806316792965] +2024-12-18 16:45:21,136 - INFO - Converted joint values [-0.02792803756892681, -0.47473445534706116, -0.038393620401620865, -0.02269524522125721, -1.0576759576797485, 0.003489806316792965] to Cartesian Pose [ 0.25014373 -0.00422201 0.36013323] and [ 7.07143014e-01 -1.78656694e-04 7.07048421e-01 5.59074264e-03]. +2024-12-18 16:45:21,202 - INFO - Moving arm to joint values: [-0.04363769 -0.40080548 -0.0883018 -0.04838655 -1.08217452 0.02273059] +2024-12-18 16:53:15,627 - INFO - Checking for faults on the robot... +2024-12-18 16:53:18,631 - INFO - Successfully initialize the robot +2024-12-18 16:53:18,632 - INFO - Current end effector pose: +2024-12-18 16:53:18,632 - INFO - Current joint values: [-0.027926120907068253, -0.47473254799842834, -0.03839745745062828, -0.022689493373036385, -1.0576682090759277, 0.003482136409729719] +2024-12-18 16:53:18,632 - INFO - Converted joint values [-0.027926120907068253, -0.47473254799842834, -0.03839745745062828, -0.022689493373036385, -1.0576682090759277, 0.003482136409729719] to Cartesian Pose [ 0.25014521 -0.00422227 0.36013329] and [ 7.07145052e-01 -1.78054523e-04 7.07046361e-01 5.59357786e-03]. +2024-12-18 16:53:38,079 - INFO - Checking for faults on the robot... +2024-12-18 16:53:41,082 - INFO - Successfully initialize the robot +2024-12-18 16:53:41,083 - INFO - Current end effector pose: +2024-12-18 16:53:41,083 - INFO - Current joint values: [-0.027922285720705986, -0.47473445534706116, -0.038393620401620865, -0.022685658186674118, -1.0576682090759277, 0.0034840537700802088] +2024-12-18 16:53:41,084 - INFO - Converted joint values [-0.027922285720705986, -0.47473445534706116, -0.038393620401620865, -0.022685658186674118, -1.0576682090759277, 0.0034840537700802088] to Cartesian Pose [ 0.25014434 -0.00422175 0.3601325 ] and [ 7.07145720e-01 -1.79236937e-04 7.07045704e-01 5.59204112e-03]. +2024-12-18 16:53:41,142 - INFO - Moving arm to joint values: [-0.04363769 -0.40080548 -0.0883018 -0.04838655 -1.08217452 0.02273059] +2024-12-18 16:54:00,768 - INFO - Checking for faults on the robot... +2024-12-18 16:54:03,772 - INFO - Successfully initialize the robot +2024-12-18 16:54:03,774 - INFO - Current end effector pose: +2024-12-18 16:54:03,775 - INFO - Current joint values: [-0.043639834970235825, -0.40081000328063965, -0.08829785138368607, -0.04839134216308594, -1.0821774005889893, 0.0227278433740139] +2024-12-18 16:54:03,777 - INFO - Converted joint values [-0.043639834970235825, -0.40081000328063965, -0.08829785138368607, -0.04839134216308594, -1.0821774005889893, 0.0227278433740139] to Cartesian Pose [ 0.2644329 -0.00567246 0.35987626] and [ 7.07105636e-01 -3.22962991e-04 7.07107777e-01 3.26516657e-04]. +2024-12-18 16:54:03,829 - INFO - Moving arm to joint values: [-0.06234195 -0.39871487 -0.08953733 -0.07749853 -1.08306928 0.04434266] +2024-12-18 16:54:09,537 - INFO - Checking for faults on the robot... +2024-12-18 16:54:12,540 - INFO - Successfully initialize the robot +2024-12-18 16:54:12,542 - INFO - Current end effector pose: +2024-12-18 16:54:12,543 - INFO - Current joint values: [-0.06234673038125038, -0.3987199664115906, -0.08954229205846786, -0.07750438153743744, -1.0830748081207275, 0.04433779790997505] +2024-12-18 16:54:12,545 - INFO - Converted joint values [-0.06234673038125038, -0.3987199664115906, -0.08954229205846786, -0.07750438153743744, -1.0830748081207275, 0.04433779790997505] to Cartesian Pose [ 0.26466588 -0.0071116 0.35971177] and [ 7.07355201e-01 -6.73306417e-04 7.06840368e-01 -4.98612488e-03]. +2024-12-18 16:54:12,595 - INFO - Moving arm to joint values: [-0.08041102 -0.39663926 -0.09074135 -0.10582517 -1.08388427 0.06553109] +2024-12-18 16:54:21,296 - INFO - Checking for faults on the robot... +2024-12-18 16:54:24,300 - INFO - Successfully initialize the robot +2024-12-18 16:54:24,302 - INFO - Current end effector pose: +2024-12-18 16:54:24,303 - INFO - Current joint values: [-0.08041702210903168, -0.39663374423980713, -0.0907483845949173, -0.10583317279815674, -1.0838935375213623, 0.06552974134683609] +2024-12-18 16:54:24,305 - INFO - Converted joint values [-0.08041702210903168, -0.39663374423980713, -0.0907483845949173, -0.10583317279815674, -1.0838935375213623, 0.06552974134683609] to Cartesian Pose [ 0.26485144 -0.00849048 0.35948423] and [ 0.70775517 -0.00104751 0.70638219 -0.01028215]. +2024-12-18 16:54:24,354 - INFO - Moving arm to joint values: [-0.09786297 -0.39458264 -0.09190178 -0.13338775 -1.0846115 0.08630933] +2024-12-18 16:54:25,586 - INFO - Checking for faults on the robot... +2024-12-18 16:54:28,590 - INFO - Successfully initialize the robot +2024-12-18 16:54:28,592 - INFO - Current end effector pose: +2024-12-18 16:54:28,593 - INFO - Current joint values: [-0.09785646945238113, -0.39458972215652466, -0.09189503639936447, -0.13338346779346466, -1.0846068859100342, 0.0863017588853836] +2024-12-18 16:54:28,595 - INFO - Converted joint values [-0.09785646945238113, -0.39458972215652466, -0.09189503639936447, -0.13338346779346466, -1.0846068859100342, 0.0863017588853836] to Cartesian Pose [ 0.26498807 -0.00980834 0.35919848] and [ 0.7082982 -0.0014492 0.70574047 -0.01555474]. +2024-12-18 16:54:28,646 - INFO - Moving arm to joint values: [-0.11471778 -0.39255499 -0.09300055 -0.16021478 -1.08523811 0.10668974] +2024-12-18 16:54:29,659 - INFO - Checking for faults on the robot... +2024-12-18 16:54:32,662 - INFO - Successfully initialize the robot +2024-12-18 16:54:32,664 - INFO - Current end effector pose: +2024-12-18 16:54:32,665 - INFO - Current joint values: [-0.11471492052078247, -0.39255911111831665, -0.09299758821725845, -0.16021662950515747, -1.085235834121704, 0.10668452829122543] +2024-12-18 16:54:32,667 - INFO - Converted joint values [-0.11471492052078247, -0.39255911111831665, -0.09299758821725845, -0.16021662950515747, -1.085235834121704, 0.10668452829122543] to Cartesian Pose [ 0.26508296 -0.01107026 0.35885742] and [ 0.70897243 -0.00188201 0.70492679 -0.02080322]. +2024-12-18 16:54:32,717 - INFO - Moving arm to joint values: [-0.13100874 -0.39055011 -0.09403675 -0.18634651 -1.08576727 0.12669831] +2024-12-18 16:55:23,068 - INFO - Checking for faults on the robot... +2024-12-18 16:55:26,071 - INFO - Successfully initialize the robot +2024-12-18 16:55:26,073 - INFO - Current end effector pose: +2024-12-18 16:55:26,074 - INFO - Current joint values: [-0.027922285720705986, -0.47473061084747314, -0.038393620401620865, -0.022691410034894943, -1.057666301727295, 0.003482136409729719] +2024-12-18 16:55:26,077 - INFO - Converted joint values [-0.027922285720705986, -0.47473061084747314, -0.038393620401620865, -0.022691410034894943, -1.057666301727295, 0.003482136409729719] to Cartesian Pose [ 0.25014485 -0.00422106 0.36013134] and [ 7.07147774e-01 -1.75824068e-04 7.07043652e-01 5.59190996e-03]. +2024-12-18 16:55:53,090 - INFO - Checking for faults on the robot... +2024-12-18 16:55:56,094 - INFO - Successfully initialize the robot +2024-12-18 16:55:56,096 - INFO - Current end effector pose: +2024-12-18 16:55:56,098 - INFO - Current joint values: [-0.027926120907068253, -0.47473061084747314, -0.038393620401620865, -0.022689493373036385, -1.0576701164245605, 0.003482136409729719] +2024-12-18 16:55:56,100 - INFO - Converted joint values [-0.027926120907068253, -0.47473061084747314, -0.038393620401620865, -0.022689493373036385, -1.0576701164245605, 0.003482136409729719] to Cartesian Pose [ 0.25014454 -0.00422225 0.36013171] and [ 7.07146418e-01 -1.78087706e-04 7.07044994e-01 5.59351919e-03]. +2024-12-18 16:55:56,164 - INFO - Moving arm to joint values: [-0.04983681 -0.47098664 -0.03980306 -0.05606362 -1.06067688 0.02740035] +2024-12-18 16:56:48,361 - INFO - Checking for faults on the robot... +2024-12-18 16:56:51,364 - INFO - Successfully initialize the robot +2024-12-18 16:56:51,366 - INFO - Current end effector pose: +2024-12-18 16:56:51,367 - INFO - Current joint values: [0.6476946473121643, -0.4308626055717468, -0.891028106212616, 1.9065041542053223, -1.011008381843567, -1.7163710594177246] +2024-12-18 16:56:51,369 - INFO - Converted joint values [0.6476946473121643, -0.4308626055717468, -0.891028106212616, 1.9065041542053223, -1.011008381843567, -1.7163710594177246] to Cartesian Pose [0.2758677 0.06362008 0.50262956] and [ 0.83171395 -0.09450834 0.5224177 0.16248024]. +2024-12-18 16:56:51,439 - INFO - Moving arm to joint values: [ 0.63201973 -0.43794378 -0.87549602 1.88070895 -0.99998811 -1.68199854] +2024-12-18 16:56:53,738 - INFO - Checking for faults on the robot... +2024-12-18 16:56:56,741 - INFO - Successfully initialize the robot +2024-12-18 16:56:56,743 - INFO - Current end effector pose: +2024-12-18 16:56:56,744 - INFO - Current joint values: [0.6320192813873291, -0.43794384598731995, -0.8754984140396118, 1.8807045221328735, -0.9999924898147583, -1.681996464729309] +2024-12-18 16:56:56,746 - INFO - Converted joint values [0.6320192813873291, -0.43794384598731995, -0.8754984140396118, 1.8807045221328735, -0.9999924898147583, -1.681996464729309] to Cartesian Pose [0.27723977 0.05818542 0.50186701] and [ 0.82589571 -0.10364114 0.53019566 0.16139192]. +2024-12-18 16:56:56,816 - INFO - Moving arm to joint values: [ 0.6160001 -0.44486083 -0.86028297 1.85435242 -0.98895687 -1.64747968] +2024-12-18 16:56:58,868 - INFO - Checking for faults on the robot... +2024-12-18 16:57:01,871 - INFO - Successfully initialize the robot +2024-12-18 16:57:01,873 - INFO - Current end effector pose: +2024-12-18 16:57:01,873 - INFO - Current joint values: [0.6159987449645996, -0.44486019015312195, -0.8602851629257202, 1.8543468713760376, -0.9889612197875977, -1.6474781036376953] +2024-12-18 16:57:01,874 - INFO - Converted joint values [0.6159987449645996, -0.44486019015312195, -0.8602851629257202, 1.8543468713760376, -0.9889612197875977, -1.6474781036376953] to Cartesian Pose [0.27857261 0.05274606 0.50121839] and [ 0.81991375 -0.11245792 0.53797986 0.16022588]. +2024-12-18 16:57:01,930 - INFO - Moving arm to joint values: [ 0.59963698 -0.45160961 -0.84538244 1.82741974 -0.97790559 -1.61278117] +2024-12-18 16:57:03,697 - INFO - Checking for faults on the robot... +2024-12-18 16:57:06,701 - INFO - Successfully initialize the robot +2024-12-18 16:57:06,703 - INFO - Current end effector pose: +2024-12-18 16:57:06,704 - INFO - Current joint values: [0.5996369123458862, -0.4516077935695648, -0.8453863859176636, 1.8274178504943848, -0.9779108166694641, -1.6127833127975464] +2024-12-18 16:57:06,706 - INFO - Converted joint values [0.5996369123458862, -0.4516077935695648, -0.8453863859176636, 1.8274178504943848, -0.9779108166694641, -1.6127833127975464] to Cartesian Pose [0.27986468 0.04730814 0.50068285] and [ 0.81378055 -0.12095384 0.54576366 0.15897616]. +2024-12-18 16:57:06,779 - INFO - Moving arm to joint values: [ 0.5829297 -0.45818858 -0.83079205 1.79989237 -0.96683475 -1.57787748] +2024-12-18 16:58:03,489 - INFO - Checking for faults on the robot... +2024-12-18 16:58:06,493 - INFO - Successfully initialize the robot +2024-12-18 16:58:06,495 - INFO - Current end effector pose: +2024-12-18 16:58:06,496 - INFO - Current joint values: [-0.027926120907068253, -0.47473061084747314, -0.038393620401620865, -0.022691410034894943, -1.0576778650283813, 0.0034878887236118317] +2024-12-18 16:58:06,498 - INFO - Converted joint values [-0.027926120907068253, -0.47473061084747314, -0.038393620401620865, -0.022691410034894943, -1.0576778650283813, 0.0034878887236118317] to Cartesian Pose [ 0.25014395 -0.004222 0.36013246] and [ 7.07143685e-01 -1.79167388e-04 7.07047746e-01 5.59121898e-03]. +2024-12-18 16:58:06,563 - INFO - Moving arm to joint values: [-0.04150686 -0.37307685 -0.10900079 -0.04580389 -1.08914953 0.02123348] +2024-12-18 16:58:21,794 - INFO - Checking for faults on the robot... +2024-12-18 16:58:24,798 - INFO - Successfully initialize the robot +2024-12-18 16:58:24,800 - INFO - Current end effector pose: +2024-12-18 16:58:24,802 - INFO - Current joint values: [-0.027920367196202278, -0.47473061084747314, -0.03839937224984169, -0.02268374152481556, -1.057664394378662, 0.003482136409729719] +2024-12-18 16:58:24,803 - INFO - Converted joint values [-0.027920367196202278, -0.47473061084747314, -0.03839937224984169, -0.02268374152481556, -1.057664394378662, 0.003482136409729719] to Cartesian Pose [ 0.25014605 -0.00422155 0.36013308] and [ 7.07146388e-01 -1.78805511e-04 7.07045034e-01 5.59232149e-03]. +2024-12-18 16:58:24,864 - INFO - Moving arm to joint values: [-0.04150686 -0.37307685 -0.10900079 -0.04580389 -1.08914953 0.02123348] +2024-12-18 17:01:19,020 - INFO - Checking for faults on the robot... +2024-12-18 17:01:22,024 - INFO - Successfully initialize the robot +2024-12-18 17:01:22,026 - INFO - Current end effector pose: +2024-12-18 17:01:22,027 - INFO - Current joint values: [-0.04150952026247978, -0.3730756342411041, -0.10900275409221649, -0.04580850154161453, -1.0891531705856323, 0.021232211962342262] +2024-12-18 17:01:22,029 - INFO - Converted joint values [-0.04150952026247978, -0.3730756342411041, -0.10900275409221649, -0.04580850154161453, -1.0891531705856323, 0.021232211962342262] to Cartesian Pose [ 0.2704345 -0.00567822 0.35987674] and [ 7.07105303e-01 -3.23751621e-04 7.07108110e-01 3.26184172e-04]. +2024-12-18 17:01:22,087 - INFO - Moving arm to joint values: [-0.05037699 -0.28696352 -0.17889097 -0.06315169 -1.10506539 0.03647968] +2024-12-18 17:03:24,234 - INFO - Checking for faults on the robot... +2024-12-18 17:03:27,237 - INFO - Successfully initialize the robot +2024-12-18 17:03:27,239 - INFO - Current end effector pose: +2024-12-18 17:03:27,240 - INFO - Current joint values: [-0.027920367196202278, -0.47473061084747314, -0.03839937224984169, -0.02268374152481556, -1.057664394378662, 0.0034840537700802088] +2024-12-18 17:03:27,242 - INFO - Converted joint values [-0.027920367196202278, -0.47473061084747314, -0.03839937224984169, -0.02268374152481556, -1.057664394378662, 0.0034840537700802088] to Cartesian Pose [ 0.25014605 -0.00422155 0.36013308] and [ 7.07146388e-01 -1.79483438e-04 7.07045039e-01 5.59164366e-03]. +2024-12-18 17:03:27,304 - INFO - Moving arm to joint values: [-0.04150686 -0.37307685 -0.10900079 -0.04580389 -1.08914953 0.02123348] +2024-12-18 17:08:03,237 - INFO - Checking for faults on the robot... +2024-12-18 17:08:06,240 - INFO - Successfully initialize the robot +2024-12-18 17:08:06,242 - INFO - Current end effector pose: +2024-12-18 17:08:06,243 - INFO - Current joint values: [-0.027922285720705986, -0.47473445534706116, -0.03839745745062828, -0.022685658186674118, -1.0576701164245605, 0.003482136409729719] +2024-12-18 17:08:06,245 - INFO - Converted joint values [-0.027922285720705986, -0.47473445534706116, -0.03839745745062828, -0.022685658186674118, -1.0576701164245605, 0.003482136409729719] to Cartesian Pose [ 0.2501449 -0.00422176 0.36013396] and [ 7.07143690e-01 -1.78524181e-04 7.07047729e-01 5.59275430e-03]. +2024-12-18 17:08:06,314 - INFO - Moving arm to joint values: [-0.04150686 -0.37307685 -0.10900079 -0.04580389 -1.08914953 0.02123348] +2024-12-18 17:08:51,120 - INFO - Checking for faults on the robot... +2024-12-18 17:08:54,124 - INFO - Successfully initialize the robot +2024-12-18 17:08:54,126 - INFO - Current end effector pose: +2024-12-18 17:08:54,127 - INFO - Current joint values: [-0.041507601737976074, -0.37307754158973694, -0.1090046763420105, -0.04580274969339371, -1.0891531705856323, 0.021230293437838554] +2024-12-18 17:08:54,129 - INFO - Converted joint values [-0.041507601737976074, -0.37307754158973694, -0.1090046763420105, -0.04580274969339371, -1.0891531705856323, 0.021230293437838554] to Cartesian Pose [ 0.2704347 -0.00567841 0.35987789] and [ 7.07103911e-01 -3.25083301e-04 7.07109501e-01 3.27099538e-04]. +2024-12-18 17:08:54,194 - INFO - Moving arm to joint values: [-0.05037576 -0.28696443 -0.17888977 -0.06315035 -1.10506454 0.03647797] +2024-12-18 17:09:02,047 - INFO - Checking for faults on the robot... +2024-12-18 17:09:05,050 - INFO - Successfully initialize the robot +2024-12-18 17:09:05,051 - INFO - Current end effector pose: +2024-12-18 17:09:05,051 - INFO - Current joint values: [-0.027920367196202278, -0.47473061084747314, -0.03839937224984169, -0.022685658186674118, -1.0576740503311157, 0.0034840537700802088] +2024-12-18 17:09:05,053 - INFO - Converted joint values [-0.027920367196202278, -0.47473061084747314, -0.03839937224984169, -0.022685658186674118, -1.0576740503311157, 0.0034840537700802088] to Cartesian Pose [ 0.25014532 -0.0042213 0.36013401] and [ 7.07142981e-01 -1.78521585e-04 7.07048449e-01 5.59137485e-03]. +2024-12-18 17:09:05,106 - INFO - Moving arm to joint values: [-0.04150686 -0.37307685 -0.10900079 -0.04580389 -1.08914953 0.02123348] +2024-12-18 17:11:18,559 - INFO - Checking for faults on the robot... +2024-12-18 17:11:21,562 - INFO - Successfully initialize the robot +2024-12-18 17:11:21,564 - INFO - Current end effector pose: +2024-12-18 17:11:21,565 - INFO - Current joint values: [-0.027924202382564545, -0.47473061084747314, -0.03839745745062828, -0.022685658186674118, -1.057666301727295, 0.0034840537700802088] +2024-12-18 17:11:21,567 - INFO - Converted joint values [-0.027924202382564545, -0.47473061084747314, -0.03839745745062828, -0.022685658186674118, -1.057666301727295, 0.0034840537700802088] to Cartesian Pose [ 0.25014554 -0.00422226 0.36013262] and [ 7.07146397e-01 -1.79922350e-04 7.07045022e-01 5.59272081e-03]. +2024-12-18 17:11:21,627 - INFO - Moving arm to joint values: [-0.04150686 -0.37307685 -0.10900079 -0.04580389 -1.08914953 0.02123348] +2024-12-18 17:12:42,354 - INFO - Checking for faults on the robot... +2024-12-18 17:12:45,359 - INFO - Successfully initialize the robot +2024-12-18 17:12:45,361 - INFO - Current end effector pose: +2024-12-18 17:12:45,362 - INFO - Current joint values: [-0.04150952026247978, -0.37307947874069214, -0.10900275409221649, -0.04580466449260712, -1.0891493558883667, 0.021232211962342262] +2024-12-18 17:12:45,424 - INFO - Moving arm to joint values: [-0.05037898 -0.28696336 -0.17889121 -0.06315503 -1.10506556 0.0364812 ] +2024-12-18 17:13:32,940 - INFO - Checking for faults on the robot... +2024-12-18 17:13:35,943 - INFO - Successfully initialize the robot +2024-12-18 17:13:35,945 - INFO - Current end effector pose: +2024-12-18 17:13:35,946 - INFO - Current joint values: [-0.027926120907068253, -0.47473061084747314, -0.03839745745062828, -0.022691410034894943, -1.0576720237731934, 0.0034840537700802088] +2024-12-18 17:13:36,013 - INFO - Moving arm to joint values: [-0.04150686 -0.37307685 -0.10900079 -0.04580389 -1.08914953 0.02123348] +2024-12-18 17:20:06,589 - INFO - Checking for faults on the robot... +2024-12-18 17:20:09,593 - INFO - Successfully initialize the robot +2024-12-18 17:20:09,595 - INFO - Current end effector pose: +2024-12-18 17:20:09,596 - INFO - Current joint values: [-0.027922285720705986, -0.47473445534706116, -0.03839745745062828, -0.022689493373036385, -1.057666301727295, 0.003482136409729719] +2024-12-18 17:20:09,651 - INFO - Moving arm to joint values: [-0.04046228 -0.37310569 -0.10897496 -0.04462554 -1.08912463 0.02068773] +2024-12-18 17:20:42,784 - INFO - Checking for faults on the robot... +2024-12-18 17:20:45,788 - INFO - Successfully initialize the robot +2024-12-18 17:20:45,790 - INFO - Current end effector pose: +2024-12-18 17:20:45,791 - INFO - Current joint values: [-0.027922285720705986, -0.47473061084747314, -0.03839937224984169, -0.02268374152481556, -1.0576740503311157, 0.0034840537700802088] +2024-12-18 17:20:45,842 - INFO - Moving arm to joint values: [-0.04046228 -0.37310569 -0.10897496 -0.04462554 -1.08912463 0.02068773] +2024-12-18 17:21:59,689 - INFO - Checking for faults on the robot... +2024-12-18 17:22:02,692 - INFO - Successfully initialize the robot +2024-12-18 17:22:02,694 - INFO - Current end effector pose: +2024-12-18 17:22:02,695 - INFO - Current joint values: [-0.027924202382564545, -0.47473061084747314, -0.03839937224984169, -0.022691410034894943, -1.0576740503311157, 0.003485971363261342] +2024-12-18 17:22:02,761 - INFO - Moving arm to joint values: [-0.04150686 -0.37307685 -0.10900079 -0.04580389 -1.08914953 0.02123348] +2024-12-18 17:22:50,097 - INFO - Checking for faults on the robot... +2024-12-18 17:22:53,101 - INFO - Successfully initialize the robot +2024-12-18 17:22:53,103 - INFO - Current end effector pose: +2024-12-18 17:22:53,104 - INFO - Current joint values: [-0.04150952026247978, -0.3730756342411041, -0.1090046763420105, -0.04580466449260712, -1.0891493558883667, 0.021228376775979996] +2024-12-18 17:22:53,171 - INFO - Moving arm to joint values: [-0.0494794 -0.28699423 -0.17885989 -0.06214749 -1.10504035 0.03602928] +2024-12-18 17:22:59,000 - INFO - Checking for faults on the robot... +2024-12-18 17:23:02,003 - INFO - Successfully initialize the robot +2024-12-18 17:23:02,005 - INFO - Current end effector pose: +2024-12-18 17:23:02,006 - INFO - Current joint values: [-0.027922285720705986, -0.47472870349884033, -0.03839937224984169, -0.022685658186674118, -1.0576740503311157, 0.0034840537700802088] +2024-12-18 17:23:02,071 - INFO - Moving arm to joint values: [-0.04046228 -0.37310569 -0.10897496 -0.04462554 -1.08912463 0.02068773] +2024-12-18 17:23:39,092 - INFO - Checking for faults on the robot... +2024-12-18 17:23:42,096 - INFO - Successfully initialize the robot +2024-12-18 17:23:42,098 - INFO - Current end effector pose: +2024-12-18 17:23:42,099 - INFO - Current joint values: [-0.027920367196202278, -0.47473636269569397, -0.03839937224984169, -0.02268374152481556, -1.057664394378662, 0.0034840537700802088] +2024-12-18 17:23:42,180 - INFO - Moving arm to joint values: [-0.04150686 -0.37307685 -0.10900079 -0.04580389 -1.08914953 0.02123348] +2024-12-18 17:24:01,186 - INFO - Checking for faults on the robot... +2024-12-18 17:24:04,190 - INFO - Successfully initialize the robot +2024-12-18 17:24:04,192 - INFO - Current end effector pose: +2024-12-18 17:24:04,193 - INFO - Current joint values: [-0.027922285720705986, -0.47472870349884033, -0.03839937224984169, -0.022687576711177826, -1.0576701164245605, 0.0034840537700802088] +2024-12-18 17:24:04,245 - INFO - Moving arm to joint values: [-0.0425513 -0.37304728 -0.10902728 -0.04698205 -1.08917507 0.02177911] +2024-12-18 17:36:09,224 - INFO - Checking for faults on the robot... +2024-12-18 17:36:12,228 - INFO - Successfully initialize the robot +2024-12-18 17:36:12,230 - INFO - Current end effector pose: +2024-12-18 17:36:12,231 - INFO - Current joint values: [-0.28328216075897217, -0.03905706852674484, -0.7451618313789368, -0.6515238285064697, -0.9098423719406128, 0.4350043535232544] +2024-12-18 17:36:12,300 - INFO - Moving arm to joint values: [-0.25766706 0.01593008 -0.80556114 -0.63292942 -0.88808863 0.43347777] +2024-12-18 17:36:42,951 - INFO - Checking for faults on the robot... +2024-12-18 17:36:45,954 - INFO - Successfully initialize the robot +2024-12-18 17:36:45,956 - INFO - Current end effector pose: +2024-12-18 17:36:45,957 - INFO - Current joint values: [-0.4824925661087036, -0.004425534512847662, -0.9801389575004578, -1.1519774198532104, -1.0745344161987305, 0.848483145236969] +2024-12-18 17:36:46,325 - INFO - Moving arm to joint values: [-0.44317461 0.03085386 -1.01770369 -1.13326286 -1.03301439 0.85269725] +2024-12-18 17:39:47,217 - INFO - Checking for faults on the robot... +2024-12-18 17:39:50,221 - INFO - Successfully initialize the robot +2024-12-18 17:39:50,223 - INFO - Current end effector pose: +2024-12-18 17:39:50,224 - INFO - Current joint values: [-0.027922285720705986, -0.47473636269569397, -0.038393620401620865, -0.022689493373036385, -1.0576778650283813, 0.0034840537700802088] +2024-12-18 17:39:50,282 - INFO - Moving arm to joint values: [-0.04150686 -0.37307685 -0.10900079 -0.04580389 -1.08914953 0.02123348] +2024-12-18 17:41:08,160 - INFO - Checking for faults on the robot... +2024-12-18 17:41:11,163 - INFO - Successfully initialize the robot +2024-12-18 17:41:11,164 - INFO - Current end effector pose: +2024-12-18 17:41:11,164 - INFO - Current joint values: [-0.027926120907068253, -0.47473061084747314, -0.038393620401620865, -0.022689493373036385, -1.0576701164245605, 0.0034840537700802088] +2024-12-18 17:41:11,515 - INFO - Moving arm to joint values: [-0.04150701 -0.37306356 -0.10899858 -0.04580481 -1.08916503 0.02123328] +2024-12-18 17:43:16,081 - INFO - Checking for faults on the robot... +2024-12-18 17:43:19,085 - INFO - Successfully initialize the robot +2024-12-18 17:43:19,085 - INFO - Current end effector pose: +2024-12-18 17:43:19,086 - INFO - Current joint values: [-0.027922285720705986, -0.47473636269569397, -0.03839745745062828, -0.022687576711177826, -1.057666301727295, 0.0034840537700802088] +2024-12-18 17:43:19,134 - INFO - Moving arm to joint values: [-0.04983681 -0.47098664 -0.03980306 -0.05606362 -1.06067688 0.02740035] +2024-12-18 17:43:31,190 - INFO - Checking for faults on the robot... +2024-12-18 17:43:34,194 - INFO - Successfully initialize the robot +2024-12-18 17:43:34,195 - INFO - Current end effector pose: +2024-12-18 17:43:34,195 - INFO - Current joint values: [-0.027920367196202278, -0.47473061084747314, -0.03839937224984169, -0.02268374152481556, -1.057664394378662, 0.003482136409729719] +2024-12-18 17:43:34,244 - INFO - Moving arm to joint values: [-0.04983681 -0.47098664 -0.03980306 -0.05606362 -1.06067688 0.02740035] +2024-12-18 17:44:03,601 - INFO - Checking for faults on the robot... +2024-12-18 17:44:06,604 - INFO - Successfully initialize the robot +2024-12-18 17:44:06,605 - INFO - Current end effector pose: +2024-12-18 17:44:06,605 - INFO - Current joint values: [-0.304119348526001, -0.20815928280353546, -0.6484577655792236, -0.6779735088348389, -0.8492079377174377, 0.4868184030056] +2024-12-18 17:44:06,956 - INFO - Moving arm to joint values: [-0.30850413 -0.20789832 -0.64596958 -0.69104339 -0.84686527 0.50116539] +2024-12-18 17:46:28,433 - INFO - Checking for faults on the robot... +2024-12-18 17:46:31,437 - INFO - Successfully initialize the robot +2024-12-18 17:46:31,438 - INFO - Current end effector pose: +2024-12-18 17:46:31,438 - INFO - Current joint values: [0.6209592819213867, -0.5777067542076111, -0.8901441097259521, -0.5743895173072815, 0.8544675707817078, 0.8672053813934326] +2024-12-18 17:46:31,795 - INFO - Moving arm to joint values: [ 0.78174218 -0.10175415 -1.48666094 2.51729017 -1.05037598 -2.19329855] +2024-12-18 17:46:57,555 - INFO - Checking for faults on the robot... +2024-12-18 17:47:00,558 - INFO - Successfully initialize the robot +2024-12-18 17:47:00,559 - INFO - Current end effector pose: +2024-12-18 17:47:00,559 - INFO - Current joint values: [0.8922533392906189, 0.14861972630023956, -0.6966497302055359, 0.391579270362854, -0.17768099904060364, -0.3300743103027344] +2024-12-18 17:47:00,713 - INFO - Moving arm to joint values: [ 0.86560539 0.15387242 -0.68913773 0.26579369 -0.18926605 -0.22577908] +2024-12-18 17:47:29,359 - INFO - Checking for faults on the robot... +2024-12-18 17:47:32,363 - INFO - Successfully initialize the robot +2024-12-18 17:47:32,363 - INFO - Current end effector pose: +2024-12-18 17:47:32,364 - INFO - Current joint values: [-0.44624075293540955, -0.050492893904447556, -1.2351939678192139, -1.9963475465774536, -0.5926649570465088, 1.7032344341278076] +2024-12-18 17:47:32,717 - INFO - Moving arm to joint values: [-0.45202711 -0.05292395 -1.22189663 -2.00784815 -0.58092481 1.71558292] +2024-12-18 17:47:53,225 - INFO - Checking for faults on the robot... +2024-12-18 17:47:56,229 - INFO - Successfully initialize the robot +2024-12-18 17:47:56,230 - INFO - Current end effector pose: +2024-12-18 17:47:56,230 - INFO - Current joint values: [-0.5194442272186279, 0.04593697190284729, -1.2915294170379639, -1.9207030534744263, -0.7628237009048462, 2.8348021507263184] +2024-12-18 17:47:56,587 - INFO - Moving arm to joint values: [-0.20058717 -0.28596695 -0.90354865 1.23583557 0.45019349 -0.18732727] +2024-12-18 17:48:07,660 - INFO - Checking for faults on the robot... +2024-12-18 17:48:10,662 - INFO - Successfully initialize the robot +2024-12-18 17:48:10,663 - INFO - Current end effector pose: +2024-12-18 17:48:10,664 - INFO - Current joint values: [-0.2005852460861206, -0.2859666049480438, -0.9035511016845703, 1.2358362674713135, 0.4501945972442627, -0.1873316466808319] +2024-12-18 17:48:10,721 - INFO - Moving arm to joint values: [-0.20524228 -0.28121923 -0.90903472 1.24630233 0.44700989 -0.20543593] +2024-12-18 17:48:15,845 - INFO - Checking for faults on the robot... +2024-12-18 17:48:18,848 - INFO - Successfully initialize the robot +2024-12-18 17:48:18,849 - INFO - Current end effector pose: +2024-12-18 17:48:18,849 - INFO - Current joint values: [-0.20524662733078003, -0.28122279047966003, -0.9090331792831421, 1.2463018894195557, 0.44700199365615845, -0.20543837547302246] +2024-12-18 17:48:18,905 - INFO - Moving arm to joint values: [-0.21001787 -0.27644871 -0.91464805 1.25692719 0.44397547 -0.22369523] +2024-12-18 17:48:32,830 - INFO - Checking for faults on the robot... +2024-12-18 17:48:35,833 - INFO - Successfully initialize the robot +2024-12-18 17:48:35,834 - INFO - Current end effector pose: +2024-12-18 17:48:35,834 - INFO - Current joint values: [-0.21001730859279633, -0.2764482796192169, -0.914649486541748, 1.2569266557693481, 0.4439723789691925, -0.22369849681854248] +2024-12-18 17:48:35,887 - INFO - Moving arm to joint values: [-0.217715 -0.27792241 -0.9165143 1.2635044 0.44428207 -0.238592 ] +2024-12-18 17:48:46,501 - INFO - Checking for faults on the robot... +2024-12-18 17:48:49,504 - INFO - Successfully initialize the robot +2024-12-18 17:48:49,505 - INFO - Current end effector pose: +2024-12-18 17:48:49,505 - INFO - Current joint values: [-0.21771980822086334, -0.27792665362358093, -0.9165132641792297, 1.2634978294372559, 0.4442753493785858, -0.2385953664779663] +2024-12-18 17:48:49,561 - INFO - Moving arm to joint values: [-0.22270591 -0.27310809 -0.92231641 1.2742957 0.44149997 -0.25702508] +2024-12-18 17:49:02,924 - INFO - Checking for faults on the robot... +2024-12-18 17:49:05,927 - INFO - Successfully initialize the robot +2024-12-18 17:49:05,928 - INFO - Current end effector pose: +2024-12-18 17:49:05,928 - INFO - Current joint values: [0.15631839632987976, 0.09691306948661804, -1.2473909854888916, 1.0285283327102661, 0.09583161771297455, 0.14001217484474182] +2024-12-18 17:49:05,977 - INFO - Moving arm to joint values: [ 0.15362745 0.10221369 -1.25871881 1.05996707 0.09302904 0.10540742] +2024-12-18 17:49:10,270 - INFO - Checking for faults on the robot... +2024-12-18 17:49:13,274 - INFO - Successfully initialize the robot +2024-12-18 17:49:13,274 - INFO - Current end effector pose: +2024-12-18 17:49:13,275 - INFO - Current joint values: [0.15362434089183807, 0.10220914334058762, -1.2587194442749023, 1.0599615573883057, 0.09301868081092834, 0.10540173947811127] +2024-12-18 17:49:13,325 - INFO - Moving arm to joint values: [ 0.15065801 0.10757326 -1.27050894 1.09204042 0.09041081 0.0700941 ] +2024-12-18 17:49:15,696 - INFO - Checking for faults on the robot... +2024-12-18 17:49:18,700 - INFO - Successfully initialize the robot +2024-12-18 17:49:18,701 - INFO - Current end effector pose: +2024-12-18 17:49:18,701 - INFO - Current joint values: [0.1506580114364624, 0.10756848752498627, -1.270508050918579, 1.092035174369812, 0.09041091054677963, 0.07008758187294006] +2024-12-18 17:49:18,756 - INFO - Moving arm to joint values: [ 0.14739752 0.1129905 -1.28274663 1.12471126 0.08798822 0.03410787] +2024-12-18 17:49:27,277 - INFO - Checking for faults on the robot... +2024-12-18 17:49:30,281 - INFO - Successfully initialize the robot +2024-12-18 17:49:30,281 - INFO - Current end effector pose: +2024-12-18 17:49:30,282 - INFO - Current joint values: [0.14739254117012024, 0.11298343539237976, -1.282751202583313, 1.1247090101242065, 0.08798146992921829, 0.034104228019714355] +2024-12-18 17:49:30,336 - INFO - Moving arm to joint values: [ 0.14382989 0.11846045 -1.29541483 1.15792432 0.08576044 -0.00249958] +2024-12-18 17:49:37,044 - INFO - Checking for faults on the robot... +2024-12-18 17:49:40,047 - INFO - Successfully initialize the robot +2024-12-18 17:49:40,048 - INFO - Current end effector pose: +2024-12-18 17:49:40,048 - INFO - Current joint values: [0.14382220804691315, 0.11846166849136353, -1.2954161167144775, 1.1579272747039795, 0.08574952930212021, -0.0025042237248271704] +2024-12-18 17:49:40,098 - INFO - Moving arm to joint values: [ 0.13993424 0.12398452 -1.30850123 1.19163928 0.08371969 -0.03968941] +2024-12-18 17:49:49,556 - INFO - Checking for faults on the robot... +2024-12-18 17:49:52,560 - INFO - Successfully initialize the robot +2024-12-18 17:49:52,561 - INFO - Current end effector pose: +2024-12-18 17:49:52,561 - INFO - Current joint values: [0.13992589712142944, 0.12398399412631989, -1.308506727218628, 1.1916403770446777, 0.08371508121490479, -0.03969558700919151] +2024-12-18 17:49:52,613 - INFO - Moving arm to joint values: [ 0.13341854 0.09541533 -1.28931201 1.21709916 0.08436061 -0.06951949] +2024-12-18 17:50:01,896 - INFO - Checking for faults on the robot... +2024-12-18 17:50:04,899 - INFO - Successfully initialize the robot +2024-12-18 17:50:04,900 - INFO - Current end effector pose: +2024-12-18 17:50:04,900 - INFO - Current joint values: [0.1334160566329956, 0.09540785104036331, -1.2893089056015015, 1.2171006202697754, 0.08435551822185516, -0.06952192634344101] +2024-12-18 17:50:04,954 - INFO - Moving arm to joint values: [ 0.12778593 0.08392466 -1.28662287 1.24705149 0.08393361 -0.10351617] +2024-12-18 17:50:16,827 - INFO - Checking for faults on the robot... +2024-12-18 17:50:19,830 - INFO - Successfully initialize the robot +2024-12-18 17:50:19,831 - INFO - Current end effector pose: +2024-12-18 17:50:19,831 - INFO - Current joint values: [0.12778443098068237, 0.08392217010259628, -1.2866244316101074, 1.247047781944275, 0.08393367379903793, -0.10351686179637909] +2024-12-18 17:50:19,885 - INFO - Moving arm to joint values: [ 0.12302339 0.08954584 -1.30051414 1.28230125 0.08241507 -0.14248518] +2024-12-18 17:50:36,379 - INFO - Checking for faults on the robot... +2024-12-18 17:50:39,383 - INFO - Successfully initialize the robot +2024-12-18 17:50:39,384 - INFO - Current end effector pose: +2024-12-18 17:50:39,384 - INFO - Current joint values: [0.12301567196846008, 0.08954229205846786, -1.30051851272583, 1.2822986841201782, 0.0824054479598999, -0.14249147474765778] +2024-12-18 17:50:39,436 - INFO - Moving arm to joint values: [ 0.11828983 0.10203954 -1.32136368 1.31938294 0.08065141 -0.18322646] +2024-12-18 17:50:52,964 - INFO - Checking for faults on the robot... +2024-12-18 17:50:55,968 - INFO - Successfully initialize the robot +2024-12-18 17:50:55,969 - INFO - Current end effector pose: +2024-12-18 17:50:55,969 - INFO - Current joint values: [0.11828909069299698, 0.10203848779201508, -1.3213653564453125, 1.319376826286316, 0.08064328879117966, -0.1832263320684433] +2024-12-18 17:50:56,023 - INFO - Moving arm to joint values: [ 0.11330268 0.11799715 -1.34597013 1.35677606 0.07893131 -0.22430512] +2024-12-18 17:51:15,427 - INFO - Checking for faults on the robot... +2024-12-18 17:51:18,430 - INFO - Successfully initialize the robot +2024-12-18 17:51:18,431 - INFO - Current end effector pose: +2024-12-18 17:51:18,431 - INFO - Current joint values: [0.43319618701934814, -0.5327189564704895, -0.7772431373596191, 2.1834797859191895, -0.6401167511940002, -1.2369810342788696] +2024-12-18 17:51:18,786 - INFO - Moving arm to joint values: [ 0.42127368 -0.53691912 -0.76062479 2.13942845 -0.62793763 -1.18875835] +2024-12-18 17:51:35,137 - INFO - Checking for faults on the robot... +2024-12-18 17:51:38,141 - INFO - Successfully initialize the robot +2024-12-18 17:51:38,142 - INFO - Current end effector pose: +2024-12-18 17:51:38,142 - INFO - Current joint values: [0.421267569065094, -0.5369182229042053, -0.760624349117279, 2.139425754547119, -0.6279369592666626, -1.1887621879577637] +2024-12-18 17:51:38,197 - INFO - Moving arm to joint values: [ 0.40891928 -0.54098801 -0.7440127 2.09377054 -0.6159024 -1.13928423] +2024-12-18 17:51:58,407 - INFO - Checking for faults on the robot... +2024-12-18 17:52:01,411 - INFO - Successfully initialize the robot +2024-12-18 17:52:01,412 - INFO - Current end effector pose: +2024-12-18 17:52:01,412 - INFO - Current joint values: [0.4089113473892212, -0.5409947633743286, -0.7440113425254822, 2.0937647819519043, -0.6158971190452576, -1.1392855644226074] +2024-12-18 17:52:01,465 - INFO - Moving arm to joint values: [ 0.39609739 -0.54493152 -0.72738504 2.04640678 -0.60401727 -1.08846185] +2024-12-18 17:52:04,038 - INFO - Checking for faults on the robot... +2024-12-18 17:52:07,042 - INFO - Successfully initialize the robot +2024-12-18 17:52:07,042 - INFO - Current end effector pose: +2024-12-18 17:52:07,043 - INFO - Current joint values: [0.396091103553772, -0.5449294447898865, -0.727390706539154, 2.0464069843292236, -0.6040241122245789, -1.0884648561477661] +2024-12-18 17:52:07,096 - INFO - Moving arm to joint values: [ 0.38277689 -0.54874779 -0.7107292 1.99723009 -0.59229567 -1.03619923] +2024-12-18 17:57:44,137 - INFO - Checking for faults on the robot... +2024-12-18 17:57:47,140 - INFO - Successfully initialize the robot +2024-12-18 17:57:47,141 - INFO - Current end effector pose: +2024-12-18 17:57:47,141 - INFO - Current joint values: [0.3827703893184662, -0.5487509369850159, -0.7107259035110474, 1.9972257614135742, -0.5922929644584656, -1.0361963510513306] +2024-12-18 17:57:47,194 - INFO - Moving arm to joint values: [ 0.36890599 -0.55244647 -0.69402382 1.9461187 -0.58073648 -0.9823948 ] +2024-12-18 17:58:00,202 - INFO - Checking for faults on the robot... +2024-12-18 17:58:03,205 - INFO - Successfully initialize the robot +2024-12-18 17:58:03,206 - INFO - Current end effector pose: +2024-12-18 17:58:03,206 - INFO - Current joint values: [-0.02792995609343052, -0.47473254799842834, -0.03839937224984169, -0.022697163745760918, -1.0576778650283813, 0.0034878887236118317] +2024-12-18 17:58:03,258 - INFO - Moving arm to joint values: [-0.04983681 -0.47098664 -0.03980306 -0.05606362 -1.06067688 0.02740035] +2024-12-18 17:58:06,583 - INFO - Checking for faults on the robot... +2024-12-18 17:58:09,587 - INFO - Successfully initialize the robot +2024-12-18 17:58:09,588 - INFO - Current end effector pose: +2024-12-18 17:58:09,588 - INFO - Current joint values: [-0.04983711987733841, -0.4709915518760681, -0.03980104997754097, -0.056068915873765945, -1.060680627822876, 0.027396896854043007] +2024-12-18 17:58:09,637 - INFO - Moving arm to joint values: [-0.07088566 -0.46821012 -0.04111678 -0.08834501 -1.06229194 0.05072978] +2024-12-18 17:59:53,811 - INFO - Checking for faults on the robot... +2024-12-18 17:59:56,814 - INFO - Successfully initialize the robot +2024-12-18 17:59:56,815 - INFO - Current end effector pose: +2024-12-18 17:59:56,815 - INFO - Current joint values: [-0.07088908553123474, -0.46820929646492004, -0.041120272129774094, -0.08834770321846008, -1.0622950792312622, 0.05073066055774689] +2024-12-18 17:59:56,815 - INFO - Converted joint values [-0.07088908553123474, -0.46820929646492004, -0.041120272129774094, -0.08834770321846008, -1.0622950792312622, 0.05073066055774689] to Cartesian Pose [ 0.25066787 -0.00703155 0.35963558] and [ 7.07401113e-01 -5.09030242e-04 7.06794962e-01 -4.92821815e-03]. +2024-12-18 17:59:56,866 - INFO - Moving arm to joint values: [-0.09110488 -0.46545679 -0.04240048 -0.11957486 -1.0638388 0.0734598 ] +2024-12-18 18:00:50,621 - INFO - Checking for faults on the robot... +2024-12-18 18:00:53,624 - INFO - Successfully initialize the robot +2024-12-18 18:00:53,625 - INFO - Current end effector pose: +2024-12-18 18:00:53,625 - INFO - Current joint values: [-0.027920367196202278, -0.47473061084747314, -0.03839937224984169, -0.022685658186674118, -1.057664394378662, 0.0034840537700802088] +2024-12-18 18:00:53,625 - INFO - Converted joint values [-0.027920367196202278, -0.47473061084747314, -0.03839937224984169, -0.022685658186674118, -1.057664394378662, 0.0034840537700802088] to Cartesian Pose [ 0.25014605 -0.00422131 0.36013308] and [ 7.07146395e-01 -1.78560461e-04 7.07045035e-01 5.59138589e-03]. +2024-12-18 18:00:53,678 - INFO - Moving arm to joint values: [-0.04999365 -0.47258667 -0.03875973 -0.0562608 -1.06012536 0.02752394] +2024-12-18 18:01:26,871 - INFO - Checking for faults on the robot... +2024-12-18 18:01:29,875 - INFO - Successfully initialize the robot +2024-12-18 18:01:29,876 - INFO - Current end effector pose: +2024-12-18 18:01:29,876 - INFO - Current joint values: [-0.019567841663956642, -0.5318924784660339, -0.4564666748046875, 0.0016988837160170078, 0.3419454097747803, 0.0018024274613708258] +2024-12-18 18:01:29,877 - INFO - Converted joint values [-0.019567841663956642, -0.5318924784660339, -0.4564666748046875, 0.0016988837160170078, 0.3419454097747803, 0.0018024274613708258] to Cartesian Pose [ 0.34950472 -0.00690589 0.38400231] and [ 0.94816015 -0.0108 0.317601 0.00230044]. +2024-12-18 18:01:30,230 - INFO - Moving arm to joint values: [-0.0232185 -0.5289069 -0.44926211 0.0006699 0.33173039 -0.0035986 ] +2024-12-18 18:33:09,086 - INFO - Checking for faults on the robot... +2024-12-18 18:33:12,089 - INFO - Successfully initialize the robot +2024-12-18 18:33:12,092 - INFO - Current end effector pose: +2024-12-18 18:33:12,092 - INFO - Current joint values: [-0.02792803756892681, -0.47473061084747314, -0.038393620401620865, -0.02269524522125721, -1.0576740503311157, 0.0034917236771434546] +2024-12-18 18:33:12,158 - INFO - Moving arm to joint values: [-0.04999365 -0.47258667 -0.03875973 -0.0562608 -1.06012536 0.02752394] +2024-12-18 18:33:14,141 - INFO - Checking for faults on the robot... +2024-12-18 18:33:17,144 - INFO - Successfully initialize the robot +2024-12-18 18:33:17,146 - INFO - Current end effector pose: +2024-12-18 18:33:17,147 - INFO - Current joint values: [-0.04998859763145447, -0.47259455919265747, -0.038757942616939545, -0.05625874549150467, -1.0601226091384888, 0.02751769870519638] +2024-12-18 18:33:17,213 - INFO - Moving arm to joint values: [-0.07134971 -0.4713872 -0.03903698 -0.08893489 -1.06121621 0.0510933 ] +2024-12-18 18:46:15,766 - INFO - Checking for faults on the robot... +2024-12-18 18:46:18,770 - INFO - Successfully initialize the robot +2024-12-18 18:46:18,773 - INFO - Current end effector pose: +2024-12-18 18:46:18,774 - INFO - Current joint values: [-0.07135311514139175, -0.47138655185699463, -0.039041727781295776, -0.08894212543964386, -1.0612213611602783, 0.05109114944934845] +2024-12-18 18:46:18,775 - INFO - Current joint values: [-0.07135311514139175, -0.47138655185699463, -0.039041727781295776, -0.08894212543964386, -1.0612213611602783, 0.05109114944934845] +2024-12-18 18:46:18,785 - INFO - Converted Cartesian position [0.42023294099999997, -0.0060612750000000005, 0.358956207] to joint values [ 0. 0. -0.05260681 -0.46451922 -0.0398952 -0.07482018 + -1.0654767 0.05109115]. +2024-12-18 18:46:18,801 - INFO - Moving arm to joint values: [-0.0398952 -0.07482018 -1.0654767 0.05109115] +2024-12-18 18:46:53,951 - INFO - Checking for faults on the robot... +2024-12-18 18:46:56,955 - INFO - Successfully initialize the robot +2024-12-18 18:46:56,957 - INFO - Current end effector pose: +2024-12-18 18:46:56,958 - INFO - Current joint values: [-0.03989308699965477, -0.07481799274682999, -1.0654820203781128, 0.05109114944934845, -1.0612232685089111, 0.05108923092484474] +2024-12-18 18:46:56,959 - INFO - Current joint values: [-0.03989308699965477, -0.07481799274682999, -1.0654820203781128, 0.05109114944934845, -1.0612232685089111, 0.05108923092484474] +2024-12-18 18:46:56,976 - INFO - Converted Cartesian position [0.544744263, -0.035832783, 0.702229614] to joint values [ 0. 0. -0.02914492 -0.07779642 -1.06724875 0.05369141 + -1.05746224 0.05108923]. +2024-12-18 18:46:56,993 - INFO - Moving arm to joint values: [-0.02914492 -0.07779642 -1.06724875 0.05369141 -1.05746224 0.05108923] +2024-12-18 18:47:01,336 - INFO - Checking for faults on the robot... +2024-12-18 18:47:04,340 - INFO - Successfully initialize the robot +2024-12-18 18:47:04,342 - INFO - Current end effector pose: +2024-12-18 18:47:04,343 - INFO - Current joint values: [-0.029141800478100777, -0.0777958333492279, -1.067247986793518, 0.05368932709097862, -1.0574573278427124, 0.05108347907662392] +2024-12-18 18:47:04,344 - INFO - Current joint values: [-0.029141800478100777, -0.0777958333492279, -1.067247986793518, 0.05368932709097862, -1.0574573278427124, 0.05108347907662392] +2024-12-18 18:47:04,371 - INFO - Converted Cartesian position [0.544324036, -0.030616127, 0.704100769] to joint values [ 0. 0. -0.0184399 -0.08076505 -1.06907166 0.0562189 + -1.05371494 0.05108348]. +2024-12-18 18:47:04,386 - INFO - Moving arm to joint values: [-0.0184399 -0.08076505 -1.06907166 0.0562189 -1.05371494 0.05108348] +2024-12-18 18:49:03,469 - INFO - Checking for faults on the robot... +2024-12-18 18:49:06,473 - INFO - Successfully initialize the robot +2024-12-18 18:49:06,475 - INFO - Current end effector pose: +2024-12-18 18:49:06,476 - INFO - Current joint values: [-0.018438449129462242, -0.080769844353199, -1.069075345993042, 0.05621464177966118, -1.0537163019180298, 0.05107772350311279] +2024-12-18 18:49:06,478 - INFO - Current joint values: [-0.018438449129462242, -0.080769844353199, -1.069075345993042, 0.05621464177966118, -1.0537163019180298, 0.05107772350311279] +2024-12-18 18:49:06,494 - INFO - Converted Cartesian position [0.543839844, -0.025415237, 0.705993835] to joint values [ 0. 0. -0.00778084 -0.0837264 -1.0709453 0.05867344 + -1.04997243 0.05107772]. +2024-12-18 18:49:06,504 - INFO - Moving arm to joint values: [-0.00778084 -0.0837264 -1.0709453 0.05867344 -1.04997243 0.05107772] +2024-12-18 19:03:30,498 - INFO - Checking for faults on the robot... +2024-12-18 19:03:33,501 - INFO - Successfully initialize the robot +2024-12-18 19:03:33,503 - INFO - Current end effector pose: +2024-12-18 19:03:33,504 - INFO - Current joint values: [-0.0077811176888644695, -0.08373042196035385, -1.0709487199783325, 0.058669012039899826, -1.0499733686447144, 0.05107197165489197] +2024-12-18 19:03:33,506 - INFO - Converted joint values [-0.0077811176888644695, -0.08373042196035385, -1.0709487199783325, 0.058669012039899826, -1.0499733686447144, 0.05107197165489197] to Cartesian Pose [ 0.40711183 -0.01031775 0.60591553] and [ 0.4502702 -0.04255652 0.8916342 -0.02083634]. +2024-12-18 19:03:33,523 - INFO - Moved end effector to pose: [0.544156680181676, -0.020046261931804074, 0.7065443288649818], [0.4502701979781566, -0.04255652157821327, 0.8916341952137145, -0.02083634347640151] +2024-12-18 19:03:35,248 - INFO - Checking for faults on the robot... +2024-12-18 19:03:38,252 - INFO - Successfully initialize the robot +2024-12-18 19:03:38,254 - INFO - Current end effector pose: +2024-12-18 19:03:38,255 - INFO - Current joint values: [0.0008705341024324298, -0.08028855174779892, -1.0714375972747803, 0.07522833347320557, -1.05265212059021, 0.029323959723114967] +2024-12-18 19:03:38,257 - INFO - Converted joint values [0.0008705341024324298, -0.08028855174779892, -1.0714375972747803, 0.07522833347320557, -1.05265212059021, 0.029323959723114967] to Cartesian Pose [ 0.40795468 -0.00880593 0.6046628 ] and [ 0.45074449 -0.04397601 0.8914375 -0.01531992]. +2024-12-18 19:03:38,275 - INFO - Moved end effector to pose: [0.5450408113034085, -0.019806113524565863, 0.7051042093298315], [0.4507444869504017, -0.043976013435077045, 0.8914375007472969, -0.015319921265793164] +2024-12-18 19:04:07,824 - INFO - Checking for faults on the robot... +2024-12-18 19:04:10,827 - INFO - Successfully initialize the robot +2024-12-18 19:04:10,829 - INFO - Current end effector pose: +2024-12-18 19:04:10,830 - INFO - Current joint values: [0.009619977325201035, -0.07683710008859634, -1.0719534158706665, 0.09182984381914139, -1.0552924871444702, 0.007551020476967096] +2024-12-18 19:04:10,832 - INFO - Converted joint values [0.009619977325201035, -0.07683710008859634, -1.0719534158706665, 0.09182984381914139, -1.0552924871444702, 0.007551020476967096] to Cartesian Pose [ 0.40879107 -0.00724129 0.60338181] and [ 0.45134669 -0.04538176 0.89114013 -0.00979443]. +2024-12-18 19:04:10,845 - INFO - Moved end effector to pose: [0.40879107281287214, -0.007241288573654841, 0.603381813188234], [0.45134669297750507, -0.04538176065002378, 0.8911401279310248, -0.009794433605394741] +2024-12-18 19:06:01,384 - INFO - Checking for faults on the robot... +2024-12-18 19:06:04,387 - INFO - Successfully initialize the robot +2024-12-18 19:06:04,389 - INFO - Current end effector pose: +2024-12-18 19:06:04,390 - INFO - Current joint values: [0.08845508098602295, -0.43194982409477234, -0.4311617314815521, 0.15491288900375366, -1.3452225923538208, -0.04047024995088577] +2024-12-18 19:06:04,392 - INFO - Converted joint values [0.08845508098602295, -0.43194982409477234, -0.4311617314815521, 0.15491288900375366, -1.3452225923538208, -0.04047024995088577] to Cartesian Pose [0.27241607 0.00688281 0.50179729] and [ 0.45243818 -0.04609228 0.89059914 -0.00289224]. +2024-12-18 19:06:04,411 - INFO - Moved end effector to pose: [0.40946135764922154, -0.006629192710045183, 0.6014768568030846], [0.45243818384761103, -0.04609227938818567, 0.8905991390772001, -0.0028922400361672523] +2024-12-18 19:28:02,212 - INFO - Checking for faults on the robot... +2024-12-18 19:28:05,216 - INFO - Successfully initialize the robot +2024-12-18 19:28:05,218 - INFO - Current end effector pose: +2024-12-18 19:28:05,219 - INFO - Current joint values: [0.10339414328336716, -0.42639681696891785, -0.4312556982040405, 0.17492558062076569, -1.35066819190979, -0.06476274877786636] +2024-12-18 19:28:05,220 - INFO - Current joint values: [0.10339414328336716, -0.42639681696891785, -0.4312556982040405, 0.17492558062076569, -1.35066819190979, -0.06476274877786636] +2024-12-18 19:28:05,259 - INFO - Converted Cartesian position [0.409461365, -0.0066291909999999996, 0.601476868] to joint values [ 0. 0. 0.11791164 -0.43554719 -0.43271841 0.17974884 + -1.34381266 -0.06476275]. +2024-12-18 19:28:05,276 - INFO - Moving arm to joint values: [ 0.11791164 -0.43554719 -0.43271841 0.17974884 -1.34381266 -0.06476275] +2024-12-18 19:28:05,277 - INFO - Converted joint values [0.10339414328336716, -0.42639681696891785, -0.4312556982040405, 0.17492558062076569, -1.35066819190979, -0.06476274877786636] to Cartesian Pose [0.27292347 0.00885446 0.50022169] and [ 0.45369526 -0.0467749 0.88991891 0.00412945]. +2024-12-18 19:28:05,302 - INFO - Moved end effector to pose: [0.4101334719036873, -0.005935331581744431, 0.599492418416786], [0.4536952636757494, -0.04677490147015898, 0.8899189086344537, 0.004129450994019601] +2024-12-18 19:29:25,721 - INFO - Checking for faults on the robot... +2024-12-18 19:29:28,725 - INFO - Successfully initialize the robot +2024-12-18 19:29:45,737 - INFO - Checking for faults on the robot... +2024-12-18 19:29:48,741 - INFO - Successfully initialize the robot +2024-12-18 19:33:22,153 - INFO - Checking for faults on the robot... +2024-12-18 19:33:25,157 - INFO - Successfully initialize the robot +2024-12-18 19:33:25,524 - INFO - Moving arm to joint values: [] +2024-12-18 19:33:43,747 - INFO - Checking for faults on the robot... +2024-12-18 19:33:46,751 - INFO - Successfully initialize the robot +2024-12-18 19:33:46,765 - ERROR - Failed to move arm to joint values: [0.6422811647339133, -0.42062434973063345, 0.4310963252425994, 0.1954768762233649, -1.356120828799594, -0.0890117918517108]. Error code: -8 +2024-12-18 19:34:10,371 - INFO - Checking for faults on the robot... +2024-12-18 19:34:13,373 - INFO - Successfully initialize the robot +2024-12-18 19:34:13,385 - ERROR - Failed to move arm to joint values: [0.2932153143350474, -0.42062434973063345, 0.4310963252425994, 0.1954768762233649, -1.356120828799594, -0.0890117918517108]. Error code: -8 +2024-12-18 19:38:13,960 - INFO - Checking for faults on the robot... +2024-12-18 19:38:16,963 - INFO - Successfully initialize the robot +2024-12-18 19:38:16,981 - INFO - Moving arm to joint values: [0.2932153143350474, -0.42062434973063345, -0.4310963252425994, 0.1954768762233649, -1.356120828799594, -0.0890117918517108] +2024-12-18 19:43:41,648 - INFO - Checking for faults on the robot... +2024-12-18 19:43:44,652 - INFO - Successfully initialize the robot +2024-12-18 19:43:44,654 - INFO - Robot states updated: {'joint_angles': [0.29321083426475525, -0.4206232726573944, -0.43110036849975586, 0.19547133147716522, -1.3561214208602905, -0.08901306986808777], 'joint_velocities': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 'tcp_pose': [404.918396, 65.631699, 599.238892, -0.135326, -0.941132, -2.907577]} +2024-12-18 19:43:44,655 - INFO - Current end effector pose: +2024-12-18 19:43:44,656 - INFO - Current joint values: [0.29321083426475525, -0.4206232726573944, -0.43110036849975586, 0.19547133147716522, -1.3561214208602905, -0.08901306986808777] +2024-12-18 19:43:44,657 - INFO - Current joint values: [0.29321083426475525, -0.4206232726573944, -0.43110036849975586, 0.19547133147716522, -1.3561214208602905, -0.08901306986808777] +2024-12-18 19:43:44,679 - INFO - Converted Cartesian position [0.40491839599999996, 0.065631699, 0.5992388919999999] to joint values [ 0. 0. 0.30733251 -0.43007508 -0.43256943 0.19979759 + -1.34929972 -0.08901307]. +2024-12-18 19:43:44,690 - INFO - Moving arm to joint values: [ 0.30733251 -0.43007508 -0.43256943 0.19979759 -1.34929972 -0.08901307] +2024-12-18 19:43:44,691 - INFO - Converted joint values [0.29321083426475525, -0.4206232726573944, -0.43110036849975586, 0.19547133147716522, -1.3561214208602905, -0.08901306986808777] to Cartesian Pose [0.2673795 0.05821301 0.49846018] and [ 0.45774199 -0.00768479 0.88657217 -0.0663552 ]. +2024-12-18 19:43:44,714 - INFO - Moved end effector to pose: [0.4055321134177814, 0.06622355938701624, 0.5972006695927298], [0.45774198538199745, -0.007684789479678209, 0.8865721665786355, -0.06635519781648087] +2024-12-18 19:48:39,844 - INFO - Checking for faults on the robot... +2024-12-18 19:48:42,849 - INFO - Successfully initialize the robot +2024-12-18 19:48:42,851 - INFO - Robot states updated: {'joint_angles': [0.3071259558200836, -0.4149245321750641, -0.4313860833644867, 0.21474197506904602, -1.3614270687103271, -0.11202086508274078], 'joint_velocities': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 'tcp_pose': {'position': [405.532104, 66.223572, 597.200684], 'orientation_quat': [-0.4577420809017105, 0.007684608623454013, -0.8865720689905066, 0.06635586370448886]}} +2024-12-18 19:48:42,852 - INFO - Current end effector pose: +2024-12-18 19:48:42,853 - INFO - Current joint values: [0.3071259558200836, -0.4149245321750641, -0.4313860833644867, 0.21474197506904602, -1.3614270687103271, -0.11202086508274078] +2024-12-18 19:48:42,854 - INFO - Current joint values: [0.3071259558200836, -0.4149245321750641, -0.4313860833644867, 0.21474197506904602, -1.3614270687103271, -0.11202086508274078] +2024-12-18 19:48:42,881 - INFO - Converted Cartesian position [0.405532104, 0.06622357200000001, 0.597200684] to joint values [ 0. 0. 0.32103894 -0.42479738 -0.43299597 0.21893948 + -1.35442554 -0.11202087]. +2024-12-18 19:48:42,898 - INFO - Moving arm to joint values: [ 0.32103894 -0.42479738 -0.43299597 0.21893948 -1.35442554 -0.11202087] +2024-12-18 19:48:42,899 - INFO - Converted joint values [0.3071259558200836, -0.4149245321750641, -0.4313860833644867, 0.21474197506904602, -1.3614270687103271, -0.11202086508274078] to Cartesian Pose [0.26755402 0.06011599 0.49685333] and [ 0.45931898 -0.00825258 0.88625121 -0.05930228]. +2024-12-18 19:48:42,923 - INFO - Moved end effector to pose: [0.4061248925566705, 0.06689042357420937, 0.5950990421364744], [0.45931898213059913, -0.008252576345193158, 0.8862512099715579, -0.05930228041253287] +2024-12-18 19:50:05,551 - INFO - Checking for faults on the robot... +2024-12-18 19:50:08,554 - INFO - Successfully initialize the robot +2024-12-18 19:50:08,555 - INFO - Robot states updated: {'joint_angles': [0.3212558329105377, -0.40918171405792236, -0.4317369759082794, 0.23415449261665344, -1.36670982837677, -0.13509002327919006], 'joint_velocities': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 'tcp_pose': {'position': [406.124878, 66.890427, 595.09906], 'orientation_quat': [-0.4593230646886693, 0.008253882092979885, -0.8862502553281563, 0.05928474176532797]}} +2024-12-18 19:50:08,556 - INFO - Current end effector pose: +2024-12-18 19:50:08,556 - INFO - Current joint values: [0.3212558329105377, -0.40918171405792236, -0.4317369759082794, 0.23415449261665344, -1.36670982837677, -0.13509002327919006] +2024-12-18 19:50:08,556 - INFO - Current joint values: [0.3212558329105377, -0.40918171405792236, -0.4317369759082794, 0.23415449261665344, -1.36670982837677, -0.13509002327919006] +2024-12-18 19:50:08,566 - INFO - Converted Cartesian position [0.406124878, 0.066890427, 0.59509906] to joint values [ 0. 0. 0.33491388 -0.41949522 -0.4334871 0.23820072 + -1.35952535 -0.13509002]. +2024-12-18 19:50:08,581 - INFO - Moving arm to joint values: [ 0.33491388 -0.41949522 -0.4334871 0.23820072 -1.35952535 -0.13509002] +2024-12-18 19:50:08,581 - INFO - Converted joint values [0.3212558329105377, -0.40918171405792236, -0.4317369759082794, 0.23415449261665344, -1.36670982837677, -0.13509002327919006] to Cartesian Pose [0.26769183 0.0620906 0.49522377] and [ 0.46103698 -0.00873728 0.88580106 -0.05220196]. +2024-12-18 19:50:08,606 - INFO - Moved end effector to pose: [0.4066985024879677, 0.06764196020182099, 0.5929290852952557], [0.4610369792157506, -0.008737284618235619, 0.8858010608341843, -0.05220195665058314] +2024-12-18 19:50:20,862 - INFO - Checking for faults on the robot... +2024-12-18 19:50:23,866 - INFO - Successfully initialize the robot +2024-12-18 19:50:23,869 - INFO - Robot states updated: {'joint_angles': [0.33550843596458435, -0.4033794105052948, -0.4321492314338684, 0.25356510281562805, -1.3719943761825562, -0.1580326110124588], 'joint_velocities': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 'tcp_pose': {'position': [406.698517, 67.641968, 592.929077], 'orientation_quat': [-0.4610324747336115, 0.008736304333459954, -0.8858023358721154, 0.05222026418067217]}} +2024-12-18 19:51:45,453 - INFO - Checking for faults on the robot... +2024-12-18 19:51:48,456 - INFO - Successfully initialize the robot +2024-12-18 19:51:48,826 - INFO - Moved end effector to pose: [0.8121770143508911, 0.04117409512400627, 0.2725994288921356], [-0.4610324747336115, 0.008736304333459954, -0.8858023358721154, 0.05222026418067217] +2024-12-18 19:52:20,156 - INFO - Checking for faults on the robot... +2024-12-18 19:52:23,157 - INFO - Successfully initialize the robot +2024-12-18 19:52:23,521 - INFO - Moved end effector to pose: [0.6121770143508911, 0.04117409512400627, 0.2725994288921356], [-0.4610324747336115, 0.008736304333459954, -0.8858023358721154, 0.05222026418067217] +2024-12-18 19:54:11,057 - INFO - Checking for faults on the robot... +2024-12-18 19:54:14,060 - INFO - Successfully initialize the robot +2024-12-18 19:54:14,063 - INFO - Robot states updated: {'joint_angles': [-0.027931872755289078, -0.47473636269569397, -0.03839745745062828, -0.022691410034894943, -1.057666301727295, 0.0034840537700802088], 'joint_velocities': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 'tcp_pose': {'position': [420.434235, -5.81615, 359.876038], 'orientation_quat': [0.7071068218077394, -0.00032550013355177364, 0.7071065907288765, 0.0003255002399235735]}} +2024-12-18 19:55:01,266 - INFO - Checking for faults on the robot... +2024-12-18 19:55:04,269 - INFO - Successfully initialize the robot +2024-12-18 19:55:04,272 - ERROR - Error updating robot states: unsupported operand type(s) for /: 'list' and 'int' +2024-12-18 19:56:13,014 - INFO - Checking for faults on the robot... +2024-12-18 19:56:16,017 - INFO - Successfully initialize the robot +2024-12-18 19:56:16,020 - INFO - Robot states updated: {'joint_angles': [-0.02792995609343052, -0.47473061084747314, -0.038393620401620865, -0.02269332855939865, -1.0576682090759277, 0.0034840537700802088], 'joint_velocities': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 'tcp_pose': {'position': [0.420434235, -0.00581615, 0.359876038], 'orientation_quat': [0.7071068218077394, -0.00032550013355177364, 0.7071065907288765, 0.0003255002399235735]}} +2024-12-18 19:56:58,801 - INFO - Checking for faults on the robot... +2024-12-18 19:57:01,805 - INFO - Successfully initialize the robot +2024-12-18 19:57:01,818 - INFO - Moved end effector to pose: [0.420434235, -0.00581615, 0.459876038], [0.7071068218077394, -0.00032550013355177364, 0.7071065907288765, 0.0003255002399235735] +2024-12-18 19:57:11,086 - INFO - Checking for faults on the robot... +2024-12-18 19:57:14,090 - INFO - Successfully initialize the robot +2024-12-18 19:57:14,109 - INFO - Moved end effector to pose: [0.420434235, -0.00581615, 0.359876038], [0.7071068218077394, -0.00032550013355177364, 0.7071065907288765, 0.0003255002399235735] +2024-12-18 19:57:45,530 - INFO - Checking for faults on the robot... +2024-12-18 19:57:48,533 - INFO - Successfully initialize the robot +2024-12-18 19:57:48,551 - INFO - Moved end effector to pose: [0.420434235, -0.00581615, 0.459876038], [0.7071068218077394, -0.00032550013355177364, 0.7071065907288765, 0.0003255002399235735] diff --git a/Install/xArm-Python-SDK b/Install/xArm-Python-SDK new file mode 160000 index 0000000..9f6ac54 --- /dev/null +++ b/Install/xArm-Python-SDK @@ -0,0 +1 @@ +Subproject commit 9f6ac54c121324ea1c8808b7884fc076211fab5a diff --git a/Robotics_API/Bestman_Real_Xarm.py b/Robotics_API/Bestman_Real_Xarm.py deleted file mode 100644 index 0ea3ecc..0000000 --- a/Robotics_API/Bestman_Real_Xarm.py +++ /dev/null @@ -1,686 +0,0 @@ -# !/usr/bin/env python -# -*- encoding: utf-8 -*- -""" -# @FileName : Bestman_real_xarm6.py -# @Time : 2024-12-17 22:22:23 -# @Author : Zhaxi & Yan -# @Email : zhaxizhuoma.ayang@gmail.com & yding25@binghamton.edu -# @Description : XXX -# @Usage : XXX -""" - - -import os -import time -import numpy as np -from .Pose import Pose -from scipy.spatial.transform import Rotation as R -import ikpy -from ikpy.chain import Chain -from ikpy.link import URDFLink -from xarm.wrapper import XArmAPI - - -class Bestman_Real_Xarm6: - """ - A class to interface with the XArm6 robotic arm. - - Attributes: - robot: The XArm6 API object initialized with the provided IP address. - frequency: The control frequency for the robot. - log: A logger object to log messages for debugging and monitoring. - robot_states: A container for storing the current state of the robot. - mode: Stores the current operation mode. - gripper: The gripper object for controlling the end effector. - """ - - def __init__(self, robot_ip, local_ip, frequency): - """ - Initializes the xArm robot and related components. - - Args: - robot_ip (str): The IP address of the xArm robot. - frequency (float): Control frequency for the robot. - """ - # Robot initialization - self.robot = XArmAPI(robot_ip) - self.gripper = None - self.frequency = frequency - self.log = self.robot.logger - self.mode = self.robot.set_mode(0) # Default mode - self.robot_states = self.robot.set_state(0) - - # Parse URDF for kinematic chain - current_dir = os.path.dirname(os.path.abspath(__file__)) - urdf_file = os.path.join(current_dir, "../Asset/xarm6_robot.urdf") - - if not os.path.exists(urdf_file): - raise FileNotFoundError(f"URDF file not found: {urdf_file}") - - # Parse the URDF file and configure the kinematic chain - self.robot_chain = Chain.from_urdf_file(urdf_file) - - # Define active links mask for xArm6 (Base + 6 DOF + End Effector) - active_links_mask = [False] + [True] * 6 + [False] # Adjust for 6-DOF robot - self.robot_chain.active_links_mask = active_links_mask - - # Filter active joints (revolute or prismatic) - self.active_joints = [ - joint - for joint in self.robot_chain.links - if isinstance(joint, URDFLink) - and (joint.joint_type == "revolute" or joint.joint_type == "prismatic") - ] - - # Validate the number of active joints for xArm6 - if len(self.active_joints) != 6: # xArm6 is a 6-DOF robot - raise ValueError( - f"Expected 6 active joints, but found {len(self.active_joints)}. " - f"Check the URDF file and active_links_mask." - ) - - # Log successful initialization - self.log.info("xArm6 robot kinematic chain successfully initialized.") - - # Additional initializations can go here - self.log_file = "xarm_command_log.tum" - - # ---------------------------------------------------------------- - # Device Initialization and State Management - # ---------------------------------------------------------------- - - def initialize_robot(self): - """ - Initializes the robot by clearing faults and enabling it. - - This function ensures the robot is operational by: - 1. Clearing any existing faults and warnings. - 2. Setting the robot state to operational (ready). - 3. Waiting until the robot becomes ready. - - Returns: - bool: True if the robot is successfully initialized, False otherwise. - """ - try: - # Step 1: Clear faults and warnings - self.log.info("Checking for faults on the robot...") - self.robot.clean_error() # Clear errors - self.robot.clean_warn() # Clear warnings - time.sleep(1) # Wait for a short duration to allow the robot to reset - - # Step 2: Set the robot to operational state - self.log.info("Setting robot to operational state...") - self.robot.set_state(0) # Set state to '0' (Ready) - time.sleep(1) # Wait for state transition - - # Step 3: Check for ready state - for seconds_waited in range(10): - if self.robot.state == 0: # State '0' indicates Ready - self.log.info("Robot is now operational.") - return True - time.sleep(1) - - self.log.warn("Robot is not operational after 10 seconds.") - return False - - except Exception as e: - self.log.error(f"Failed to initialize the robot: {str(e)}") - return False - - - def update_robot_states(self): - """ - Updates the current robot states by fetching them from the robot. - """ - self.robot.getRobotStates(self.robot_states) - - def go_home(self, dist=0): - """ - Moves the robot arm to its initial (home) pose. - """ - self.robot.set_position( - x=396.4 + dist, y=-5.5, z=360, roll=-90, pitch=-90, yaw=-90, wait=False - ) - time.sleep(3) - - def set_mode(self, _mode): - """ - Parameters: - _mode= - 0: position controlmode - 1: servo motionmode - 2: joint teachingmodemode (invalid)3: cartesian teaching - 4: joint velocity control mode - 5: cartesian velocity control mode - 6: joint online trajectory planningmode - 7: cartesian online trajectory planning - - Notes: - https://github.com/xArm-Developer/xArm-Python-SDK/blob/master/doc/api/xarm_api.md#mode - """ - print("set mode") - self.robot.set_mode(_mode) - - def pose_to_euler(self, pose): - """ - Convert robot pose from a list [x, y, z, qw, qx, qy, qz] to [x, y, z] and Euler angles. - - Parameters: - pose: list of 7 floats - [x, y, z, qw, qx, qy, qz] - - Returns: - tuple: (x, y, z, roll, pitch, yaw) where (x, y, z) is the position and (roll, pitch, yaw) are the Euler angles in radians. - """ - x, y, z, qw, qx, qy, qz = pose - r = R.from_quat( - [qx, qy, qz, qw] - ) # Reordering to match scipy's [qx, qy, qz, qw] - roll, pitch, yaw = r.as_euler("xyz", degrees=False) - return [x, y, z, roll, pitch, yaw] - - def euler_to_pose(self, position_euler): - """ - Convert robot pose from [x, y, z, roll, pitch, yaw] to [x, y, z, qw, qx, qy, qz]. - - Parameters: - position_euler: list of 6 floats - [x, y, z, roll, pitch, yaw] - - Returns: - list: [x, y, z, qw, qx, qy, qz] - """ - x, y, z, roll, pitch, yaw = position_euler - r = R.from_euler("xyz", [roll, pitch, yaw], degrees=False) - qx, qy, qz, qw = r.as_quat() # Getting [qx, qy, qz, qw] from scipy - return [x, y, z, qw, qx, qy, qz] # Reordering to match [qw, qx, qy, qz] - - def set_pay_load(self, payload_weight, payload_cog): - - self.robot.set_tcp_load(payload_weight, payload_cog) - - # ---------------------------------------------------------------- - # Functions for others - # ---------------------------------------------------------------- - - def get_joint_bounds(self): - """ - Retrieves the joint bounds of the robot arm. - - Returns: - list: A list of tuples representing the joint bounds, where each tuple contains the minimum and maximum values for a joint. - """ - maxbounds = self.robot.info().qMax - minbounds = self.robot.info().qMin - jointbounds = list(zip(maxbounds, minbounds)) - return jointbounds - - def get_DOF(self): - """ - Retrieves the degree of freedom (DOF) of the robot arm. - - Returns: - int: The degree of freedom of the robot arm. - """ - return 6 - - # ---------------------------------------------------------------- - # Functions for joint control - # ---------------------------------------------------------------- - - def print_joint_link_info(self, name): - """ - Prints the joint and link information of a robot. - - Args: - name (str): 'base' or 'arm' - """ - if name == "base": - print("Base joint and link information:") - for i, link in enumerate( - self.robot_chain.links[:1] - ): # Assuming the base is the first link - print(f"Link {i}: {link.name}") - elif name == "arm": - print("Arm joint and link information:") - for i, link in enumerate( - self.robot_chain.links[1:] - ): # Assuming the arm starts from the second link - print(f"Link {i + 1}: {link.name}") - - def get_joint_idx(self): - """ - Retrieves the indices of the joints in the robot arm. - - Returns: - list: A list of indices for the joints in the robot arm. - """ - return list(range(len(self.active_joints))) - - def get_tcp_link(self): - """ - Retrieves the TCP (Tool Center Point) link of the robot arm. - - Returns: - str: The TCP link of the robot arm. - """ - return self.robot_chain.links[6].name - - def get_current_joint_angles(self): - """ - Retrieves the current joint angles of the robot arm. - - Returns: - list: A list of the current joint angles of the robot arm. - """ - _joint_states = self.robot.get_joint_states(is_radian=True) - _joint_angles = _joint_states[1][0][0:6] - - return _joint_angles - - def get_current_joint_velocities(self): - """ - Retrieves the current joint velocities of the robot arm. - - Returns: - list: A list of the current joint velocities of the robot arm. - """ - - _joint_states = self.robot.get_joint_states(is_radian=True) - _joint_velocities = _joint_states[1][1][0:6] - - return _joint_velocities - - def move_arm_to_joint_angles( - self, - joint_angles, - target_vel=None, - target_acc=None, - MAX_VEL=None, - MAX_ACC=None, - wait_for_finish=None, - ): - """ - Move arm to a specific set of joint angles, considering physics. - - Args: - Set the servo angle, the API will modify self.last_used_angles value - Note: https://github.com/xArm-Developer/xArm-Python-SDK/blob/master/doc/api/xarm_api.md#def-set_servo_angleself-servo_idnone-anglenone-speednone-mvaccnone-mvtimenone-relativefalse-is_radiannone-waitfalse-timeoutnone-radiusnone-kwargs - """ - - #! Force mode switch - self.robot.set_mode(6) # 6: online joint - self.robot.set_state(0) - - self.robot.set_servo_angle( - angle=joint_angles, is_radian=True, speed=0.7, wait=wait_for_finish - ) # speed in rad/s - - def move_arm_follow_joint_angles( - self, - target_trajectory, - target_vel=None, - target_acc=None, - MAX_VEL=None, - MAX_ACC=None, - ): - """ - Move arm to a few set of joint angles, considering physics. - - Args: - target_trajectory: A list of desired joint angles (in radians) for each joint of the arm. - target_vel: Optional. A list of target velocities for each joint. - target_acc: Optional. A list of target accelerations for each joint. - MAX_VEL: Optional. A list of maximum velocities for each joint. - MAX_ACC: Optional. A list of maximum accelerations for each joint. - """ - period = 1.0 / self.frequency - self.update_robot_states() - DOF = len(self.robot_states.q) - - for target_pos in target_trajectory: - # Monitor fault on robot server - if self.robot.isFault(): - raise Exception("Fault occurred on robot server, exiting ...") - - # Send command - self.robot.set_servo_angle( - angle=target_pos, is_radian=True, speed=target_vel, wait=True - ) # speed in rad/s - print(f"Sent joint positions: {target_pos}") - - # Use sleep to control loop period - time.sleep(period) - - # ---------------------------------------------------------------- - # Functions for end effector - # ---------------------------------------------------------------- - - # TODO - def get_current_tcp_speed(self): - """ - Retrieves the current tcp velocities of the robot arm. - - Returns: - list: A list of the current tcp velocities of the robot arm. - """ - speed = self.robot.realtime_tcp_speed - return speed - - def get_current_eef_pose(self): - """ - Retrieves the current pose of the robot arm's end effector. - - This function obtains the position and orientation of the end effector. - - Returns: - pose: the [x, y, z, roll, pitch, yaw] value of tcp in meter and radian - """ - - _pose = self.robot.get_position(is_radian=True) - pose = _pose[1] - pose[0] = pose[0] / 1000 - pose[1] = pose[1] / 1000 - pose[2] = pose[2] / 1000 - return pose - - def move_eef_to_tcp_velocity(self, _velocity_setpoint, _duration): - """ - Move arm's end effector to a target tcp velocity. - - Args: - _velocity_setpoint: mm/s in transition, rad/s in orientation - _duration: function calling loop time - Notes: - https://github.com/xArm-Developer/xArm-Python-SDK/blob/master/doc/api/xarm_api.md#def-vc_set_cartesian_velocityself-speeds-is_radiannone-is_tool_coordfalse-duration-1-kwargs - """ - - self.robot.set_mode(5) # 5 for cartesian vel - self.robot.set_state(0) - self.robot.vc_set_cartesian_velocity( - speeds=[ - _velocity_setpoint[0], - _velocity_setpoint[1], - _velocity_setpoint[2], - _velocity_setpoint[3], - _velocity_setpoint[4], - _velocity_setpoint[5], - ], - duration=_duration, - ) - - def move_eef_to_goal_pose(self, goal_pose, speed=1000, mvacc=50000, wait=False): - """ - Move arm's end effector to a target position. - - Args: - goal_pose (Pose): The desired pose of the end effector (includes both position in mm and euler_orientation), please use radian for orientaiton. - """ - - self.robot.set_position( - x=goal_pose[0] * 1000, - y=goal_pose[1] * 1000, - z=goal_pose[2] * 1000, - roll=goal_pose[3], - pitch=goal_pose[4], - yaw=goal_pose[5], - speed=speed, - mvacc=mvacc, - is_radian=True, - wait=wait, - ) - - def rotate_eef_joint(self, angle): - """ - Rotate the end effector of the robot arm by a specified angle by joint. - - Args: - angle (float): The desired rotation angle in radians. - """ - current_joint_angles = self.get_current_joint_angles() - - target_joint_angles = current_joint_angles.copy() - target_joint_angles[6] += angle - target_vel = 1 - self.robot.set_mode(6) # 0: joint control mode; 6: online joint - self.robot.set_state(0) - - self.robot.set_servo_angle( - angle=target_joint_angles, is_radian=True, speed=target_vel, wait=False - ) # speed in rad/s - - # ---------------------------------------------------------------- - # Functions for IK - # ---------------------------------------------------------------- - - def joints_to_cartesian(self, joint_angles): - """ - Transforms the robot arm's joint angles to its Cartesian coordinates. - - Args: - joint_angles (list): A list of joint angles for the robot arm. - - Returns: - tuple: A tuple containing the Cartesian coordinates (position and orientation) of the robot arm. - """ - # Validate the number of joint values matches the number of active joints - if len(joint_angles) != len(self.active_joints): - raise ValueError( - "The number of joint values does not match the number of active joints" - ) - - # Map joint values to the full joint chain - full_joint_angles = np.zeros(len(self.robot_chain.links)) - active_joint_indices = [ - self.robot_chain.links.index(joint) for joint in self.active_joints - ] - - for i, joint_value in enumerate(joint_angles): - full_joint_angles[active_joint_indices[i]] = joint_value - - # Calculate the end effector position and orientation - cartesian_matrix = self.robot_chain.forward_kinematics(full_joint_angles) - - # Extract position and orientation - position = cartesian_matrix[:3, 3] - orientation_matrix = cartesian_matrix[:3, :3] - - # Convert rotation matrix to quaternion - r = R.from_matrix(orientation_matrix) - quaternion = r.as_quat() - orientation = [quaternion[3], quaternion[0], quaternion[1], quaternion[2]] - - return position, orientation - - def cartesian_to_joints(self, position, orientation): - """ - Transforms the robot arm's Cartesian coordinates to its joint angles. - - Args: - position (list): The Cartesian position of the robot arm. - orientation (list): The Cartesian orientation of the robot arm. - - Returns: - list: A list of joint angles corresponding to the given Cartesian coordinates. - """ - rotation_matrix = R.from_euler("xyz", orientation).as_matrix() - - # Combine rotation matrix and position into a list - target_pose = np.eye(4) - target_pose[:3, :3] = rotation_matrix - target_pose[:3, 3] = position - - initial_joint_angles = [0] * len(self.robot_chain) - - # inverse kinematics calculations and return joint angles - joint_angles = ikpy.inverse_kinematics.inverse_kinematic_optimization( - chain=self.robot_chain, - target_frame=target_pose, - starting_nodes_angles=initial_joint_angles, - orientation_mode="all", - ) - - return joint_angles[1:8] - - def calculate_IK_error(self, goal_position, goal_orientation): - """ - Calculate the inverse kinematics (IK) error for performing pick-and-place manipulation of an object using a robot arm. - - Args: - goal_position: The desired goal position for the target object. - goal_orientation: The desired goal orientation for the target object. - """ - pass - - # ---------------------------------------------------------------- - # Functions for gripper - # ---------------------------------------------------------------- - - def find_gripper_xarm(self): - """ - Searches for the gripper on available serial ports and returns the port if found. - - Returns: - str: The serial port where the gripper is connected, or None if not found. - """ - _pos = self.robot.get_gripper_position() - _ver = self.robot.get_gripper_version() - - if _ver is not None and _pos is not None: - print("Have Xarm gripper", _ver) - return True - else: - print("Not found Xarm gripper") - return None - - def find_gripper_robotiq(self): - """ - Config the parameter via Python SDK - """ - # Baud rate - # Modify the baud rate to 115200, the default is 2000000. - self.robot.set_tgpio_modbus_baudrate(115200) - - # TCP Payload and offset - # Robotiq 2F/85 Gripper - self.robot.set_tcp_load(0.925, [0, 0, 58]) - self.robot.set_tcp_offset([0, 0, 174, 0, 0, 0]) - self.robot.save_conf() - - # Self-Collision Prevention Model - # Robotiq 2F/85 Gripper - self.robot.set_collision_tool_model(4) - - self.robot.robotiq_reset() - self.robot.robotiq_set_activate() # enable the robotiq gripper - - def get_gripper_position_xarm(self): - """ - Get the position of the XArm gripper. - """ - gripper_pos = self.robot.get_gripper_position() - - return gripper_pos[1] - - def get_gripper_position_robotiq(self, number_of_registers=3): - """ - Reading the status of robotiq gripper - - :param number_of_registers: number of registers, 1/2/3, default is 3 - number_of_registers=1: reading the content of register 0x07D0 - number_of_registers=2: reading the content of register 0x07D0/0x07D1 - number_of_registers=3: reading the content of register 0x07D0/0x07D1/0x07D2 - - Note: - register 0x07D0: Register GRIPPER STATUS - register 0x07D1: Register FAULT STATUS and register POSITION REQUEST ECHO - register 0x07D2: Register POSITION and register CURRENT - :return: tuple((code, robotiq_response)) - code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details. - robotiq_response: See the robotiq documentation - """ - status = self.robot.robotiq_get_status(number_of_registers=number_of_registers) - gripper_width = status[1][-2] - return gripper_width - - def gripper_goto_xarm(self, value, speed=5000, force=None): - """ - Moves the gripper to a specified position with given speed. - - Args: - value (int): Position of the gripper. Integer between 0 and 800. - 0 represents the open position, and 255 represents the closed position. - speed (int): Speed of the gripper movement, between 0 and 8000. - force (int): Not applicable for xarm gripper - - Note: - - 0 means fully closed. - - 800 means fully open. - """ - self.robot.set_gripper_position( - pos=value, speed=speed, wait=False, timeout=1, auto_enable=True - ) - - def gripper_goto_robotiq( - self, pos, speed=0xFF, force=0xFF, wait=False, timeout=5, **kwargs - ): - """ - Go to the position with determined speed and force. - - :param pos: position of the gripper. Integer between 0 and 255. 0 being the open position and 255 being the close position. - :param speed: gripper speed between 0 and 255 - :param force: gripper force between 0 and 255 - :param wait: whether to wait for the robotion motion complete, default is True - :param timeout: maximum waiting time(unit: second), default is 5, only available if wait=True - - :return: tuple((code, robotiq_response)) - code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details. - robotiq_response: See the robotiq documentation - """ - return self.robot.robotiq_set_position( - pos, speed=speed, force=force, wait=wait, timeout=timeout, **kwargs - ) - - def open_gripper_xarm(self): - """Opens the gripper to its maximum position with maximum speed and force.""" - self.gripper_goto(value=850, speed=5000, force=None) - - def open_gripper_robotiq( - self, speed=0xFF, force=0xFF, wait=False, timeout=5, **kwargs - ): - """ - Open the robotiq gripper - - :param speed: gripper speed between 0 and 255 - :param force: gripper force between 0 and 255 - :param wait: whether to wait for the robotiq motion to complete, default is True - :param timeout: maximum waiting time(unit: second), default is 5, only available if wait=True - - :return: tuple((code, robotiq_response)) - code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details. - robotiq_response: See the robotiq documentation - """ - return self.robot.robotiq_open( - speed=speed, force=force, wait=wait, timeout=timeout, **kwargs - ) - - def close_gripper_xarm(self): - """Closes the gripper to its minimum position with maximum speed and force.""" - self.gripper_goto(value=0, speed=5000, force=None) - - def close_gripper_robotiq( - self, speed=0xFF, force=0xFF, wait=False, timeout=5, **kwargs - ): - """ - Close the robotiq gripper - - :param speed: gripper speed between 0 and 255 - :param force: gripper force between 0 and 255 - :param wait: whether to wait for the robotiq motion to complete, default is True - :param timeout: maximum waiting time(unit: second), default is 3, only available if wait=True - - :return: tuple((code, robotiq_response)) - code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details. - robotiq_response: See the robotiq documentation - """ - return self.robot.robotiq_close( - speed=speed, force=force, wait=wait, timeout=timeout, **kwargs - ) diff --git a/Robotics_API/Bestman_Real_Xarm6.py b/Robotics_API/Bestman_Real_Xarm6.py new file mode 100644 index 0000000..b3721b2 --- /dev/null +++ b/Robotics_API/Bestman_Real_Xarm6.py @@ -0,0 +1,808 @@ +# !/usr/bin/env python +# -*- encoding: utf-8 -*- +""" +# @FileName : Bestman_real_xarm6.py +# @Time : 2024-12-17 22:22:23 +# @Author : Zhaxi & Yan +# @Email : zhaxizhuoma.ayang@gmail.com & yding25@binghamton.edu +# @Description : XXX +# @Usage : XXX +""" + + +import os +import sys +import time +import numpy as np +from .Pose import Pose +from scipy.spatial.transform import Rotation as R +import ikpy +from ikpy.chain import Chain +from ikpy.link import URDFLink +from xarm.wrapper import XArmAPI +import logging +import math + + +class Bestman_Real_Xarm6: + """ + A class to interface with the XArm6 robotic arm. + + Attributes: + robot: The XArm6 API object initialized with the provided IP address. + frequency: The control frequency for the robot. + log: A logger object to log messages for debugging and monitoring. + robot_states: A container for storing the current state of the robot. + mode: Stores the current operation mode. + gripper: The gripper object for controlling the end effector. + """ + + def __init__(self, robot_ip, local_ip=None, frequency=None): + """ + Initializes the xArm robot and related components. + + Args: + robot_ip (str): The IP address of the xArm robot. + """ + # Robot initialization + self.robot = XArmAPI(robot_ip) + self.gripper = None + self.local_ip = local_ip + self.frequency = frequency + self.robot.set_mode(0) # Default mode + + # Parse URDF for kinematic chain + current_dir = os.path.dirname(os.path.abspath(__file__)) + urdf_file = os.path.join(current_dir, "../Asset/xarm6_robot.urdf") + + if not os.path.exists(urdf_file): + raise FileNotFoundError(f"URDF file not found: {urdf_file}") + + # Parse the URDF file and configure the kinematic chain + self.robot_chain = Chain.from_urdf_file(urdf_file, base_elements=["world"]) + + # Define active links mask for xArm6 (Base + 6 DOF + End Effector) + active_links_mask = [False] + [True] * 6 + [False] # Adjust for 6-DOF robot + self.robot_chain.active_links_mask = active_links_mask + + # Filter active joints (revolute or prismatic) + self.active_joints = [ + joint + for joint in self.robot_chain.links + if isinstance(joint, URDFLink) + and (joint.joint_type == "revolute" or joint.joint_type == "prismatic") + ] + + # Validate the number of active joints for xArm6 + if len(self.active_joints) != 6: # xArm6 is a 6-DOF robot + raise ValueError( + f"Expected 6 active joints, but found {len(self.active_joints)}. " + f"Check the URDF file and active_links_mask." + ) + + # Configure the logger (simplified setup) + logging.basicConfig( + level=logging.INFO, + format="%(asctime)s - %(levelname)s - %(message)s", + handlers=[ + logging.StreamHandler(), # Output to console + logging.FileHandler("xarm_command_log.log"), # Output to file + ], + ) + self.log = logging.getLogger("XArmRobotController") + + # ---------------------------------------------------------------- + # Device Initialization and State Management + # ---------------------------------------------------------------- + + + + def initialize_robot(self): + """ + Initializes the robot by clearing faults and enabling it. + + Returns: + bool: True if the robot is successfully initialized, False otherwise. + """ + try: + # Step 1: Clear faults and warnings + self.log.info("Checking for faults on the robot...") + self.robot.clean_error() # Clear errors + self.robot.clean_warn() # Clear warnings + time.sleep(3) # Wait for a short duration to allow the robot to reset + self.log.info(f"Successfully initialize the robot") + return True + + except Exception as e: + self.log.error(f"Failed to initialize the robot: {str(e)}") + return False + + def update_robot_states(self): + """ + Fetches and updates the current robot states (joint angles and TCP pose in quaternion format). + """ + try: + # Retrieve joint states + joint_states = self.robot.get_joint_states(is_radian=True) + joint_angles = joint_states[1][0][:6] + joint_velocities = joint_states[1][1][:6] + + # Retrieve end effector pose (Euler angles) + success, tcp_pose_raw = self.robot.get_position(is_radian=True) + if success != 0: + raise ValueError("Failed to retrieve end-effector pose") + + # Convert position from millimeters to meters + position = [value / 1000 for value in tcp_pose_raw[:3]] # x, y, z in meters + + # Convert orientation from Euler angles to quaternion + roll, pitch, yaw = tcp_pose_raw[3:6] # Extract Euler angles + orientation_quat = R.from_euler('xyz', [roll, pitch, yaw]).as_quat() + + # Store updated robot states + self.robot_states = { + "joint_angles": joint_angles, + "joint_velocities": joint_velocities, + "tcp_pose": { + "position": position, # Position in meters + "orientation_quat": orientation_quat.tolist(), # Quaternion (x, y, z, w) + }, + } + + # Log and display TCP pose in quaternion format + formatted_quaternion = ["%.2f" % i for i in orientation_quat] + self.log.info(f"Robot states updated: {self.robot_states}") + print("tcp_pose(quaternion):", formatted_quaternion) + + except Exception as e: + self.log.error(f"Error updating robot states: {str(e)}") + + def go_home(self): + """ + Moves the robot arm to its initial (home) pose. + Converts input position (in meters) to millimeters for the xArm API, + and quaternion orientation to Euler angles. + """ + position = [0.420, -0.0055, 0.36] # x, y, z in meters + orientation_quat = [ + 0.7071065119402606, + -0.0006170669964236503, + 0.7071065119402605, + 0.0006170669964236504, + ] # Identity quaternion (no rotation) + pose = Pose(position, orientation_quat) + + position_mm = [ + p * 1000 for p in position + ] # Convert position to millimeters for xArm API + + euler_orientation = pose.get_orientation( + type="euler" + ) # Convert to [roll, pitch, yaw] + euler_orientation_degree = np.degrees( + euler_orientation + ) # Convert radians to degrees for xArm API + + # Command the robot to move to the home pose + self.robot.set_position( + x=position_mm[0], + y=position_mm[1], + z=position_mm[2], + roll=euler_orientation_degree[0], + pitch=euler_orientation_degree[1], + yaw=euler_orientation_degree[2], + ) + + self.log.info(f"Robot moved to home position") + + def set_pay_load(self, payload_weight, payload_cog): + self.robot.set_tcp_load(payload_weight, payload_cog) + + # ---------------------------------------------------------------- + # Arm Functions + # ---------------------------------------------------------------- + def get_joint_bounds(self): + """ + Retrieves the joint limits of the xArm6 robot arm. + + Returns: + list: A list of tuples, where each tuple contains the minimum and maximum value for each joint. + """ + joint_min = [-3.14, -1.57, -3.14, -1.57, -3.14, -1.57] + joint_max = [3.14, 2.09, 3.14, 2.09, 3.14, 2.09] + + return list(zip(joint_min, joint_max)) + + def print_arm_jointInfo(self): + """ + Prints the joint and link information of the robot's arm. + + Returns: + None + """ + print("Arm joint and link information:") + for i, link in enumerate( + self.robot_chain.links[1:] + ): # Assuming arm starts at the second link + print(f"Link {i + 1}: {link.name}") + self.log.info(f"Link {i + 1}: {link.name}") + + def get_joint_idx(self): + """ + Retrieves the indices of the joints in the robot arm. + + Returns: + list: A list of indices for the joints in the robot arm. + """ + return list(range(len(self.active_joints))) + + def get_DOF(self): + """ + Retrieves the degree of freedom (DOF) of the xArm robot. + + Returns: + int: The number of degrees of freedom. + """ + dof = 6 # xArm6 is a 6-DOF robotic arm + self.log.info(f"Robot DOF: {dof}") + return dof + + def get_tcp_link(self): + """ + Retrieves the name of the TCP (Tool Center Point) link. + + Returns: + str: Name of the TCP link. + """ + try: + tcp_link_name = self.robot_chain.links[-1].name + self.log.info(f"TCP link retrieved: {tcp_link_name}") + return tcp_link_name + except IndexError as e: + self.log.error(f"Failed to retrieve TCP link: {str(e)}") + return None + + def get_current_joint_values(self): + """ + Retrieves the current joint angles of the robot arm. + + Returns: + list[float]: A list of the current joint angles of the robot arm (in radians). + """ + try: + # Get joint states and extract joint angles for the first 6 joints + joint_states = self.robot.get_joint_states(is_radian=True) + joint_values = joint_states[1][0][:6] + self.log.info(f"Current joint values: {joint_values}") + return joint_values + except Exception as e: + self.log.error(f"Failed to retrieve current joint values: {str(e)}") + return [] + + def get_current_joint_velocities(self): + """ + Retrieves the current joint velocities of the robot arm. + + Returns: + list[float]: A list of the current joint velocities of the robot arm (in radians per second). + """ + try: + # Get joint states and extract joint velocities for the first 6 joints + joint_states = self.robot.get_joint_states(is_radian=True) + joint_velocities = joint_states[1][1][:6] + self.log.info(f"Current joint velocities: {joint_velocities}") + return joint_velocities + except Exception as e: + self.log.error(f"Failed to retrieve current joint velocities: {str(e)}") + return [] + + def get_current_eef_pose(self): + """ + Retrieves the current pose of the robot arm's end effector. + + Returns: + Pose: A Pose object representing the end effector's position and orientation. + """ + try: + success, raw_pose = self.robot.get_position(is_radian=True) + if success != 0: + raise ValueError( + f"Failed to retrieve end effector pose, error code: {success}" + ) + # Convert position values from millimeters to meters + position = [ + raw_pose[0] / 1000, # x in meters + raw_pose[1] / 1000, # y in meters + raw_pose[2] / 1000, # z in meters + ] + orientation = raw_pose[3:] # roll, pitch, yaw (already in radians) + pose = Pose(position, orientation) + self.log.info(f"Current end effector pose: {pose}") + return pose + except Exception as e: + self.log.error(f"Failed to retrieve end effector pose: {str(e)}") + return None + + def move_arm_to_joint_values( + self, joint_values, target_vel=None, target_acc=None, max_vel=None, max_acc=None + ): + """ + Moves the robotic arm to a specific set of joint values. + + Args: + joint_values (list[float]): Target joint values in radians. + """ + try: + # Set robot to online joint control mode + if self.robot.set_mode(6) != 0: + self.log.error( + "Failed to set robot mode to 'online joint control mode'." + ) + return + if self.robot.set_state(0) != 0: + self.log.error("Failed to set robot state to operational.") + return + + # Execute joint angle command + result = self.robot.set_servo_angle( + angle=joint_values, + is_radian=True, + speed=0.7, # Fixed speed in radians per second + wait=False, # Disable this function + ) + + if result != 0: + self.log.error( + f"Failed to move arm to joint values: {joint_values}. Error code: {result}" + ) + else: + self.log.info(f"Moving arm to joint values: {joint_values}") + + except Exception as e: + self.log.error( + f"An error occurred while moving arm to joint values: {str(e)}" + ) + + # TODO + def get_current_tcp_speed(self): + """ + Retrieves the current tcp velocities of the robot arm. + + Returns: + list: A list of the current tcp velocities of the robot arm. + """ + speed = self.robot.realtime_tcp_speed + return speed + + # ---------------------------------------------------------------- + # End Effector (EEF) Functions + # ---------------------------------------------------------------- + + def move_eef_to_goal_pose(self, goal_pose, max_linear_vel=100, max_angular_vel=5000): + """ + Move arm's end effector to a target position. + + Args: + goal_pose (Pose): The desired pose of the end effector (includes both position in mm and euler_orientation), please use radian for orientaiton. + """ + try: + # Set robot to online joint control mode + if self.robot.set_mode(7) != 0: + self.log.error( + "Failed to set robot mode to 'online joint control mode'." + ) + return + if self.robot.set_state(0) != 0: + self.log.error("Failed to set robot state to operational.") + return + + if not isinstance(goal_pose, Pose): + raise TypeError("pose must be an instance of Pose") + + position = goal_pose.get_position() + orientation = goal_pose.get_orientation(type="euler") + + self.robot.set_position( + x=position[0] * 1000, + y=position[1] * 1000, + z=position[2] * 1000, + roll=orientation[0], + pitch=orientation[1], + yaw=orientation[2], + speed=max_linear_vel, + mvacc=max_angular_vel, + is_radian=True, + wait=False, + ) + self.log.info(f"Moved end effector to pose: {goal_pose.position}, {goal_pose.orientation}") + + except Exception as e: + self.log.error(f"Failed to move end effector to goal pose: {str(e)}") + + def move_eef_to_tcp_velocity(self, _velocity_setpoint, _duration): + #TODO: No test + """ + Move arm's end effector to a target tcp velocity. + + Args: + _velocity_setpoint: mm/s in transition, rad/s in orientation + _duration: function calling loop time + Notes: + https://github.com/xArm-Developer/xArm-Python-SDK/blob/master/doc/api/xarm_api.md#def-vc_set_cartesian_velocityself-speeds-is_radiannone-is_tool_coordfalse-duration-1-kwargs + """ + + self.robot.set_mode(5) # 5 for cartesian vel + self.robot.set_state(0) + self.robot.vc_set_cartesian_velocity( + speeds=[ + _velocity_setpoint[0], + _velocity_setpoint[1], + _velocity_setpoint[2], + _velocity_setpoint[3], + _velocity_setpoint[4], + _velocity_setpoint[5], + ], + duration=_duration, + ) + + def rotate_eef_tcp(self, axis, angle): + # TODO: No Test + """ + Rotate the end effector of the robot arm by a specified angle by joint. + + Args: + angle (float): The desired rotation angle in radians. + """ + current_joint_values = self.get_current_joint_values() + + target_joint_values = current_joint_values.copy() + target_joint_values[6] += angle + target_vel = 1 + self.robot.set_mode(6) # 0: joint control mode; 6: online joint + self.robot.set_state(0) + + self.robot.set_servo_angle( + angle=target_joint_values, is_radian=True, speed=target_vel, wait=False + ) # speed in rad/s + + # ---------------------------------------------------------------- + # Functions for IK + # ---------------------------------------------------------------- + + # def joints_to_cartesian(self, joint_values): + # """ + # Converts the robot's joint angles to its Cartesian coordinates. + + # Args: + # joint_values (list[float]): A list of joint angles for the robot arm. + + # Returns: + # Pose: A Pose object representing the Cartesian position and quaternion orientation. + + # Raises: + # ValueError: If the number of joint values does not match the number of active joints. + # """ + # try: + # if len(joint_values) != len(self.active_joints): + # raise ValueError( + # "The number of joint values does not match the number of active joints." + # ) + + # full_joint_values = np.zeros(len(self.robot_chain.links)) + # active_joint_indices = [ + # self.robot_chain.links.index(joint) for joint in self.active_joints + # ] + + # for i, joint_value in enumerate(joint_values): + # full_joint_values[active_joint_indices[i]] = joint_value + + # cartesian_matrix = self.robot_chain.forward_kinematics(full_joint_values) + # position = cartesian_matrix[:3, 3] + # orientation_matrix = cartesian_matrix[:3, :3] + # quaternion = R.from_matrix(orientation_matrix).as_quat() + + # self.log.info(f"Converted joint values {joint_values} to Cartesian Pose {position} and {quaternion}.") + + # def calculate_new_pose(x, y, z, quaternion, distance = -0.1703): + # # Step 1: 根据 roll, pitch, yaw 计算旋转矩阵 + # rotation = R.from_quat(quaternion) + # rotation_matrix = rotation.as_matrix() + # # Step 2: 提取旋转矩阵的 z 轴方向 + # z_axis = rotation_matrix[:, 2] # 第三列就是 z 轴方向 + # new_position = np.array([x, y, z]) - distance * z_axis + # # Step 3: 返回新的位姿 (新位置 + 原来的姿态) + # return [new_position[0], new_position[1], new_position[2]], quaternion + + # position, quaternion= calculate_new_pose(position[0],position[1],position[2], quaternion) + # print('nnn',position) + + # return Pose(position, quaternion) + # except Exception as e: + # self.log.error(f"Error converting joint values to Cartesian: {str(e)}") + # raise + + def joints_to_cartesian(self, joint_values): + """ + Converts the robot's joint angles to its Cartesian coordinates. + + Args: + joint_values (list[float]): A list of joint angles for the robot arm. + + Returns: + Pose: A Pose object representing the Cartesian position and quaternion orientation. + + Raises: + ValueError: If the number of joint values does not match the number of active joints. + """ + try: + if len(joint_values) != len(self.active_joints): + raise ValueError( + "The number of joint values does not match the number of active joints." + ) + + full_joint_values = np.zeros(len(self.robot_chain.links)) + active_joint_indices = [ + self.robot_chain.links.index(joint) for joint in self.active_joints + ] + + for i, joint_value in enumerate(joint_values): + full_joint_values[active_joint_indices[i]] = joint_value + + cartesian_matrix = self.robot_chain.forward_kinematics(full_joint_values) + position = cartesian_matrix[:3, 3] + orientation_matrix = cartesian_matrix[:3, :3] + quaternion = R.from_matrix(orientation_matrix).as_quat() + + # Add the gripper length along the end-effector's Z-axis + gripper_length = 0.17 # 17cm; adjust as needed + gripper_offset = gripper_length * orientation_matrix[:, 2] # Z-axis of the end-effector + adjusted_position = position + gripper_offset + + self.log.info(f"Converted joint values {joint_values} to Cartesian Pose {position} and {quaternion}.") + return Pose(adjusted_position, quaternion) + except Exception as e: + self.log.error(f"Error converting joint values to Cartesian: {str(e)}") + raise + + # def cartesian_to_joints(self, pose, initial_joint_angles=None): + # position = pose.position + # orientation = pose.orientation + + # def calculate_new_pose(x, y, z, quaternion, distance = 0.1703): + # # Step 1: 根据 roll, pitch, yaw 计算旋转矩阵 + # rotation = R.from_quat(quaternion) + # rotation_matrix = rotation.as_matrix() + # # Step 2: 提取旋转矩阵的 z 轴方向 + # z_axis = rotation_matrix[:, 2] # 第三列就是 z 轴方向 + # new_position = np.array([x, y, z]) - distance * z_axis + # # Step 3: 返回新的位姿 (新位置 + 原来的姿态) + # return [new_position[0], new_position[1], new_position[2]], quaternion + + # position, quaternion= calculate_new_pose(position[0],position[1],position[2], orientation) + + # my_chain = ikpy.chain.Chain.from_urdf_file("../Asset/xarm6_robot.urdf", base_elements=['world']) + # """ + # 将机械臂的笛卡尔坐标转换为关节角度。 + # """ + # rotation = R.from_quat(orientation) + # rotation_matrix = rotation.as_matrix() + + # if initial_joint_angles is None: + # # initial_joint_angles = [0, 0, -0.02792803756892681, -0.47473636269569397, -0.0384012907743454, -0.022689493373036385, -1.057666301727295, 0.0034878887236118317] + # initial_joint_angles = [0, 0] + self.get_current_joint_values() + + # joint_angles = my_chain.inverse_kinematics( + # position, + # rotation_matrix, + # orientation_mode='all', + # initial_position=initial_joint_angles) + + # print('4',joint_angles) + # return joint_angles + + + def cartesian_to_joints(self, pose, initial_joint_values=None): + """ + Converts the robot's Cartesian coordinates to its joint angles. + + Args: + position (list[float]): Cartesian position of the robot arm. + orientation (list[float]): Quaternion orientation of the robot arm. + + Returns: + list[float]: Joint angles corresponding to the given Cartesian coordinates. + + Raises: + ValueError: If the solution is invalid or out of bounds. + """ + try: + orientation = pose.orientation + rotation = R.from_quat(orientation) # qx, qy, qz, qw + rotation_matrix = rotation.as_matrix() + + target_pose = np.eye(4) + target_pose[:3, :3] = rotation_matrix + # Adjust position by subtracting the gripper length along the end-effector Z-axis + gripper_length = 0.17 # Gripper length in meters (15cm open, 17cm closed) + target_pose[:3, 3] = pose.position - gripper_length * rotation_matrix[:, 2] + + if initial_joint_values is None: + initial_joint_values = [0, 0] + self.get_current_joint_values() # 不同的urdf，[0]位置不一样 + + joint_values = ikpy.inverse_kinematics.inverse_kinematic_optimization( + chain=self.robot_chain, + target_frame=target_pose, + starting_nodes_angles=initial_joint_values, + orientation_mode="all", + ) + self.log.info(f"Converted Cartesian position {pose.position} to joint values {joint_values}.") + return joint_values[2:9] + except Exception as e: + self.log.error(f"Error converting Cartesian to joint values: {str(e)}") + raise + + + def calculate_IK_error(self, goal_position, goal_orientation): + """ + Calculate the inverse kinematics (IK) error for performing pick-and-place manipulation of an object using a robot arm. + + Args: + goal_position: The desired goal position for the target object. + goal_orientation: The desired goal orientation for the target object. + """ + pass + + # ---------------------------------------------------------------- + # Functions for gripper + # ---------------------------------------------------------------- + + def find_gripper_xarm(self): + """ + Searches for the gripper on available serial ports and returns the port if found. + + Returns: + str: The serial port where the gripper is connected, or None if not found. + """ + _pos = self.robot.get_gripper_position() + _ver = self.robot.get_gripper_version() + + if _ver is not None and _pos is not None: + print("Have Xarm gripper", _ver) + return True + else: + print("Not found Xarm gripper") + return None + + def find_gripper_robotiq(self): + """ + Config the parameter via Python SDK + """ + # Baud rate + # Modify the baud rate to 115200, the default is 2000000. + self.robot.set_tgpio_modbus_baudrate(115200) + + # TCP Payload and offset + # Robotiq 2F/85 Gripper + self.robot.set_tcp_load(0.925, [0, 0, 58]) + self.robot.set_tcp_offset([0, 0, 174, 0, 0, 0]) + self.robot.save_conf() + + # Self-Collision Prevention Model + # Robotiq 2F/85 Gripper + self.robot.set_collision_tool_model(4) + + self.robot.robotiq_reset() + self.robot.robotiq_set_activate() # enable the robotiq gripper + + def get_gripper_position_xarm(self): + """ + Get the position of the XArm gripper. + """ + gripper_pos = self.robot.get_gripper_position() + + return gripper_pos[1] + + def get_gripper_position_robotiq(self, number_of_registers=3): + """ + Reading the status of robotiq gripper + + :param number_of_registers: number of registers, 1/2/3, default is 3 + number_of_registers=1: reading the content of register 0x07D0 + number_of_registers=2: reading the content of register 0x07D0/0x07D1 + number_of_registers=3: reading the content of register 0x07D0/0x07D1/0x07D2 + + Note: + register 0x07D0: Register GRIPPER STATUS + register 0x07D1: Register FAULT STATUS and register POSITION REQUEST ECHO + register 0x07D2: Register POSITION and register CURRENT + :return: tuple((code, robotiq_response)) + code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details. + robotiq_response: See the robotiq documentation + """ + status = self.robot.robotiq_get_status(number_of_registers=number_of_registers) + gripper_width = status[1][-2] + return gripper_width + + def gripper_goto_xarm(self, value, speed=5000, force=None): + """ + Moves the gripper to a specified position with given speed. + + Args: + value (int): Position of the gripper. Integer between 0 and 800. + 0 represents the open position, and 255 represents the closed position. + speed (int): Speed of the gripper movement, between 0 and 8000. + force (int): Not applicable for xarm gripper + + Note: + - 0 means fully closed. + - 800 means fully open. + """ + self.robot.set_gripper_position( + pos=value, speed=speed, wait=False, timeout=1, auto_enable=True + ) + + def gripper_goto_robotiq( + self, pos, speed=0xFF, force=0xFF, wait=False, timeout=5, **kwargs + ): + """ + Go to the position with determined speed and force. + + :param pos: position of the gripper. Integer between 0 and 255. 0 being the open position and 255 being the close position. + :param speed: gripper speed between 0 and 255 + :param force: gripper force between 0 and 255 + :param wait: whether to wait for the robotion motion complete, default is True + :param timeout: maximum waiting time(unit: second), default is 5, only available if wait=True + + :return: tuple((code, robotiq_response)) + code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details. + robotiq_response: See the robotiq documentation + """ + return self.robot.robotiq_set_position( + pos, speed=speed, force=force, wait=wait, timeout=timeout, **kwargs + ) + + def open_gripper_xarm(self): + """Opens the gripper to its maximum position with maximum speed and force.""" + self.gripper_goto(value=850, speed=5000, force=None) + + def open_gripper_robotiq( + self, speed=0xFF, force=0xFF, wait=False, timeout=5, **kwargs + ): + """ + Open the robotiq gripper + + :param speed: gripper speed between 0 and 255 + :param force: gripper force between 0 and 255 + :param wait: whether to wait for the robotiq motion to complete, default is True + :param timeout: maximum waiting time(unit: second), default is 5, only available if wait=True + + :return: tuple((code, robotiq_response)) + code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details. + robotiq_response: See the robotiq documentation + """ + return self.robot.robotiq_open( + speed=speed, force=force, wait=wait, timeout=timeout, **kwargs + ) + + def close_gripper_xarm(self): + """Closes the gripper to its minimum position with maximum speed and force.""" + self.gripper_goto(value=0, speed=5000, force=None) + + def close_gripper_robotiq( + self, speed=0xFF, force=0xFF, wait=False, timeout=5, **kwargs + ): + """ + Close the robotiq gripper + + :param speed: gripper speed between 0 and 255 + :param force: gripper force between 0 and 255 + :param wait: whether to wait for the robotiq motion to complete, default is True + :param timeout: maximum waiting time(unit: second), default is 3, only available if wait=True + + :return: tuple((code, robotiq_response)) + code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details. + robotiq_response: See the robotiq documentation + """ + return self.robot.robotiq_close( + speed=speed, force=force, wait=wait, timeout=timeout, **kwargs + ) diff --git a/Robotics_API/Utils.py b/Robotics_API/Utils.py index c4036c1..8bd4bac 100644 --- a/Robotics_API/Utils.py +++ b/Robotics_API/Utils.py @@ -7,168 +7,4 @@ # @Email : yding25@binghamton.edu # @Description : Utils # @Usage : None -""" - -from scipy.spatial.transform import Rotation as R -import xml.etree.ElementTree as ET - -def quat2eulerZYX(quat, degree=False): - """ - Convert a quaternion to Euler angles (ZYX order). - - Parameters - ---------- - quat : list of float - Quaternion in [w, x, y, z] order. - degree : bool - If True, return angles in degrees; otherwise, in radians. - - Returns - ------- - list of float - Euler angles in [x, y, z] order. - """ - # Note: scipy's `R.from_quat` expects [x, y, z, w] order - euler_angles = R.from_quat([quat[1], quat[2], quat[3], quat[0]]).as_euler( - "xyz", degrees=degree - ) - return euler_angles.tolist() - - -def print_description(): - """ - Print the description of the demonstration. - """ - print("This script demonstrates teaching by demonstration for a robotic arm.") - print("Free-drive the robot to record Cartesian poses and reproduce them.") - print() - - -def list2str(lst): - """ - Convert a list to a space-separated string. - - Parameters - ---------- - lst : list - List of elements to convert. - - Returns - ------- - str - Space-separated string representation of the list. - """ - return " ".join(map(str, lst)) + " " - - -def parse_primitive_state(states, target_state): - """ - Extract the value of a specific primitive state from a list of states. - - Parameters - ---------- - states : list of str - List of primitive state strings. - target_state : str - The state name to extract. - - Returns - ------- - str - Value of the target state, or an empty string if not found. - """ - for state in states: - words = state.split() - if words[0] == target_state: - return words[-1] - return "" - - -def parse_pt_states(pt_states, parse_target): - """ - Parse the value of a specified primitive state from the pt_states string list. - - Parameters - ---------- - pt_states : str list - Primitive states string list returned from Robot::getPrimitiveStates(). - parse_target : str - Name of the primitive state to parse for. - - Returns - ---------- - str - Value of the specified primitive state in string format. Empty string is - returned if parse_target does not exist. - """ - for state in pt_states: - # Split the state sentence into words - words = state.split() - - if words[0] == parse_target: - return words[-1] - - return "" - - -def load_poses_from_xml(filename="saved_poses.xml"): - """ - Load poses from an XML file. - - Args: - filename (str): Name of the XML file to load the poses. - - Returns: - list: List of poses with positions and orientations. - """ - tree = ET.parse(filename) - root = tree.getroot() - - poses = [] - for pose in root.findall("Pose"): - position = pose.find("Position") - orientation = pose.find("Orientation") - pose_data = [ - float(position.get("x")), - float(position.get("y")), - float(position.get("z")), - float(orientation.get("qx")), - float(orientation.get("qy")), - float(orientation.get("qz")), - float(orientation.get("qw")), - ] - poses.append(pose_data) - - return poses - - -def pose_to_euler(pose): - """ - Convert robot pose from a list [x, y, z, qw, qx, qy, qz] to [x, y, z] and Euler angles. - - Parameters: - pose: list of 7 floats - [x, y, z, qw, qx, qy, qz] - - Returns: - tuple: (x, y, z, roll, pitch, yaw) where (x, y, z) is the position and (roll, pitch, yaw) are the Euler angles in radians. - """ - x, y, z, qw, qx, qy, qz = pose - r = R.from_quat([qx, qy, qz, qw]) # Reordering to match scipy's [qx, qy, qz, qw] - roll, pitch, yaw = r.as_euler("xyz", degrees=False) - return [x, y, z, roll, pitch, yaw] - - -def euler_to_pose(position_euler): - """ - Convert robot pose from [x, y, z, roll, pitch, yaw] to [x, y, z, qw, qx, qy, qz]. - - Parameters: - position_euler: list of 6 floats - [x, y, z, roll, pitch, yaw] - - Returns: - list: [x, y, z, qw, qx, qy, qz] - """ - x, y, z, roll, pitch, yaw = position_euler - r = R.from_euler("xyz", [roll, pitch, yaw], degrees=False) - qx, qy, qz, qw = r.as_quat() # Getting [qx, qy, qz, qw] from scipy - return [x, y, z, qw, qx, qy, qz] # Reordering to match [qw, qx, qy, qz] +""" \ No newline at end of file diff --git a/Robotics_API/__init__.py b/Robotics_API/__init__.py index 0ddfe4e..042dd72 100644 --- a/Robotics_API/__init__.py +++ b/Robotics_API/__init__.py @@ -1,2 +1,2 @@ -from .Bestman_Real_Xarm import Bestman_Real_Xarm +from .Bestman_Real_Xarm6 import Bestman_Real_Xarm6 from .Pose import Pose \ No newline at end of file diff --git a/Robotics_API/__pycache__/Bestman_Real_Xarm6.cpython-38.pyc b/Robotics_API/__pycache__/Bestman_Real_Xarm6.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..4078589e256add3c1e3ebfcbd24318c18bbcef81 GIT binary patch literal 23465 zcmeHvYm6P&ec#O7xzD}3++8l8qNI@}QF|rsE=5_7Z1o^TawS^wiYAv9J(hcQ_nz6i zcX?l$nI*Y{T{SV~CJ4&JLE5}*CA)Ewf-R5~2oTssQKv2%^n;Oat&90kz$ogPE!qMB z`VwrjzyJTt^X~8wryq(oOWt#5&OPVMdHm1s|2%VPbhM)2?;m}0x%I_OMfrDhll;r! z<|IDOFCr0&sR&hQ9o1C%TQfEO&Y3y>*3G=g3B6P37R-W5W%He4w`7+2TI*EI3i1k_ zk#5zja$d1B+8r~;_`1~D(;YX*gR&Fm1h+Y9PQIatvZ#Edh{_Gk+$*$;N^QjZFO|oQ zlMC&Rwb*xE9h40dZzT2P%aGgKB+Gx{7>C%NW3vai3S8!dte9G>gI)AQ7 z)sufZZcgIk97dCtf(2D*F|m+u&=P*6n!3n~!bb|0v?xmWF33eNi(*7nabFUnVhrD9 zu}6&KyCNpUB)&()UNMF5s@Ny?<9k#b5C`!+CJu?i_}(KP5=ZboF5VOmi%0OJ3Gt{f zkTWUHiO0ng$k{8N6i1OWC7u$~_}(XK;%R*E7oQQ&h+`;mK+K3)2@^ZsN=Qx}M)E)z{PzK=<((4w6(nw^H@7;fKa_gu?fX*4b4TH9SU z+*Qk<$$!DH`^$Z|-84{hwjyh}uH9Zy2y`>>vj6A zc?=3)Ez1rckHI0s=;8^WOo!_VYk9rZ0#O@{o-n$7^ssN^Hg1Yrt~;Pl43j1beE@-? zS$ei3i*p~E>$Yw6T;!pw(O=2xShp{%dE+8#p##+Muu^|u*}MhN31P*LZ((cUCprG$ zCL9oTAefR44T0LTFbr#D1=HQPWh;wK+9cs$5f}71jgRvqB(Acl-cUD{I@{vN-|mQ-YK|^99II}1tS)tPVd-`O4K}IO@K3`<9-oW&II~DvnC&ke z-BO7fT=j-#>znGDCX_3hbCj>tE1dT$+6Ox3O*2cjb+zBY-Z#qwyX|yAyUkk8ue6h16U_#+whLf8mCvS<`v#_@p=%7 z$Vzo=$tzC1+iM+hXR0xa0dV*w9xuq!FGm>a zmq287LLFc0JH9?>xT}6qv~B91Uu-NpoKvdTiQ?*YU#IEt3)Xc^nd48e@X~h|hzifz zcHg#%vdpm+yWg!7y>dN&gu7C25_k6}cFhsSNAu;62EWKD_j4Vq=T|=8=&W1ZwqGPn z2iTb-Ao6w-LtbgOeErCzgo%_w8BqHr7=}g$M5I`UDZ`hE2yQPXg@BLyoYx4=h?9BU3-^koDuwIapAoT zWK_0+--Oe+*0ysyN^qsqm7Cu#NE8oB=BVVmiojmh6}Nb$VtcBxg*?yT5+#v$%fuH5 z!DZPmD8H+V!bM>9minQ3S^0v}(=Ou9(M2~EQ0Ry#I@w%zLr)}w5c?c3Zn7irb zM3LCJD2Xz}00<7ELhXOA9H%pu47DSQl!j7_4fgqEE zKvCH)?kJ-Abxn+ZorIc_7~@te+}a3wQQoeIJ@_t##O{uW@y(GN8f}r@JT^%6O69An zeGpp&>YjI)>x_yCcWkqo+#=6obX(OAtI_7jvxqT>HphbHHdi6|5tmpW2ofooN!@w_Na>Osp%zOJOSe?N+;-}AL$n(`o0vVz z6pMBlm);XGwb~XWc(5W88)rhXk)M%Tb83NvNER1}lvrG-@>(koDN-o7rQZG_b_~-a z$g_%Ke*P*U_YjHNd+^gQ;Nwu61w~W5`3x$Q6C`OkkiWC|wf!_2F>}{9yqe~hFg>9D ze$O9=7!T=(Md~_D7FcMNXS6P7)E}33;KThU>R66ha+`xXP4I&aUO-mb9}K>xdis;I zCstCUSKC=5|IO6M_g^3hN9qhtKok(>ynSI`rsI{a&3g|s7nJ1)y^<6&Z`AfMviRd* zc6B*CNlbo~a>B{6A4eB_^@3Sp9&N2dVgRFec%J-%+b3bjta97I%rkh|&rrQQzFya09-mhz5y_#D1iT>kK zg?=5=CROj~gHd6MI!Mea;ltecBV3TRN90<$Emg4T0lJO8rIB2{sol`FasjBR3_tp2 z4!{X*2f&$k!M90!0>mk6`WnGu^^k&el+!V!1 zKY#Vxt;+YlF?gl&z1Lc=FaE{9`gr>XueGl3eDBRmFP-^z>r|L?P3F9oTBds<*i^8I z?dP$6=^Z>6qBX{@KSD8yT`I6^HiK$f{W?u)7G8mi(`W(FIw?gA%nZ@DsgJdSuU_}n z4PW(a(q`-hO5Q-?9g#@U#d5NwK@W&lWwBPWNnG*w&_+*8udnq6uyt82+p-*q4pox( z!{v-OcCE-;nD->_K&S%1LUSZ6TJLtv3GRII0p`J7S#|lip{etW!K`efpUw#+N}+;H z$p3z%Qtx|oOHzS(aL4^2*KV}l#`Gc@VY zVk)ZlXhwf|`h&75BBf4~(#)MEd}S;lJi(_`0HVO88GdjNRB?GoH(+JKfbw3v>mful zXO{wo6eX}v+t|;6Wr{XwVAV_5^bGuE3gE;O*rtYWZeo1^x#XB}4Z=0#hei{&Ldpsa zWb(mXKumYN%Mwo)24=cunSil^YjNKayZ+Iy{HM=-^O5qOw&cbC`gU+pKH2)_BlMec z5}aPA{`fhsJqXvUEEyi2Xoi=K>%$9`;EcBli`=G-g=izo0k0u?rnoIbuuj^kVoz~`(^%( z+8$oEy)1Lo(QPT#oWs!m4N5yCWh%uIKxFJrHFr3hhi6nyEg@OPQeZ**%7vYB4OT_^ zM_}Rob~GF5z18lCHuTSMikS+DwZ{72gG~4E_6l3Y5^_U#ZI%|0{%X2?8RLM-2lB0D ze+t+3V@PhP64r-grqP(F+v55UP*6fv(Tki<2{T)&aBA)d=c2hgj2l5JptMIo?F&_^ zO`5};_se&gL&-2`3SpMA1YAzjXWm(;g>#xTlO|i(Z6?58khWG(-7;hfv>5QdMB_2o zNQG!ba#lPMbgL!kQYzVSCiSIYFUgYaK1G@m)cCJ7a}qyv6*6QdgOWv4^F~-7snKh= z@s850ks5snu)%Yf+tx*D4&Mdaf;yUqy+%M81tVj7O%8yObBmBKqxpUp_8Hh~VBZmt zK0kz|Ca~8a2Ylko$yrSB3EG$8q?5Key#%w1aTrLRFNMpNBIv)UY7hg{%`{iTxhtE?zRfa;_(=>#QXuw3C3Pk1~%( zH|h*PS-J!(N@1~2XmM$|VLDOAu)GD8jV4)Ko#-K~ms)As9nnhvEN;B{aHWNISAz*# z@dVa1+Cf^d`g&lk%VG6Dg_blJKj4?y1XhJ*TM#3d0^nnYNh0@QE?R#wNtC@mzRUWD zh6qNR(Dabx9S;g+UtEFc2wxHgd$ zTS_vi=|~10L@iO|LpDWaxxVhzQi`fbrj20hpWw#3o`fwa$C6za7Tc#?jU-lrAF|tS zLgFP$??Vc%3eifaR3|dF8FbhlL38kW;IA-?|S)lCHap3Lz?|9qP$Oc1s`U(FQ>t zeTPnZ+Zw>v4-QJ^x?vAJ+7GpBma7wrd(0`9B=C`x>Yd0Sv1AX~hI7ndM*%c33O2CG zDJ5a^vvl<|l11;KkdAlN1|c7Pkf%UaDofyem70B=5|V1}4xxd8;S37Ff84mnwpM8a z97&AaUZPgAnDbPUdM#^(8+eB3pTu2a(q!Y))Dlbw?C1Ego+pn-gt1TE2V=uFn=H{$ zoA@L1UbGYb95>!qX(vdBo6tc2PcR$KAq)aEE8S{b0^-{av^J$LMr8k4p71~qqY!tAwbgJ2#P0j74ksC|(Mjc>fYRq!@T8U`!OF*dg$w zgkr(F3Op40R`J7Pv^NN?$Gn}0tcQ$+GTetubPdCSSG>J~9M~^tqa^ul033sg(ZpD4*qTGOF zn)l0OYvKX=d-&3_ulA(_Bq8Lm0ly>}QH!coTwazsc&8a&H=0ee2onxx@&$7I0Q=J2VLvpw}0m*BA8tT&jA<%aW1q z_LLMiBQ=!(l=v(g4QPL^v9}UCK=r;6?SQm$ABb=UBzxl?l0<_3AIx}`kcIEtv=RIT z)=F~{K@J^q5wpI*g=_R_4M;U1>xnmgA6$&8-4>{E`xg)^ zu`8;KOffvY2!mC;Kc@-)t3sp;cytRlH0QEg6kzobE!6GOj zFtSlR8LleP)ktuKC^q;{AY5>YTyhVKDg}SutOT(VBX9$&YgIt*IKD?i&<&ixW9$Sj z1wsfpfeB(K+&yU_gdsL5eh|b?djByZFl>GXiXquTf~?$lkx-DJtKGQT>LZYo#g=Ij zID#NEVL5FZ9s>R$P~B2I9{!tVXC1{1I38D{M-l9HrJ;70LBDp)fMpj!WyU%_5}et9 z8*ps|P4;4}zD=T|^lNl*wP<&dda^^^by$y=G5#K&2Vg>+81WM7zx^4i_7){+iDhX4 z!4r#={|+VRDM|ZAY@&NJ-@OV*VwAQ&KnCM1W+F9upfk-3P+2=>cu&W?gkWS>QJUhQ zd~I9`J86WqZR!n42h#mgUV=|YS|3;{`W*d2ZgUB@X7t0>+iKTZjotK9fCjO{PAzY>_canC(Q^H1Rc8+EYH0U5yNu}kKpeHWHi8EtTk z_Jqt#$gxq62*2xMUB>s$8!PKQgaX1SkI+M`atl$({eeM%9@SXvr-G`ffmPQXIP-IJ zEsm0;u>3hPi_V;(NJ0`_A@#JcP^}g2vO`KxQ`;NX2P+}JUep* z5lk&78pv&m1&n>1cBFyaCf;j!tjInS%gYVPC1)58f`E0&|P~O`hF&(en;zM zmZK>j=7>@-2{s6^ws4XEfcD5iJW{DvIYcVqfBZx%Y5(Ky0!T({sck?C1$~%9l7h3T zY%AboP=2>n=4iT5^9o2@dr`?LFDpVS#KdFbxIT)sRwcLM_yELMQ$W>C9a}+0;{AhY z>iJ9sgyNx~@Lf7C1?M$F=^k*&vMyz^6noFzA&XA4t#63^*uja8pt-xt^dy-vfh;Z9 zJzOu=3ic|#eZAQqYz&ct?cf&_AYsSU;ZJnzK3z@)a#~8vH1`H#Y*GQSU6dath%xRh zLZ%5bn}a`%$e={9IF3L~kOR4=5vzoMcf7|k-C9K48R zM>z!2`QS4sm)|bI*G<7B@FtSgPvlpCtXswCo^v^`@(=S7BX3=T%!_ky>I@*pD z5ac=(H~aZ0bb8)6Cj)0=*O(kjpqYz4L_#`Gfbep1<^wn4lBq!WSk&_tmQuxj+6H&& zYQ#36WZ+yE5{@>U+)_a~p#N>S#CB8}bI2PROdxUx`FS`_t?Tv(s@d;T!hTQMfE>DG zU|YtaJ6U0eJOpBKF;mP6<5^@o+JE@x|84rcU;fU&c#RpGcOalnplE6)yuCz~@v;&X z7+#)N)mLY|W1)F~Bd$m!NL5ePh1G2`cJ4~6+Xaly%zKvIcjB3_zeu^0pippRQNIFl zk`HM(_G|RR2#5Y5$8KM@Pa-E_wj-Dtc7Ke;<^gkpLaozKJCzLEM?^fk#Mz;5j__Ed z{l^@SC5_tTPS?)9AeM8B#W+^yGkr;b@_Qb7yo6m!^QwxrL)iy#%TxW+^cv z56Bc8I?P2%2@X`WdteD|fhbL*kH}99L&iPDxNm<5;*}6+-@v7m>#?GWpMz3UgUOT+ z`XJm_S`t@m1Ou4+pz+(}2&wmAuXTb*Y%>+BO_p5m;TT9W*2$ZoDUcKpFfaQfJo`3> zx9{L_-Gv#ps%|HY}-jOWCx2Ww2pRVxkcCY9+wc zd}LQ%y^qry-L}V}=n|ig(v)R6nJW@yNk1^Bg6P~7&~k6#JPD+0b5D2`^wu8Fyb=l-;WA~ zBZ!FLXo#$^>5QgRNJuP($c2%RR6#zDsWqNYp+ck6gv|qoX^Gr}i8W$QW=x-Zs}|VG zNfLt+j4uQLwv0i$d1VHXa5IAr9c9K@y7qvY3S!}4ehSVcuj9NTEb-w%&s1F8ZZHY0 zDJ+!9K%>y!7MhBrycP0I-Pv6uCB)GJ(}x%MbzDfZ4_KRw4%2Xjv&KTb zQnpW!S%f#UdGEUsfL?@gjL*`9Gaa3c?ir1%IF5yrWfojj zHjYBUf=6-e6c~vpBXZj^K~XLpCgbz7NyN;;W{zkgVK>oa5c|qlERSgudLo3N$LB|n z5r9${B|RiMs2dZ(qBrqo14c(W=xPMrU-I_D_9MHL`XR82`0N4=rJ;~>nS05`ALbG! zfzCPpGBA+%ItfOm4oinUy!I0-5Z9v0bo%sHY3WH|Qi?DK!dM7l2;1L#B7+QeRZdX> zZSlzXw-Ql7WYJ*bA0nEu(sXbuFbL9p4#Eb1w+ey9L)q4YQ;iBlqd2z^>$ra zlmb?QBcgJ}8^nevmhEc0VMtS%0aIC2wyk_eRa(FGoAmdeUt_bxZ92qq`wx*wrib7& zGEbR@$ZqDg2JJqbon2mc>dTFF!GW%yr{Bl;cNmuf5w+}Ktzk6lt(XVdf1!lmrFz;SEhQkl4^{m4#HNc`rs&`38Kmv@=2)4ed^sVYCdXJ>Cl(pAa~> z>|a9bfk(?638@;@4wfjKiTG6l^8+H%K=lUk zfD~FVJz!7IC-m_s^O*7M+=;p8jv>sIPq7jLJr_Nj_>5HIQ!TynEW$z(-2zp*h)|&= z3L7?#UqAlhndA3u>BNJ!G{+4-d*241`%5+`+dUCaAdQ*q=kYs9D2M)r#x{?@cR97j z8yC)3KJKoF$%`s3oVxV(Qcx{mFhP^&-??~h>D)Vu#)Y$=yL9&A zl5zI*oA2DGWlUz&a#UZkt)SJ@mo8j5ySNm!GcOt1ygYj`Jxvh?@J8@k6BtFHGr%=i ziz?7?WEjFlOUhMWq&KvjMm^(9zlm7=;Lzlkre`B>0bmR7B!SBSo`V^>uCX{GEW8P& zlXzVV&(+Yg@wSuTK_SV*7ZE=u>KNYgvp0iO85nN=1hK-g`#X37ZDF~g!YI1Ght261 zWS%)1BFDA1a94-1gG1`V5)pO;UHoSRA7rgpCdQ!eXTC!s6F`FcDc?B32YEypILVw9 z!Ust|q^b-3W*&TtB6ACkf1eb9$yF?6m7u4j_cRi=`mp2jbvjZZWQR&ZoY^hKvd%=o zk1mY%=R*@?psfZ^nl;V=6X@+Z%a(i1f}(kx>HDSQ$79r{^-lgB#}*5s-DJ1nm+D0v z;21Y>;`!%?OEo)CR@h_B!gIQP9!}WVixhhka*}XP$OlPei}!O3cXg<*kQJc}1Oq2I zt1%dK+D#S}Nsc7pXE!4629p^~Mi*!IapI9*Iv@v-?)Qv=sY;ci$%xA=Fl8^*@_r7h z$R2F=TKh%v1VZmNM;hzU%lI7$R(Mp+oFY+Rm~KP*e-YHhE%|vK>M+qygcVzTv`nM@ z*F@WxM24RUZ~iF&&#-@b$AE!bN~Qvq{C?ermEzLR8_D+moGCk2dF>euzn!mn$; zMCU@Z#y>?jA|!YN<_&4#NRv0<#v<(w6d;L$kdeokp!R@$qUL-=ctZqpH_`@W&SGJg zEw-_MTc@ZuBHs9SUiRoI@zMr;;7-nm^c3_tO`>>YH0+=j4=Aq1w?l-TQH!F{=1Cuc z$b|z#eSq)7&oums~y7jN()$1`4*)4^<}qy~Do#&Bl>QiRVtIJ-y9^7Md4r`PeO ziFqhQ5FZ3WL=qCn^U!^IcpFEoi9rRTy07q4;>Y|y!@h3matnVAG|&6k{w-XEJS!#0 zhg2ONxTO~YWL5SH(wRQIi>Fc>FVgk0gKy$By7ea_$vA%TGAQD3*^< z?6D+@(WI~f6f97P1F@j`e<@6S5TXQ751 zkOc+y@1W8y97s`bKxU+=%F`fw5D9-j49IHXr^0|YPm>In4FbUt@CO3{87ssfVDHeN zf0vT?GloCO>^HUD_tM~l?Vf3~q)w)4{FV0mr;_`fC*ZF)Jjb!!Lo#^o|2xSbwc#TJ z9}}$is2<7!N-1rD(Y~Au5y>MX2vhQESLIKP^gU8(JOe- R691zpO2ifOKYIDn{{~gxA*}!a literal 0 HcmV?d00001 diff --git a/Robotics_API/__pycache__/__init__.cpython-38.pyc b/Robotics_API/__pycache__/__init__.cpython-38.pyc index 87cf5317a428e352f8b8992014ab08b039a71aff..fc6b4619d3e868332d58b749e2d7256fd8f00408 100644 GIT binary patch delta 44 ycmcb@c$JYil$V!_0SLsTlhQpW^12HPIi(ht Date: Thu, 19 Dec 2024 00:31:00 +0800 Subject: [PATCH 05/24] update --- Examples/close_gripper_robotiq.py | 4 +- Examples/display_states.py | 2 +- Examples/move_arm_to_joint_values.py | 2 +- Examples/move_eef_to_pose.py | 2 +- Examples/test.py | 41 +- Examples/xarm_command_log.log | 1124 ----------------- Install/install.md | 49 + README.md | 29 +- Robotics_API/Bestman_Real_Xarm6.py | 325 ++--- .../Bestman_Real_Xarm6.cpython-38.pyc | Bin 23465 -> 23109 bytes {Robotics_API => TODO}/Utils.py | 0 11 files changed, 199 insertions(+), 1379 deletions(-) delete mode 100644 Examples/xarm_command_log.log create mode 100644 Install/install.md rename {Robotics_API => TODO}/Utils.py (100%) diff --git a/Examples/close_gripper_robotiq.py b/Examples/close_gripper_robotiq.py index eadebd4..5c8f9c3 100644 --- a/Examples/close_gripper_robotiq.py +++ b/Examples/close_gripper_robotiq.py @@ -1,11 +1,11 @@ # !/usr/bin/env python # -*- encoding: utf-8 -*- """ -# @FileName : open_gripper_robotiq.py +# @FileName : close_gripper_robotiq.py # @Time : 2024-12-01 15:21:45 # @Author : Zhaxi & Yan # @Email : zhaxizhuoma.ayang@gmail.com & yding25@binghamton.edu -# @Description : Open robotiq gripper +# @Description : Close robotiq gripper # @Usage : python close_gripper_robotiq.py 192.168.1.208 """ diff --git a/Examples/display_states.py b/Examples/display_states.py index 0bc6075..645db11 100644 --- a/Examples/display_states.py +++ b/Examples/display_states.py @@ -1,7 +1,7 @@ # !/usr/bin/env python # -*- encoding: utf-8 -*- """ -# @FileName : move_arm_to_home.py +# @FileName : display_states.py # @Time : 2024-12-01 15:21:45 # @Author : Zhaxi & Yan # @Email : zhaxizhuoma.ayang@gmail.com & yding25@binghamton.edu diff --git a/Examples/move_arm_to_joint_values.py b/Examples/move_arm_to_joint_values.py index 204b5b1..181d1b6 100644 --- a/Examples/move_arm_to_joint_values.py +++ b/Examples/move_arm_to_joint_values.py @@ -1,7 +1,7 @@ # !/usr/bin/env python # -*- encoding: utf-8 -*- """ -# @FileName : move_arm_to_home.py +# @FileName : move_arm_to_joint_values.py # @Time : 2024-12-01 15:21:45 # @Author : Zhaxi & Yan # @Email : zhaxizhuoma.ayang@gmail.com & yding25@binghamton.edu diff --git a/Examples/move_eef_to_pose.py b/Examples/move_eef_to_pose.py index 8d3d947..17085a7 100644 --- a/Examples/move_eef_to_pose.py +++ b/Examples/move_eef_to_pose.py @@ -1,7 +1,7 @@ # !/usr/bin/env python # -*- encoding: utf-8 -*- """ -# @FileName : move_arm_to_home.py +# @FileName : move_eef_to_pose.py # @Time : 2024-12-01 15:21:45 # @Author : Zhaxi & Yan # @Email : zhaxizhuoma.ayang@gmail.com & yding25@binghamton.edu diff --git a/Examples/test.py b/Examples/test.py index 237fc37..7f2c7c5 100644 --- a/Examples/test.py +++ b/Examples/test.py @@ -33,23 +33,35 @@ def main(): if not bestman.initialize_robot(): return # Exit if initialization fails - bestman.update_robot_states() + # bestman.update_robot_states() + + # bestman.go_home() + # bestman.get_tcp_link() + # bestman.get_current_eef_pose() + # Test gt_pose = bestman.get_current_eef_pose() gt_joint = bestman.get_current_joint_values() - print(f'gt position:{gt_pose.position}, gt orientation:{gt_pose.orientation}') - print(f'gt joint:{gt_joint}') - # joint_values = [-1.6, -27.2, -2.2, -1.3, -60.6, 0.2] - # joint_values_rad = [v / 180 * math.pi for v in joint_values] - # joint_values_rad = [-0.12792803756892681, -0.47472870349884033, -0.038403209298849106, -0.022691410034894943, -1.0576682090759277, 0.0034917236771434546] - # bestman.move_arm_to_joint_values(joint_values_rad, wait_for_finish=False) - # joint_values_rad = [-0.22792803756892681, -0.47472870349884033, -0.038403209298849106, -0.022691410034894943, -1.0576682090759277, 0.0034917236771434546] - # bestman.move_arm_to_joint_values(joint_values_rad, wait_for_finish=False) + # gt_joint[1] += 0.3 + # bestman.move_arm_to_joint_values(gt_joint) + + # gt_pose.position[2] += 0.3 + # bestman.move_eef_to_goal_pose(gt_pose) + + # print(f'gt position:{gt_pose.position}, gt orientation:{gt_pose.orientation}') + # print(f'gt joint:{gt_joint}') + # # joint_values = [-1.6, -27.2, -2.2, -1.3, -60.6, 0.2] + # # joint_values_rad = [v / 180 * math.pi for v in joint_values] + # # joint_values_rad = [-0.12792803756892681, -0.47472870349884033, -0.038403209298849106, -0.022691410034894943, -1.0576682090759277, 0.0034917236771434546] + # # bestman.move_arm_to_joint_values(joint_values_rad, wait_for_finish=False) + + # # joint_values_rad = [-0.22792803756892681, -0.47472870349884033, -0.038403209298849106, -0.022691410034894943, -1.0576682090759277, 0.0034917236771434546] + # # bestman.move_arm_to_joint_values(joint_values_rad, wait_for_finish=False) - # pose.position[2] += 0.1 - # print(f'test position:{pose.position}, test orientation:{pose.orientation}') - # bestman.move_eef_to_goal_pose(pose) + # # pose.position[2] += 0.1 + # # print(f'test position:{pose.position}, test orientation:{pose.orientation}') + # # bestman.move_eef_to_goal_pose(pose) trans_joint = bestman.cartesian_to_joints(gt_pose) print(f'trans_joint:{trans_joint}') @@ -58,6 +70,11 @@ def main(): trans_pose = bestman.joints_to_cartesian(gt_joint) print(f'trans_pose:{trans_pose}') bestman.move_eef_to_goal_pose(trans_pose) + + bestman.close_gripper_robotiq() + bestman.open_gripper_robotiq() + bestman.gripper_goto_robotiq(100) + # print(bestman.get_gripper_position_robotiq()) except Exception as e: # Log any exceptions that occur diff --git a/Examples/xarm_command_log.log b/Examples/xarm_command_log.log deleted file mode 100644 index 26ae865..0000000 --- a/Examples/xarm_command_log.log +++ /dev/null @@ -1,1124 +0,0 @@ -2024-12-18 10:21:28,018 - INFO - Checking for faults on the robot... -2024-12-18 10:21:31,021 - INFO - Successfully initialize the robot -2024-12-18 10:21:40,888 - INFO - Checking for faults on the robot... -2024-12-18 10:21:43,892 - INFO - Successfully initialize the robot -2024-12-18 10:57:45,836 - INFO - Checking for faults on the robot... -2024-12-18 10:57:48,840 - INFO - Successfully initialize the robot -2024-12-18 10:58:51,895 - INFO - Checking for faults on the robot... -2024-12-18 10:58:54,898 - INFO - Successfully initialize the robot -2024-12-18 10:59:08,786 - INFO - Checking for faults on the robot... -2024-12-18 10:59:11,790 - INFO - Successfully initialize the robot -2024-12-18 10:59:11,793 - INFO - Robot states updated. -2024-12-18 10:59:41,411 - INFO - Checking for faults on the robot... -2024-12-18 10:59:44,415 - INFO - Successfully initialize the robot -2024-12-18 10:59:44,416 - INFO - Robot states updated: {'joint_angles': [-0.027926120907068253, -0.4747267961502075, -0.03839937224984169, -0.022685658186674118, -1.0576759576797485, 0.0034840537700802088], 'joint_velocities': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 'tcp_pose': [420.434235, -5.81615, 359.876038, 3.140672, -1.570796, 0.0]} -2024-12-18 11:11:04,631 - INFO - Checking for faults on the robot... -2024-12-18 11:11:07,635 - INFO - Successfully initialize the robot -2024-12-18 11:11:07,638 - INFO - Robot states updated: {'joint_angles': [-0.027922285720705986, -0.47472870349884033, -0.0384012907743454, -0.022689493373036385, -1.0576720237731934, 0.003482136409729719], 'joint_velocities': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 'tcp_pose': [420.434235, -5.81615, 359.876038, 3.140672, -1.570796, 0.0]} -2024-12-18 11:11:15,198 - INFO - Checking for faults on the robot... -2024-12-18 11:11:18,202 - INFO - Successfully initialize the robot -2024-12-18 11:12:21,981 - INFO - Checking for faults on the robot... -2024-12-18 11:12:24,984 - INFO - Successfully initialize the robot -2024-12-18 11:14:48,311 - INFO - Checking for faults on the robot... -2024-12-18 11:14:51,314 - INFO - Successfully initialize the robot -2024-12-18 11:17:17,238 - INFO - Checking for faults on the robot... -2024-12-18 11:17:20,241 - INFO - Successfully initialize the robot -2024-12-18 11:20:44,817 - INFO - Checking for faults on the robot... -2024-12-18 11:20:47,821 - INFO - Successfully initialize the robot -2024-12-18 11:21:40,372 - INFO - Checking for faults on the robot... -2024-12-18 11:21:43,375 - INFO - Successfully initialize the robot -2024-12-18 11:23:34,190 - INFO - Checking for faults on the robot... -2024-12-18 11:23:37,194 - INFO - Successfully initialize the robot -2024-12-18 11:23:37,196 - INFO - x:396.4, y:-5.5, z:360.0, roll:90.0, pitch:0.0, yaw:-90.0 -2024-12-18 11:28:44,131 - INFO - Checking for faults on the robot... -2024-12-18 11:28:47,135 - INFO - Successfully initialize the robot -2024-12-18 11:29:29,591 - INFO - Checking for faults on the robot... -2024-12-18 11:29:32,593 - INFO - Successfully initialize the robot -2024-12-18 11:29:32,595 - INFO - x:396.4, y:-5.5, z:360.0, roll:-180.0, pitch:-90.0, yaw:0.0 -2024-12-18 11:34:57,158 - INFO - Checking for faults on the robot... -2024-12-18 11:35:00,162 - INFO - Successfully initialize the robot -2024-12-18 11:35:00,166 - INFO - x:396.4, y:-5.5, z:360.0, roll:-180.0, pitch:-90.0, yaw:0.0 -2024-12-18 11:36:29,865 - INFO - Checking for faults on the robot... -2024-12-18 11:36:32,869 - INFO - Successfully initialize the robot -2024-12-18 11:36:32,873 - INFO - x:396.4, y:-5.5, z:360.0, roll:-180.0, pitch:-90.0, yaw:0.0 -2024-12-18 11:41:01,984 - INFO - Checking for faults on the robot... -2024-12-18 11:41:04,987 - INFO - Successfully initialize the robot -2024-12-18 11:41:04,991 - INFO - x:396.4, y:-5.5, z:360.0, roll:-180.0, pitch:-90.0, yaw:0.0 -2024-12-18 11:41:20,576 - INFO - Checking for faults on the robot... -2024-12-18 11:41:23,579 - INFO - Successfully initialize the robot -2024-12-18 11:41:23,583 - INFO - x:396.4, y:-5.5, z:360.0, roll:180.0, pitch:-90.0, yaw:0.0 -2024-12-18 11:42:39,940 - INFO - Checking for faults on the robot... -2024-12-18 11:42:42,943 - INFO - Successfully initialize the robot -2024-12-18 11:42:42,944 - INFO - x:396.4, y:-5.5, z:360.0, roll:90.0, pitch:0.0, yaw:-90.0 -2024-12-18 11:45:45,965 - INFO - Checking for faults on the robot... -2024-12-18 11:45:48,968 - INFO - Successfully initialize the robot -2024-12-18 11:45:48,969 - INFO - x:396.4, y:-5.5, z:360.0, roll:0.0, pitch:0.0, yaw:0.0 -2024-12-18 11:47:12,772 - INFO - Checking for faults on the robot... -2024-12-18 11:47:15,775 - INFO - Successfully initialize the robot -2024-12-18 11:47:15,779 - INFO - x:396.4, y:-5.5, z:360.0, roll:0.0, pitch:-90.0, yaw:0.0 -2024-12-18 11:49:38,744 - INFO - Checking for faults on the robot... -2024-12-18 11:49:41,747 - INFO - Successfully initialize the robot -2024-12-18 11:49:41,750 - INFO - x:396.4, y:-5.5, z:360.0, roll:0.0, pitch:0.0, yaw:0.0 -2024-12-18 11:49:59,651 - INFO - Checking for faults on the robot... -2024-12-18 11:50:02,654 - INFO - Successfully initialize the robot -2024-12-18 11:50:02,657 - INFO - x:396.4, y:-5.5, z:360.0, roll:180.0, pitch:0.0, yaw:0.0 -2024-12-18 11:50:22,326 - INFO - Checking for faults on the robot... -2024-12-18 11:50:25,328 - INFO - Successfully initialize the robot -2024-12-18 11:50:25,331 - INFO - x:396.4, y:-5.5, z:360.0, roll:0.0, pitch:0.0, yaw:180.0 -2024-12-18 11:51:16,922 - INFO - Checking for faults on the robot... -2024-12-18 11:51:19,924 - INFO - Successfully initialize the robot -2024-12-18 11:51:19,927 - INFO - x:396.4, y:-5.5, z:360.0, roll:-178.4289429205916, pitch:1.614731541901501, yaw:-1.5710570794084195 -2024-12-18 11:52:19,863 - INFO - Checking for faults on the robot... -2024-12-18 11:52:22,866 - INFO - Successfully initialize the robot -2024-12-18 11:52:22,869 - INFO - x:396.4, y:-5.5, z:360.0, roll:-178.42847802292002, pitch:1.5803633679586777, yaw:-1.5715219770799764 -2024-12-18 11:54:12,711 - INFO - Checking for faults on the robot... -2024-12-18 11:54:15,714 - INFO - Successfully initialize the robot -2024-12-18 11:54:15,717 - INFO - x:396.4, y:-5.5, z:360.0, roll:90.0, pitch:0.0, yaw:-90.0 -2024-12-18 11:57:56,821 - INFO - Checking for faults on the robot... -2024-12-18 11:57:59,824 - INFO - Successfully initialize the robot -2024-12-18 11:57:59,825 - INFO - x:396.4, y:-5.5, z:360.0, roll:90.0, pitch:0.0, yaw:-90.0 -2024-12-18 11:58:30,983 - INFO - Checking for faults on the robot... -2024-12-18 11:58:33,986 - INFO - Successfully initialize the robot -2024-12-18 11:58:33,988 - INFO - x:396.4, y:-5.5, z:360.0, roll:90.0, pitch:0.0, yaw:-90.0 -2024-12-18 11:59:04,562 - INFO - Checking for faults on the robot... -2024-12-18 11:59:07,565 - INFO - Successfully initialize the robot -2024-12-18 11:59:07,566 - INFO - x:396.4, y:-5.5, z:360.0, roll:179.9, pitch:-90.0, yaw:0.0 -2024-12-18 11:59:22,540 - INFO - Checking for faults on the robot... -2024-12-18 11:59:25,543 - INFO - Successfully initialize the robot -2024-12-18 11:59:25,544 - INFO - x:396.4, y:-5.5, z:360.0, roll:179.9, pitch:-90.0, yaw:0.0 -2024-12-18 12:00:29,627 - INFO - Checking for faults on the robot... -2024-12-18 12:00:32,630 - INFO - Successfully initialize the robot -2024-12-18 12:00:32,631 - INFO - x:420.0, y:-5.5, z:360.0, roll:179.9, pitch:-90.0, yaw:0.0 -2024-12-18 12:00:56,124 - INFO - Checking for faults on the robot... -2024-12-18 12:00:59,125 - INFO - Successfully initialize the robot -2024-12-18 12:00:59,126 - INFO - x:420.0, y:-5.5, z:360.0, roll:179.9, pitch:-90.0, yaw:0.0 -2024-12-18 12:00:59,127 - INFO - Robot moved to home position -2024-12-18 12:01:43,640 - INFO - Checking for faults on the robot... -2024-12-18 12:01:46,643 - INFO - Successfully initialize the robot -2024-12-18 12:01:46,644 - INFO - x:420.0, y:-5.5, z:360.0, roll:179.9, pitch:-90.0, yaw:0.0 -2024-12-18 12:02:33,026 - INFO - Checking for faults on the robot... -2024-12-18 12:02:36,030 - INFO - Successfully initialize the robot -2024-12-18 12:03:06,355 - INFO - Checking for faults on the robot... -2024-12-18 12:03:09,359 - INFO - Successfully initialize the robot -2024-12-18 12:03:09,360 - INFO - x:420.0, y:-5.5, z:360.0, roll:179.9, pitch:-90.0, yaw:0.0 -2024-12-18 12:03:16,307 - INFO - Checking for faults on the robot... -2024-12-18 12:03:19,310 - INFO - Successfully initialize the robot -2024-12-18 12:03:19,311 - INFO - x:420.0, y:-5.5, z:360.0, roll:179.9, pitch:-90.0, yaw:0.0 -2024-12-18 12:03:27,613 - INFO - Checking for faults on the robot... -2024-12-18 12:03:30,617 - INFO - Successfully initialize the robot -2024-12-18 12:03:30,618 - INFO - x:420.0, y:-5.5, z:360.0, roll:179.9, pitch:-90.0, yaw:0.0 -2024-12-18 12:03:39,292 - INFO - Checking for faults on the robot... -2024-12-18 12:03:42,296 - INFO - Successfully initialize the robot -2024-12-18 12:03:42,296 - INFO - x:420.0, y:-5.5, z:360.0, roll:179.9, pitch:-90.0, yaw:0.0 -2024-12-18 12:03:45,992 - INFO - Checking for faults on the robot... -2024-12-18 12:03:48,995 - INFO - Successfully initialize the robot -2024-12-18 12:03:48,996 - INFO - x:420.0, y:-5.5, z:360.0, roll:179.9, pitch:-90.0, yaw:0.0 -2024-12-18 12:03:48,996 - INFO - Robot moved to home position -2024-12-18 12:07:10,398 - INFO - Checking for faults on the robot... -2024-12-18 12:07:13,401 - INFO - Successfully initialize the robot -2024-12-18 12:07:13,401 - INFO - Link 1: world_joint -2024-12-18 12:07:13,401 - INFO - Link 2: joint1 -2024-12-18 12:07:13,401 - INFO - Link 3: joint2 -2024-12-18 12:07:13,401 - INFO - Link 4: joint3 -2024-12-18 12:07:13,401 - INFO - Link 5: joint4 -2024-12-18 12:07:13,401 - INFO - Link 6: joint5 -2024-12-18 12:07:13,401 - INFO - Link 7: joint6 -2024-12-18 12:08:58,020 - INFO - Checking for faults on the robot... -2024-12-18 12:09:01,023 - INFO - Successfully initialize the robot -2024-12-18 12:10:20,385 - INFO - Checking for faults on the robot... -2024-12-18 12:10:23,388 - INFO - Successfully initialize the robot -2024-12-18 12:10:23,389 - INFO - Robot states updated: {'joint_angles': [-0.022722089663147926, -0.47703543305397034, -0.036874979734420776, -0.01575014740228653, -1.0572023391723633, 7.669904152862728e-05], 'joint_velocities': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 'tcp_pose': [420.0, -5.500007, 360.0, 3.139847, -1.570796, 0.0]} -2024-12-18 12:10:23,389 - ERROR - Failed to retrieve DOF: 'dict' object has no attribute 'q' -2024-12-18 12:11:08,557 - INFO - Checking for faults on the robot... -2024-12-18 12:11:11,560 - INFO - Successfully initialize the robot -2024-12-18 12:11:11,561 - INFO - Robot states updated: {'joint_angles': [-0.02271825633943081, -0.47703543305397034, -0.036874979734420776, -0.015753982588648796, -1.057206153869629, 7.861651829443872e-05], 'joint_velocities': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 'tcp_pose': [420.0, -5.500007, 360.0, 3.139847, -1.570796, 0.0]} -2024-12-18 12:11:32,931 - INFO - Checking for faults on the robot... -2024-12-18 12:11:35,935 - INFO - Successfully initialize the robot -2024-12-18 12:12:01,559 - INFO - Checking for faults on the robot... -2024-12-18 12:12:04,562 - INFO - Successfully initialize the robot -2024-12-18 12:19:18,638 - INFO - Checking for faults on the robot... -2024-12-18 12:19:21,641 - INFO - Successfully initialize the robot -2024-12-18 12:31:09,340 - INFO - Checking for faults on the robot... -2024-12-18 12:31:12,344 - INFO - Successfully initialize the robot -2024-12-18 12:31:27,842 - INFO - Checking for faults on the robot... -2024-12-18 12:31:30,846 - INFO - Successfully initialize the robot -2024-12-18 12:31:57,790 - INFO - Checking for faults on the robot... -2024-12-18 12:32:00,793 - INFO - Successfully initialize the robot -2024-12-18 12:32:00,794 - INFO - Link 1: world_joint -2024-12-18 12:32:00,794 - INFO - Link 2: joint1 -2024-12-18 12:32:00,794 - INFO - Link 3: joint2 -2024-12-18 12:32:00,794 - INFO - Link 4: joint3 -2024-12-18 12:32:00,795 - INFO - Link 5: joint4 -2024-12-18 12:32:00,795 - INFO - Link 6: joint5 -2024-12-18 12:32:00,795 - INFO - Link 7: joint6 -2024-12-18 12:32:10,772 - INFO - Checking for faults on the robot... -2024-12-18 12:32:13,774 - INFO - Successfully initialize the robot -2024-12-18 12:32:26,923 - INFO - Checking for faults on the robot... -2024-12-18 12:32:29,926 - INFO - Successfully initialize the robot -2024-12-18 12:32:29,929 - INFO - Robot states updated: {'joint_angles': [-0.02792995609343052, -0.47473061084747314, -0.03839937224984169, -0.02269524522125721, -1.0576720237731934, 0.0034878887236118317], 'joint_velocities': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 'tcp_pose': [420.434235, -5.81615, 359.876038, 3.140672, -1.570796, 0.0]} -2024-12-18 12:32:29,930 - ERROR - Failed to retrieve DOF: 'dict' object has no attribute 'q' -2024-12-18 12:33:22,978 - INFO - Checking for faults on the robot... -2024-12-18 12:33:25,982 - INFO - Successfully initialize the robot -2024-12-18 12:33:25,982 - INFO - Robot DOF: 6 -2024-12-18 12:33:36,446 - INFO - Checking for faults on the robot... -2024-12-18 12:33:39,450 - INFO - Successfully initialize the robot -2024-12-18 12:33:39,450 - INFO - TCP link retrieved: joint5 -2024-12-18 12:35:03,307 - INFO - Checking for faults on the robot... -2024-12-18 12:35:06,311 - INFO - Successfully initialize the robot -2024-12-18 12:35:15,775 - INFO - Checking for faults on the robot... -2024-12-18 12:35:18,778 - INFO - Successfully initialize the robot -2024-12-18 12:35:18,779 - INFO - TCP link retrieved: joint6 -2024-12-18 12:36:45,902 - INFO - Checking for faults on the robot... -2024-12-18 12:36:48,906 - INFO - Successfully initialize the robot -2024-12-18 12:38:38,619 - INFO - Checking for faults on the robot... -2024-12-18 12:38:41,622 - INFO - Successfully initialize the robot -2024-12-18 12:38:41,624 - INFO - Current joint values: [-0.02792995609343052, -0.4747267961502075, -0.03839937224984169, -0.02269524522125721, -1.0576759576797485, 0.0034917236771434546] -2024-12-18 12:41:02,526 - INFO - Checking for faults on the robot... -2024-12-18 12:41:05,529 - INFO - Successfully initialize the robot -2024-12-18 12:41:05,530 - INFO - Current joint values: [-0.02792803756892681, -0.47472870349884033, -0.03839937224984169, -0.02269524522125721, -1.0576720237731934, 0.0034917236771434546] -2024-12-18 12:42:22,550 - INFO - Checking for faults on the robot... -2024-12-18 12:42:25,554 - INFO - Successfully initialize the robot -2024-12-18 12:42:25,555 - INFO - Current joint velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -2024-12-18 12:47:09,221 - INFO - Checking for faults on the robot... -2024-12-18 12:47:12,224 - INFO - Successfully initialize the robot -2024-12-18 12:47:12,225 - INFO - Current end effector pose: [0.420434235, -0.00581615, 0.359876038, 3.140672, -1.570796, 0.0] -2024-12-18 12:48:49,846 - INFO - Checking for faults on the robot... -2024-12-18 12:48:52,849 - INFO - Successfully initialize the robot -2024-12-18 12:48:52,850 - INFO - Current end effector pose: -2024-12-18 12:52:36,977 - INFO - Checking for faults on the robot... -2024-12-18 12:52:39,980 - INFO - Successfully initialize the robot -2024-12-18 12:54:48,198 - INFO - Checking for faults on the robot... -2024-12-18 12:54:51,202 - INFO - Successfully initialize the robot -2024-12-18 12:56:16,603 - INFO - Checking for faults on the robot... -2024-12-18 12:56:19,607 - INFO - Successfully initialize the robot -2024-12-18 12:56:40,999 - INFO - Checking for faults on the robot... -2024-12-18 12:56:44,003 - INFO - Successfully initialize the robot -2024-12-18 12:56:44,005 - INFO - Current joint values: [-0.02792803756892681, -0.47472870349884033, -0.038403209298849106, -0.022691410034894943, -1.0576682090759277, 0.0034917236771434546] -2024-12-18 12:57:48,238 - INFO - Checking for faults on the robot... -2024-12-18 12:57:51,242 - INFO - Successfully initialize the robot -2024-12-18 12:57:51,243 - INFO - Current joint values: [-0.02792803756892681, -0.47472870349884033, -0.03839937224984169, -0.022689493373036385, -1.0576720237731934, 0.0034878887236118317] -2024-12-18 12:58:02,951 - INFO - Checking for faults on the robot... -2024-12-18 12:58:05,955 - INFO - Successfully initialize the robot -2024-12-18 12:58:05,956 - INFO - Current joint values: [-0.027931872755289078, -0.47472870349884033, -0.03840704262256622, -0.02269524522125721, -1.0576682090759277, 0.0034917236771434546] -2024-12-18 12:58:35,739 - INFO - Checking for faults on the robot... -2024-12-18 12:58:38,740 - INFO - Successfully initialize the robot -2024-12-18 12:58:38,741 - INFO - Current joint values: [-0.1279320865869522, -0.4747248589992523, -0.03840704262256622, -0.02269524522125721, -1.057666301727295, 0.0034878887236118317] -2024-12-18 13:00:44,350 - INFO - Checking for faults on the robot... -2024-12-18 13:00:47,354 - INFO - Successfully initialize the robot -2024-12-18 13:00:47,355 - INFO - Current joint values: [-0.02792995609343052, -0.47472870349884033, -0.03840896114706993, -0.022691410034894943, -1.057666301727295, 0.0034917236771434546] -2024-12-18 13:02:31,278 - INFO - Checking for faults on the robot... -2024-12-18 13:02:34,281 - INFO - Successfully initialize the robot -2024-12-18 13:02:34,282 - INFO - Current joint values: [-0.02792995609343052, -0.4747248589992523, -0.03840704262256622, -0.022691410034894943, -1.057666301727295, 0.0034917236771434546] -2024-12-18 13:02:34,297 - INFO - Moving arm to joint values: [-0.02792803756892681, -0.47472870349884033, -0.038403209298849106, -0.022691410034894943, -1.0576682090759277, 0.0034917236771434546] -2024-12-18 13:02:34,297 - INFO - Motion complete. -2024-12-18 13:03:24,602 - INFO - Checking for faults on the robot... -2024-12-18 13:03:27,606 - INFO - Successfully initialize the robot -2024-12-18 13:03:27,607 - INFO - Current joint values: [-0.027926120907068253, -0.4747248589992523, -0.03840896114706993, -0.022691410034894943, -1.057666301727295, 0.0034917236771434546] -2024-12-18 13:03:27,628 - INFO - Moving arm to joint values: [-0.02792803756892681, -0.47472870349884033, -0.038403209298849106, -0.022691410034894943, -1.0576682090759277, 0.0034917236771434546] -2024-12-18 13:06:00,687 - INFO - Checking for faults on the robot... -2024-12-18 13:06:03,691 - INFO - Successfully initialize the robot -2024-12-18 13:06:03,692 - INFO - Current joint values: [-0.02792995609343052, -0.47472870349884033, -0.03840896114706993, -0.022691410034894943, -1.057666301727295, 0.0034917236771434546] -2024-12-18 13:06:43,781 - INFO - Checking for faults on the robot... -2024-12-18 13:06:46,785 - INFO - Successfully initialize the robot -2024-12-18 13:06:46,787 - INFO - Current joint values: [-0.22792653739452362, -0.4747267961502075, -0.03840704262256622, -0.022691410034894943, -1.057666301727295, 0.003485971363261342] -2024-12-18 13:07:12,502 - INFO - Checking for faults on the robot... -2024-12-18 13:07:15,505 - INFO - Successfully initialize the robot -2024-12-18 13:07:15,506 - INFO - Current joint values: [-0.22793228924274445, -0.4747267961502075, -0.03840896114706993, -0.02269524522125721, -1.057666301727295, 0.0034878887236118317] -2024-12-18 13:07:30,905 - INFO - Checking for faults on the robot... -2024-12-18 13:07:33,909 - INFO - Successfully initialize the robot -2024-12-18 13:07:33,910 - INFO - Current joint values: [-0.22793036699295044, -0.47472870349884033, -0.03840704262256622, -0.02269524522125721, -1.0576682090759277, 0.0034917236771434546] -2024-12-18 13:59:04,860 - INFO - Checking for faults on the robot... -2024-12-18 13:59:07,864 - INFO - Successfully initialize the robot -2024-12-18 13:59:07,865 - INFO - Current joint values: [-0.4279269576072693, -0.47472870349884033, -0.03840896114706993, -0.02269524522125721, -1.057666301727295, 0.0034878887236118317] -2024-12-18 13:59:30,813 - INFO - Checking for faults on the robot... -2024-12-18 13:59:33,817 - INFO - Successfully initialize the robot -2024-12-18 13:59:33,818 - INFO - Current joint values: [-0.4279327094554901, -0.47472870349884033, -0.038403209298849106, -0.022691410034894943, -1.0576682090759277, 0.0034878887236118317] -2024-12-18 14:01:49,643 - INFO - Checking for faults on the robot... -2024-12-18 14:01:52,647 - INFO - Successfully initialize the robot -2024-12-18 14:01:52,649 - INFO - Current joint values: [-0.12792633473873138, -0.47472870349884033, -0.038403209298849106, -0.02269524522125721, -1.057666301727295, 0.003485971363261342] -2024-12-18 14:05:11,257 - INFO - Checking for faults on the robot... -2024-12-18 14:05:14,261 - INFO - Successfully initialize the robot -2024-12-18 14:05:14,263 - INFO - Current joint values: [-0.1279301643371582, -0.47472870349884033, -0.038403209298849106, -0.02269524522125721, -1.0576682090759277, 0.003485971363261342] -2024-12-18 14:06:30,357 - INFO - Checking for faults on the robot... -2024-12-18 14:06:33,361 - INFO - Successfully initialize the robot -2024-12-18 14:06:33,362 - INFO - Current joint values: [-0.027926120907068253, -0.47472870349884033, -0.03839745745062828, -0.022691410034894943, -1.0576701164245605, 0.0034878887236118317] -2024-12-18 14:10:53,013 - INFO - Checking for faults on the robot... -2024-12-18 14:10:56,017 - INFO - Successfully initialize the robot -2024-12-18 14:10:56,017 - INFO - Current joint values: [-0.1279301643371582, -0.47472870349884033, -0.038405127823352814, -0.022691410034894943, -1.0576682090759277, 0.003485971363261342] -2024-12-18 14:11:58,497 - INFO - Checking for faults on the robot... -2024-12-18 14:12:01,501 - INFO - Successfully initialize the robot -2024-12-18 14:12:01,501 - INFO - Current joint values: [-0.1279301643371582, -0.47473061084747314, -0.038405127823352814, -0.02269524522125721, -1.0576682090759277, 0.0034878887236118317] -2024-12-18 14:12:01,515 - INFO - Moving arm to joint values: [-0.12792803756892682, -0.47472870349884033, -0.038403209298849106, -0.022691410034894943, -1.0576682090759277, 0.0034917236771434546] -2024-12-18 14:12:01,516 - INFO - Moving arm to joint values: [-0.22792803756892682, -0.47472870349884033, -0.038403209298849106, -0.022691410034894943, -1.0576682090759277, 0.0034917236771434546] -2024-12-18 14:39:51,033 - INFO - Checking for faults on the robot... -2024-12-18 14:39:54,036 - INFO - Successfully initialize the robot -2024-12-18 14:39:54,037 - INFO - Current joint values: [-0.22793036699295044, -0.4747267961502075, -0.03840896114706993, -0.02269524522125721, -1.0576682090759277, 0.0034878887236118317] -2024-12-18 14:40:09,463 - INFO - Checking for faults on the robot... -2024-12-18 14:40:12,466 - INFO - Successfully initialize the robot -2024-12-18 14:40:12,467 - INFO - Current joint values: [-0.12792441248893738, -0.4747267961502075, -0.03840704262256622, -0.02269524522125721, -1.0576682090759277, 0.0034878887236118317] -2024-12-18 14:40:12,485 - INFO - Moving arm to joint values: [-0.12792803756892682, -0.47472870349884033, -0.038403209298849106, -0.022691410034894943, -1.0576682090759277, 0.0034917236771434546] -2024-12-18 14:40:12,488 - INFO - Moving arm to joint values: [-0.22792803756892682, -0.47472870349884033, -0.038403209298849106, -0.022691410034894943, -1.0576682090759277, 0.0034917236771434546] -2024-12-18 14:40:39,115 - INFO - Checking for faults on the robot... -2024-12-18 14:40:42,118 - INFO - Successfully initialize the robot -2024-12-18 14:40:42,119 - INFO - Current joint values: [-0.22792845964431763, -0.47472870349884033, -0.03840704262256622, -0.022691410034894943, -1.0576682090759277, 0.0034878887236118317] -2024-12-18 14:45:31,107 - INFO - Checking for faults on the robot... -2024-12-18 14:45:34,110 - INFO - Successfully initialize the robot -2024-12-18 14:45:34,111 - INFO - Current joint values: [-0.1279282420873642, -0.4747267961502075, -0.03840896114706993, -0.022691410034894943, -1.057666301727295, 0.0034878887236118317] -2024-12-18 14:45:34,129 - INFO - Moving arm to joint values: [-0.12792803756892682, -0.47472870349884033, -0.038403209298849106, -0.022691410034894943, -1.0576682090759277, 0.0034917236771434546] -2024-12-18 14:45:34,132 - INFO - Moving arm to joint values: [-0.22792803756892682, -0.47472870349884033, -0.038403209298849106, -0.022691410034894943, -1.0576682090759277, 0.0034917236771434546] -2024-12-18 14:55:05,039 - INFO - Checking for faults on the robot... -2024-12-18 14:55:08,042 - INFO - Successfully initialize the robot -2024-12-18 14:55:08,044 - INFO - Current end effector pose: -2024-12-18 14:55:54,576 - INFO - Checking for faults on the robot... -2024-12-18 14:55:57,580 - INFO - Successfully initialize the robot -2024-12-18 14:55:57,581 - INFO - Current end effector pose: -2024-12-18 14:56:56,664 - INFO - Checking for faults on the robot... -2024-12-18 14:56:59,667 - INFO - Successfully initialize the robot -2024-12-18 14:56:59,669 - INFO - Current end effector pose: -2024-12-18 15:01:35,440 - INFO - Checking for faults on the robot... -2024-12-18 15:01:38,443 - INFO - Successfully initialize the robot -2024-12-18 15:01:38,445 - INFO - Current end effector pose: -2024-12-18 15:01:38,453 - ERROR - Failed to move end effector to goal pose: index 3 is out of bounds for axis 0 with size 3 -2024-12-18 15:01:55,409 - INFO - Checking for faults on the robot... -2024-12-18 15:01:58,413 - INFO - Successfully initialize the robot -2024-12-18 15:01:58,415 - INFO - Current end effector pose: -2024-12-18 15:01:58,417 - INFO - Moved end effector to pose: -2024-12-18 15:02:20,114 - INFO - Checking for faults on the robot... -2024-12-18 15:02:23,118 - INFO - Successfully initialize the robot -2024-12-18 15:02:23,120 - INFO - Current end effector pose: -2024-12-18 15:02:23,122 - INFO - Moved end effector to pose: -2024-12-18 15:02:35,737 - INFO - Checking for faults on the robot... -2024-12-18 15:02:38,740 - INFO - Successfully initialize the robot -2024-12-18 15:02:38,742 - INFO - Current end effector pose: -2024-12-18 15:02:38,744 - INFO - Moved end effector to pose: -2024-12-18 15:02:48,428 - INFO - Checking for faults on the robot... -2024-12-18 15:02:51,432 - INFO - Successfully initialize the robot -2024-12-18 15:02:51,435 - INFO - Current end effector pose: -2024-12-18 15:02:51,436 - INFO - Moved end effector to pose: -2024-12-18 15:04:23,813 - INFO - Checking for faults on the robot... -2024-12-18 15:04:26,817 - INFO - Successfully initialize the robot -2024-12-18 15:04:26,819 - INFO - Current end effector pose: -2024-12-18 15:04:26,821 - INFO - Moved end effector to pose: -2024-12-18 15:05:03,880 - INFO - Checking for faults on the robot... -2024-12-18 15:05:06,883 - INFO - Successfully initialize the robot -2024-12-18 15:05:06,885 - INFO - Current end effector pose: -2024-12-18 15:05:06,887 - INFO - Moved end effector to pose: -2024-12-18 15:05:20,891 - INFO - Checking for faults on the robot... -2024-12-18 15:05:23,895 - INFO - Successfully initialize the robot -2024-12-18 15:05:23,898 - INFO - Current end effector pose: -2024-12-18 15:05:23,900 - INFO - Moved end effector to pose: -2024-12-18 15:06:39,124 - INFO - Checking for faults on the robot... -2024-12-18 15:06:42,127 - INFO - Successfully initialize the robot -2024-12-18 15:06:42,129 - INFO - Current end effector pose: -2024-12-18 15:06:42,131 - INFO - Moved end effector to pose: -2024-12-18 15:07:12,280 - INFO - Checking for faults on the robot... -2024-12-18 15:07:15,284 - INFO - Successfully initialize the robot -2024-12-18 15:07:15,286 - INFO - Current end effector pose: -2024-12-18 15:07:15,287 - INFO - Moved end effector to pose: [0.41095602400000003, -0.089484787, 0.7598677060000001], [0.7035419144838481, -0.07091498202064685, 0.7035416845699785, 0.07091500519530484] -2024-12-18 15:10:30,859 - INFO - Checking for faults on the robot... -2024-12-18 15:10:33,863 - INFO - Successfully initialize the robot -2024-12-18 15:10:33,865 - INFO - Current end effector pose: -2024-12-18 15:10:33,866 - INFO - Moved end effector to pose: [0.420434235, -0.00581615, 0.759876038], [0.7071068218077394, -0.00032550013355177364, 0.7071065907288765, 0.0003255002399235735] -2024-12-18 15:11:02,831 - INFO - Checking for faults on the robot... -2024-12-18 15:11:05,835 - INFO - Successfully initialize the robot -2024-12-18 15:11:05,837 - INFO - Current end effector pose: -2024-12-18 15:11:05,839 - INFO - Moved end effector to pose: [0.420434235, -0.005816157, 0.5598760380000001], [0.7071068218077394, -0.00032550013355177364, 0.7071065907288765, 0.0003255002399235735] -2024-12-18 15:11:35,530 - INFO - Checking for faults on the robot... -2024-12-18 15:11:38,533 - INFO - Successfully initialize the robot -2024-12-18 15:11:38,535 - INFO - Current end effector pose: -2024-12-18 15:11:38,548 - INFO - Moved end effector to pose: [0.420432465, -0.006418663, 0.35980011000000006], [0.7071051997683803, -0.0015491469907030697, 0.7071049686900474, 0.001549147496956483] -2024-12-18 15:12:25,733 - INFO - Checking for faults on the robot... -2024-12-18 15:12:28,736 - INFO - Successfully initialize the robot -2024-12-18 15:12:28,738 - INFO - Current end effector pose: -2024-12-18 15:12:28,739 - INFO - Moved end effector to pose: [0.420432465, -0.006418671, 0.45980011], [0.7071051997683803, -0.0015491469907030697, 0.7071049686900474, 0.001549147496956483] -2024-12-18 15:12:48,752 - INFO - Checking for faults on the robot... -2024-12-18 15:12:51,756 - INFO - Successfully initialize the robot -2024-12-18 15:12:51,758 - INFO - Current end effector pose: -2024-12-18 15:12:51,759 - INFO - Moved end effector to pose: [0.420432465, -0.006418671, 0.45980011], [0.7071051997683803, -0.0015491469907030697, 0.7071049686900474, 0.001549147496956483] -2024-12-18 15:13:00,445 - INFO - Checking for faults on the robot... -2024-12-18 15:13:03,449 - INFO - Successfully initialize the robot -2024-12-18 15:13:03,451 - INFO - Current end effector pose: -2024-12-18 15:13:03,815 - INFO - Moved end effector to pose: [0.420432465, -0.006418671, 0.45980011], [0.7071051997683803, -0.0015491469907030697, 0.7071049686900474, 0.001549147496956483] -2024-12-18 15:13:25,774 - INFO - Checking for faults on the robot... -2024-12-18 15:13:28,777 - INFO - Successfully initialize the robot -2024-12-18 15:13:28,779 - INFO - Current end effector pose: -2024-12-18 15:13:28,795 - INFO - Moved end effector to pose: [0.420432465, -0.00641867, 0.559800079], [0.7071051997683803, -0.0015491469907030697, 0.7071049686900474, 0.001549147496956483] -2024-12-18 15:13:49,828 - INFO - Checking for faults on the robot... -2024-12-18 15:13:52,832 - INFO - Successfully initialize the robot -2024-12-18 15:13:52,834 - INFO - Current end effector pose: -2024-12-18 15:13:52,850 - INFO - Moved end effector to pose: [0.420432465, -0.00641867, 0.559800079], [0.7071051997683803, -0.0015491469907030697, 0.7071049686900474, 0.001549147496956483] -2024-12-18 15:14:07,443 - INFO - Checking for faults on the robot... -2024-12-18 15:14:10,446 - INFO - Successfully initialize the robot -2024-12-18 15:14:10,448 - INFO - Current end effector pose: -2024-12-18 15:14:10,449 - INFO - Moved end effector to pose: [0.42028936800000005, -0.012681033, 0.660812195], [-0.7049980142066017, 0.015339326301879369, -0.7089204044956668, -0.013204738659008564] -2024-12-18 15:20:57,806 - INFO - Checking for faults on the robot... -2024-12-18 15:21:00,810 - INFO - Successfully initialize the robot -2024-12-18 15:21:00,812 - INFO - Current end effector pose: -2024-12-18 15:21:00,813 - INFO - Current joint values: [-0.040353283286094666, -0.8794407844543457, -0.7431389093399048, -0.06232563778758049, 0.045714545994997025, 0.06825639307498932] -2024-12-18 15:21:00,814 - INFO - Converted joint values [-0.040353283286094666, -0.8794407844543457, -0.7431389093399048, -0.06232563778758049, 0.045714545994997025, 0.06825639307498932] to Cartesian Pose. -2024-12-18 15:21:57,496 - INFO - Checking for faults on the robot... -2024-12-18 15:22:00,499 - INFO - Successfully initialize the robot -2024-12-18 15:22:00,501 - INFO - Current end effector pose: -2024-12-18 15:22:00,502 - INFO - Current joint values: [-0.040353283286094666, -0.8794465065002441, -0.7431408166885376, -0.06232372298836708, 0.04571837931871414, 0.06825447827577591] -2024-12-18 15:22:00,503 - INFO - Converted joint values [-0.040353283286094666, -0.8794465065002441, -0.7431408166885376, -0.06232372298836708, 0.04571837931871414, 0.06825447827577591] to Cartesian Pose. -2024-12-18 15:25:35,901 - INFO - Checking for faults on the robot... -2024-12-18 15:25:38,904 - INFO - Successfully initialize the robot -2024-12-18 15:25:38,906 - INFO - Current end effector pose: -2024-12-18 15:25:38,907 - INFO - Current joint values: [-0.040355198085308075, -0.8794426918029785, -0.7431408166885376, -0.06232563778758049, 0.04571837931871414, 0.0682525560259819] -2024-12-18 15:25:38,908 - INFO - Converted joint values [-0.040355198085308075, -0.8794426918029785, -0.7431408166885376, -0.06232563778758049, 0.04571837931871414, 0.0682525560259819] to Cartesian Pose. -2024-12-18 15:26:18,610 - INFO - Checking for faults on the robot... -2024-12-18 15:26:21,614 - INFO - Successfully initialize the robot -2024-12-18 15:26:21,616 - INFO - Current end effector pose: -2024-12-18 15:26:21,617 - INFO - Current joint values: [-0.040355198085308075, -0.8794426918029785, -0.7431427240371704, -0.0623275563120842, 0.04571837931871414, 0.0682525560259819] -2024-12-18 15:26:21,619 - INFO - Converted joint values [-0.040355198085308075, -0.8794426918029785, -0.7431427240371704, -0.0623275563120842, 0.04571837931871414, 0.0682525560259819] to Cartesian Pose [ 0.24948594 -0.00561569 0.66118192] and [ 0.70471241 -0.01734275 0.70915838 0.01319249]. -2024-12-18 15:27:35,231 - INFO - Checking for faults on the robot... -2024-12-18 15:27:38,235 - INFO - Successfully initialize the robot -2024-12-18 15:27:38,237 - INFO - Current end effector pose: -2024-12-18 15:27:38,238 - INFO - Current joint values: [-0.027922285720705986, -0.4747248589992523, -0.038393620401620865, -0.022691410034894943, -1.0576778650283813, 0.003482136409729719] -2024-12-18 15:27:38,239 - INFO - Converted joint values [-0.027922285720705986, -0.4747248589992523, -0.038393620401620865, -0.022691410034894943, -1.0576778650283813, 0.003482136409729719] to Cartesian Pose [ 0.2501445 -0.00422106 0.36013102] and [ 7.07145719e-01 -1.75818220e-04 7.07045707e-01 5.59184046e-03]. -2024-12-18 15:38:34,821 - INFO - Checking for faults on the robot... -2024-12-18 15:38:37,825 - INFO - Successfully initialize the robot -2024-12-18 15:38:37,827 - INFO - Current end effector pose: -2024-12-18 15:38:37,828 - INFO - Current joint values: [-0.02792995609343052, -0.47473254799842834, -0.03839745745062828, -0.022691410034894943, -1.057666301727295, 0.0034878887236118317] -2024-12-18 15:38:37,830 - INFO - Converted joint values [-0.02792995609343052, -0.47473254799842834, -0.03839745745062828, -0.022691410034894943, -1.057666301727295, 0.0034878887236118317] to Cartesian Pose [ 0.25014534 -0.00422299 0.3601331 ] and [ 7.07145732e-01 -1.80529106e-04 7.07045687e-01 5.59264452e-03]. -2024-12-18 15:38:37,884 - ERROR - Error converting Cartesian to joint values: 'Bestman_Real_Xarm6' object has no attribute '_validate_joint_limits' -2024-12-18 15:39:28,165 - INFO - Checking for faults on the robot... -2024-12-18 15:39:31,169 - INFO - Successfully initialize the robot -2024-12-18 15:39:31,171 - INFO - Current end effector pose: -2024-12-18 15:39:31,172 - INFO - Current joint values: [-0.02792995609343052, -0.47473254799842834, -0.03839553892612457, -0.02269524522125721, -1.057666301727295, 0.0034878887236118317] -2024-12-18 15:39:31,174 - INFO - Converted joint values [-0.02792995609343052, -0.47473254799842834, -0.03839553892612457, -0.02269524522125721, -1.057666301727295, 0.0034878887236118317] to Cartesian Pose [ 0.25014499 -0.00422252 0.36013246] and [ 7.07146424e-01 -1.78695836e-04 7.07045000e-01 5.59210996e-03]. -2024-12-18 15:39:31,202 - INFO - Converted Cartesian position [0.420434235, -0.00581615, 0.359876038] to joint values [ 0. 0. -0.00379779 0.11826075 -0.63562283 -0.00222824 - -1.05343637 0. ]. -2024-12-18 15:43:24,534 - INFO - Checking for faults on the robot... -2024-12-18 15:43:27,537 - INFO - Successfully initialize the robot -2024-12-18 15:43:27,539 - INFO - Current end effector pose: -2024-12-18 15:43:27,540 - INFO - Current joint values: [-0.02792803756892681, -0.47473254799842834, -0.038393620401620865, -0.022691410034894943, -1.0576701164245605, 0.003489806316792965] -2024-12-18 15:43:27,542 - INFO - Converted joint values [-0.02792803756892681, -0.47473254799842834, -0.038393620401620865, -0.022691410034894943, -1.0576701164245605, 0.003489806316792965] to Cartesian Pose [ 0.25014435 -0.00422249 0.3601322 ] and [ 7.07145739e-01 -1.80540572e-04 7.07045691e-01 5.59124647e-03]. -2024-12-18 15:43:27,559 - INFO - Converted Cartesian position [0.420434235, -0.00581615, 0.359876038] to joint values [ 0. 0. -0.00379779 0.11826075 -0.63562283 -0.00222824 - -1.05343637 0. ]. -2024-12-18 15:48:21,264 - INFO - Checking for faults on the robot... -2024-12-18 15:48:24,267 - INFO - Successfully initialize the robot -2024-12-18 15:48:24,269 - INFO - Current end effector pose: -2024-12-18 15:48:24,270 - INFO - Current joint values: [-0.02792995609343052, -0.47473254799842834, -0.03839745745062828, -0.02269524522125721, -1.0576682090759277, 0.0034917236771434546] -2024-12-18 15:48:24,272 - INFO - Converted joint values [-0.02792995609343052, -0.47473254799842834, -0.03839745745062828, -0.02269524522125721, -1.0576682090759277, 0.0034917236771434546] to Cartesian Pose [ 0.2501452 -0.00422252 0.36013328] and [ 7.07145071e-01 -1.80030507e-04 7.07046363e-01 5.59077080e-03]. -2024-12-18 15:49:10,269 - INFO - Checking for faults on the robot... -2024-12-18 15:49:13,272 - INFO - Successfully initialize the robot -2024-12-18 15:49:13,274 - INFO - Current end effector pose: -2024-12-18 15:49:13,275 - INFO - Current joint values: [-0.02792803756892681, -0.47473254799842834, -0.038393620401620865, -0.02269332855939865, -1.0576682090759277, 0.003489806316792965] -2024-12-18 15:49:13,277 - INFO - Converted joint values [-0.02792803756892681, -0.47473254799842834, -0.038393620401620865, -0.02269332855939865, -1.0576682090759277, 0.003489806316792965] to Cartesian Pose [ 0.2501445 -0.00422226 0.36013201] and [ 7.07146421e-01 -1.79624382e-04 7.07045012e-01 5.59099063e-03]. -2024-12-18 15:49:31,135 - INFO - Checking for faults on the robot... -2024-12-18 15:49:34,139 - INFO - Successfully initialize the robot -2024-12-18 15:49:34,141 - INFO - Current end effector pose: -2024-12-18 15:49:34,142 - INFO - Current joint values: [-0.02792995609343052, -0.47473445534706116, -0.03839745745062828, -0.02269524522125721, -1.0576682090759277, 0.0034917236771434546] -2024-12-18 15:49:34,144 - INFO - Converted joint values [-0.02792995609343052, -0.47473445534706116, -0.03839745745062828, -0.02269524522125721, -1.0576682090759277, 0.0034917236771434546] to Cartesian Pose [ 0.25014502 -0.00422252 0.36013376] and [ 7.07144397e-01 -1.80017004e-04 7.07047037e-01 5.59078946e-03]. -2024-12-18 15:57:31,038 - INFO - Checking for faults on the robot... -2024-12-18 15:57:34,040 - INFO - Successfully initialize the robot -2024-12-18 15:57:34,042 - INFO - Current end effector pose: -2024-12-18 15:57:34,043 - INFO - Current joint values: [-0.02792995609343052, -0.47473254799842834, -0.03839745745062828, -0.02269524522125721, -1.0576682090759277, 0.003489806316792965] -2024-12-18 15:57:34,046 - INFO - Converted joint values [-0.02792995609343052, -0.47473254799842834, -0.03839745745062828, -0.02269524522125721, -1.0576682090759277, 0.003489806316792965] to Cartesian Pose [ 0.2501452 -0.00422252 0.36013328] and [ 7.07145071e-01 -1.79352581e-04 7.07046358e-01 5.59144864e-03]. -2024-12-18 16:09:33,916 - INFO - Checking for faults on the robot... -2024-12-18 16:09:36,920 - INFO - Successfully initialize the robot -2024-12-18 16:09:36,922 - INFO - Current end effector pose: -2024-12-18 16:09:36,923 - INFO - Current joint values: [-0.02792995609343052, -0.47473445534706116, -0.03839745745062828, -0.02269524522125721, -1.0576701164245605, 0.003489806316792965] -2024-12-18 16:09:36,925 - INFO - Converted joint values [-0.02792995609343052, -0.47473445534706116, -0.03839745745062828, -0.02269524522125721, -1.0576701164245605, 0.003489806316792965] to Cartesian Pose [ 0.25014488 -0.00422252 0.36013394] and [ 7.07143723e-01 -1.79331395e-04 7.07047706e-01 5.59146511e-03]. -2024-12-18 16:14:19,449 - INFO - Checking for faults on the robot... -2024-12-18 16:14:22,453 - INFO - Successfully initialize the robot -2024-12-18 16:14:22,454 - INFO - Current end effector pose: -2024-12-18 16:14:22,454 - INFO - Current joint values: [-0.02792995609343052, -0.47473445534706116, -0.03839745745062828, -0.022691410034894943, -1.057666301727295, 0.003489806316792965] -2024-12-18 16:14:22,454 - INFO - Converted joint values [-0.02792995609343052, -0.47473445534706116, -0.03839745745062828, -0.022691410034894943, -1.057666301727295, 0.003489806316792965] to Cartesian Pose [ 0.25014516 -0.00422299 0.36013358] and [ 7.07145057e-01 -1.81193613e-04 7.07046367e-01 5.59198527e-03]. -2024-12-18 16:15:05,156 - INFO - Checking for faults on the robot... -2024-12-18 16:15:08,159 - INFO - Successfully initialize the robot -2024-12-18 16:15:08,161 - INFO - Current end effector pose: -2024-12-18 16:15:08,162 - INFO - Current joint values: [-0.02792995609343052, -0.47473445534706116, -0.038393620401620865, -0.02269524522125721, -1.0576682090759277, 0.003489806316792965] -2024-12-18 16:15:08,164 - INFO - Converted joint values [-0.02792995609343052, -0.47473445534706116, -0.038393620401620865, -0.02269524522125721, -1.0576682090759277, 0.003489806316792965] to Cartesian Pose [ 0.25014431 -0.0042225 0.36013248] and [ 7.07145753e-01 -1.79366242e-04 7.07045676e-01 5.59142976e-03]. -2024-12-18 16:18:14,396 - INFO - Checking for faults on the robot... -2024-12-18 16:18:17,399 - INFO - Successfully initialize the robot -2024-12-18 16:18:17,400 - INFO - Current end effector pose: -2024-12-18 16:18:17,400 - INFO - Current joint values: [-0.02792803756892681, -0.47473254799842834, -0.038393620401620865, -0.02269524522125721, -1.0576701164245605, 0.003489806316792965] -2024-12-18 16:18:17,400 - INFO - Converted joint values [-0.02792803756892681, -0.47473254799842834, -0.038393620401620865, -0.02269524522125721, -1.0576701164245605, 0.003489806316792965] to Cartesian Pose [ 0.25014436 -0.00422202 0.36013219] and [ 7.07145753e-01 -1.78693723e-04 7.07045681e-01 5.59073067e-03]. -2024-12-18 16:20:30,031 - INFO - Checking for faults on the robot... -2024-12-18 16:20:33,035 - INFO - Successfully initialize the robot -2024-12-18 16:20:33,037 - INFO - Current end effector pose: -2024-12-18 16:20:33,038 - INFO - Current joint values: [-0.02792995609343052, -0.47473445534706116, -0.03839745745062828, -0.022691410034894943, -1.0576701164245605, 0.003489806316792965] -2024-12-18 16:20:33,040 - INFO - Converted joint values [-0.02792995609343052, -0.47473445534706116, -0.03839745745062828, -0.022691410034894943, -1.0576701164245605, 0.003489806316792965] to Cartesian Pose [ 0.25014487 -0.00422298 0.36013395] and [ 7.07143709e-01 -1.81178245e-04 7.07047715e-01 5.59198090e-03]. -2024-12-18 16:21:02,151 - INFO - Checking for faults on the robot... -2024-12-18 16:21:05,155 - INFO - Successfully initialize the robot -2024-12-18 16:21:05,157 - INFO - Current end effector pose: -2024-12-18 16:21:05,158 - INFO - Current joint values: [-0.02792803756892681, -0.47473254799842834, -0.03839745745062828, -0.02269524522125721, -1.0576701164245605, 0.003489806316792965] -2024-12-18 16:21:05,160 - INFO - Converted joint values [-0.02792803756892681, -0.47473254799842834, -0.03839745745062828, -0.02269524522125721, -1.0576701164245605, 0.003489806316792965] to Cartesian Pose [ 0.25014506 -0.00422204 0.36013347] and [ 7.07144397e-01 -1.78666561e-04 7.07047038e-01 5.59076821e-03]. -2024-12-18 16:23:58,176 - INFO - Checking for faults on the robot... -2024-12-18 16:24:01,180 - INFO - Successfully initialize the robot -2024-12-18 16:24:01,182 - INFO - Current end effector pose: -2024-12-18 16:24:01,183 - INFO - Current joint values: [-0.02792995609343052, -0.47473254799842834, -0.03839745745062828, -0.02269524522125721, -1.0576682090759277, 0.003489806316792965] -2024-12-18 16:24:01,185 - INFO - Converted joint values [-0.02792995609343052, -0.47473254799842834, -0.03839745745062828, -0.02269524522125721, -1.0576682090759277, 0.003489806316792965] to Cartesian Pose [ 0.2501452 -0.00422252 0.36013328] and [ 7.07145071e-01 -1.79352581e-04 7.07046358e-01 5.59144864e-03]. -2024-12-18 16:25:25,381 - INFO - Checking for faults on the robot... -2024-12-18 16:25:28,384 - INFO - Successfully initialize the robot -2024-12-18 16:25:28,386 - INFO - Current end effector pose: -2024-12-18 16:25:28,387 - INFO - Current joint values: [-0.3434295356273651, -0.8785529732704163, -0.3339322805404663, -0.7777052521705627, -0.4970596432685852, 0.7144553661346436] -2024-12-18 16:25:28,389 - INFO - Converted joint values [-0.3434295356273651, -0.8785529732704163, -0.3339322805404663, -0.7777052521705627, -0.4970596432685852, 0.7144553661346436] to Cartesian Pose [ 0.25007871 -0.00518193 0.50048514] and [7.03587891e-01 9.64928697e-05 7.10606195e-01 1.70444139e-03]. -2024-12-18 16:28:19,565 - INFO - Checking for faults on the robot... -2024-12-18 16:28:22,569 - INFO - Successfully initialize the robot -2024-12-18 16:28:22,571 - INFO - Current end effector pose: -2024-12-18 16:28:22,572 - INFO - Current joint values: [-0.3434295356273651, -0.8785548806190491, -0.3339361250400543, -0.7777071595191956, -0.4970596432685852, 0.7144553661346436] -2024-12-18 16:28:22,574 - INFO - Converted joint values [-0.3434295356273651, -0.8785548806190491, -0.3339361250400543, -0.7777071595191956, -0.4970596432685852, 0.7144553661346436] to Cartesian Pose [ 0.25007825 -0.00518161 0.50048707] and [7.03586178e-01 9.80098062e-05 7.10607889e-01 1.70547698e-03]. -2024-12-18 16:29:51,847 - INFO - Checking for faults on the robot... -2024-12-18 16:29:54,851 - INFO - Successfully initialize the robot -2024-12-18 16:29:54,853 - INFO - Current end effector pose: -2024-12-18 16:29:54,854 - INFO - Current joint values: [-0.3434333801269531, -0.8785567879676819, -0.3339322805404663, -0.7777109742164612, -0.49706345796585083, 0.7144534587860107] -2024-12-18 16:29:54,856 - INFO - Converted joint values [-0.3434333801269531, -0.8785567879676819, -0.3339322805404663, -0.7777109742164612, -0.49706345796585083, 0.7144534587860107] to Cartesian Pose [ 0.25007756 -0.00518186 0.50048589] and [7.03586229e-01 9.96554519e-05 7.10607834e-01 1.70711965e-03]. -2024-12-18 16:29:55,215 - ERROR - Failed to move arm to joint values: [ 0. -4.00149035 -0.19108453 -0.79580863 -0.32920702 -0.41989968 - -0.48213191 0.37667688]. Error code: -8 -2024-12-18 16:30:30,315 - INFO - Checking for faults on the robot... -2024-12-18 16:30:33,317 - INFO - Successfully initialize the robot -2024-12-18 16:30:33,319 - INFO - Current end effector pose: -2024-12-18 16:30:33,320 - INFO - Current joint values: [-0.3434295356273651, -0.8785548806190491, -0.3339322805404663, -0.7777109742164612, -0.49706345796585083, 0.7144553661346436] -2024-12-18 16:30:33,322 - INFO - Converted joint values [-0.3434295356273651, -0.8785548806190491, -0.3339322805404663, -0.7777109742164612, -0.49706345796585083, 0.7144553661346436] to Cartesian Pose [ 0.250078 -0.00518105 0.50048544] and [7.03586868e-01 1.00110267e-04 7.10607207e-01 1.70484998e-03]. -2024-12-18 16:30:33,392 - INFO - Moving arm to joint values: [-0.19108453 -0.79580863 -0.32920702 -0.41989968 -0.48213191 0.37667688] -2024-12-18 16:30:40,356 - INFO - Checking for faults on the robot... -2024-12-18 16:30:43,359 - INFO - Successfully initialize the robot -2024-12-18 16:30:43,361 - INFO - Current end effector pose: -2024-12-18 16:30:43,362 - INFO - Current joint values: [-0.19108223915100098, -0.7958043217658997, -0.3292037844657898, -0.41989463567733765, -0.4821263253688812, 0.3766689896583557] -2024-12-18 16:30:43,363 - INFO - Converted joint values [-0.19108223915100098, -0.7958043217658997, -0.3292037844657898, -0.41989463567733765, -0.4821263253688812, 0.3766689896583557] to Cartesian Pose [ 0.27043563 -0.00567838 0.49721064] and [ 7.07110973e-01 -3.25483434e-04 7.07102439e-01 3.26887858e-04]. -2024-12-18 16:30:43,415 - INFO - Moving arm to joint values: [-0.11595228 -0.70094913 -0.36571698 -0.24654865 -0.51095967 0.2169669 ] -2024-12-18 16:30:47,320 - INFO - Checking for faults on the robot... -2024-12-18 16:30:50,323 - INFO - Successfully initialize the robot -2024-12-18 16:30:50,325 - INFO - Current end effector pose: -2024-12-18 16:30:50,326 - INFO - Current joint values: [-0.11594785749912262, -0.7009525299072266, -0.365714430809021, -0.24654905498027802, -0.5109613537788391, 0.21696048974990845] -2024-12-18 16:30:50,328 - INFO - Converted joint values [-0.11594785749912262, -0.7009525299072266, -0.365714430809021, -0.24654905498027802, -0.5109613537788391, 0.21696048974990845] to Cartesian Pose [ 0.29081091 -0.00592617 0.49499266] and [ 0.70926564 0.00074095 0.70493846 -0.00186094]. -2024-12-18 16:30:50,385 - INFO - Moving arm to joint values: [-0.09228574 -0.60722254 -0.41695144 -0.19691551 -0.54535215 0.17136337] -2024-12-18 16:31:44,365 - INFO - Checking for faults on the robot... -2024-12-18 16:31:47,369 - INFO - Successfully initialize the robot -2024-12-18 16:31:47,371 - INFO - Current end effector pose: -2024-12-18 16:31:47,372 - INFO - Current joint values: [-0.09229003638029099, -0.6072282195091248, -0.41695132851600647, -0.1969171166419983, -0.5453512668609619, 0.17136099934577942] -2024-12-18 16:31:47,374 - INFO - Converted joint values [-0.09229003638029099, -0.6072282195091248, -0.41695132851600647, -0.1969171166419983, -0.5453512668609619, 0.17136099934577942] to Cartesian Pose [ 0.31117608 -0.0061466 0.4933169 ] and [ 0.71057051 0.00211385 0.70360843 -0.00450106]. -2024-12-18 16:31:47,428 - INFO - Moving arm to joint values: [-0.08646262 -0.5351637 -0.46150452 -0.1895752 -0.56940426 0.16462447] -2024-12-18 16:31:58,814 - INFO - Checking for faults on the robot... -2024-12-18 16:32:01,818 - INFO - Successfully initialize the robot -2024-12-18 16:32:01,820 - INFO - Current end effector pose: -2024-12-18 16:32:01,821 - INFO - Current joint values: [-0.08646474778652191, -0.5351617932319641, -0.4615115523338318, -0.18957893550395966, -0.5694059729576111, 0.16461722552776337] -2024-12-18 16:32:01,823 - INFO - Converted joint values [-0.08646474778652191, -0.5351617932319641, -0.4615115523338318, -0.18957893550395966, -0.5694059729576111, 0.16461722552776337] to Cartesian Pose [ 0.32750492 -0.00638833 0.49186177] and [ 0.71161951 0.00341994 0.70251796 -0.00738136]. -2024-12-18 16:32:01,886 - INFO - Moving arm to joint values: [-0.11727354 -0.57439067 -0.43518294 -0.26255545 -0.56093625 0.22992395] -2024-12-18 16:32:05,003 - INFO - Checking for faults on the robot... -2024-12-18 16:32:08,006 - INFO - Successfully initialize the robot -2024-12-18 16:32:08,008 - INFO - Current end effector pose: -2024-12-18 16:32:08,009 - INFO - Current joint values: [-0.1172804981470108, -0.5743914246559143, -0.435186505317688, -0.26255616545677185, -0.5609422326087952, 0.22992071509361267] -2024-12-18 16:32:08,011 - INFO - Converted joint values [-0.1172804981470108, -0.5743914246559143, -0.435186505317688, -0.26255616545677185, -0.5609422326087952, 0.22992071509361267] to Cartesian Pose [ 0.31780266 -0.00713857 0.49084825] and [ 0.71262516 0.00459916 0.70145311 -0.01038105]. -2024-12-18 16:32:08,068 - INFO - Moving arm to joint values: [-0.15492367 -0.61383727 -0.41136466 -0.35116185 -0.55333455 0.30944015] -2024-12-18 16:32:15,447 - INFO - Checking for faults on the robot... -2024-12-18 16:32:18,448 - INFO - Successfully initialize the robot -2024-12-18 16:32:18,450 - INFO - Current end effector pose: -2024-12-18 16:32:18,451 - INFO - Current joint values: [-0.15493014454841614, -0.6138396859169006, -0.41136571764945984, -0.3511684834957123, -0.5533298850059509, 0.30944034457206726] -2024-12-18 16:32:18,453 - INFO - Converted joint values [-0.15493014454841614, -0.6138396859169006, -0.41136571764945984, -0.3511684834957123, -0.5533298850059509, 0.30944034457206726] to Cartesian Pose [ 0.30807579 -0.00785491 0.48961191] and [ 0.71400726 0.00570048 0.69999012 -0.01322784]. -2024-12-18 16:32:18,508 - INFO - Moving arm to joint values: [-0.14425605 -0.56811725 -0.43563316 -0.33252851 -0.56697177 0.29251705] -2024-12-18 16:32:24,363 - INFO - Checking for faults on the robot... -2024-12-18 16:32:27,366 - INFO - Successfully initialize the robot -2024-12-18 16:32:27,368 - INFO - Current end effector pose: -2024-12-18 16:32:27,369 - INFO - Current joint values: [-0.1442517191171646, -0.5681136250495911, -0.43563327193260193, -0.332524836063385, -0.5669784545898438, 0.2925109565258026] -2024-12-18 16:32:27,371 - INFO - Converted joint values [-0.1442517191171646, -0.5681136250495911, -0.43563327193260193, -0.332524836063385, -0.5669784545898438, 0.2925109565258026] to Cartesian Pose [ 0.31830267 -0.00790493 0.48761692] and [ 0.71582114 0.00664203 0.69807135 -0.01588601]. -2024-12-18 16:32:27,424 - INFO - Moving arm to joint values: [-0.1461244 -0.54412963 -0.44837743 -0.3419138 -0.57480053 0.30096639] -2024-12-18 16:32:33,877 - INFO - Checking for faults on the robot... -2024-12-18 16:32:36,881 - INFO - Successfully initialize the robot -2024-12-18 16:32:36,883 - INFO - Current end effector pose: -2024-12-18 16:32:36,884 - INFO - Current joint values: [-0.14612509310245514, -0.5441317558288574, -0.4483787417411804, -0.341922402381897, -0.5748094320297241, 0.3009631931781769] -2024-12-18 16:32:36,886 - INFO - Converted joint values [-0.14612509310245514, -0.5441317558288574, -0.4483787417411804, -0.341922402381897, -0.5748094320297241, 0.3009631931781769] to Cartesian Pose [ 0.32350733 -0.00812022 0.48581259] and [ 0.71753681 0.00755203 0.69623051 -0.01862699]. -2024-12-18 16:32:36,948 - INFO - Moving arm to joint values: [-0.15797634 -0.54111601 -0.44860192 -0.37300491 -0.57753913 0.32885722] -2024-12-18 16:32:46,193 - INFO - Checking for faults on the robot... -2024-12-18 16:32:49,197 - INFO - Successfully initialize the robot -2024-12-18 16:32:49,199 - INFO - Current end effector pose: -2024-12-18 16:32:49,200 - INFO - Current joint values: [-0.15798276662826538, -0.5411117076873779, -0.4486011862754822, -0.3730066120624542, -0.5775476098060608, 0.3288567066192627] -2024-12-18 16:32:49,201 - INFO - Converted joint values [-0.15798276662826538, -0.5411117076873779, -0.4486011862754822, -0.3730066120624542, -0.5775476098060608, 0.3288567066192627] to Cartesian Pose [ 0.32369599 -0.00852857 0.48413646] and [ 0.7192974 0.00838029 0.69432259 -0.02138135]. -2024-12-18 16:32:49,256 - INFO - Moving arm to joint values: [-0.16911621 -0.53815818 -0.44864048 -0.40244552 -0.58010083 0.35528901] -2024-12-18 16:33:04,626 - INFO - Checking for faults on the robot... -2024-12-18 16:33:07,630 - INFO - Successfully initialize the robot -2024-12-18 16:33:07,631 - INFO - Current end effector pose: -2024-12-18 16:33:07,631 - INFO - Current joint values: [-0.16911371052265167, -0.5381645560264587, -0.44864335656166077, -0.40245136618614197, -0.5801016688346863, 0.35528528690338135] -2024-12-18 16:33:07,632 - INFO - Converted joint values [-0.16911371052265167, -0.5381645560264587, -0.44864335656166077, -0.40245136618614197, -0.5801016688346863, 0.35528528690338135] to Cartesian Pose [ 0.32385438 -0.0089123 0.48236601] and [ 0.72119884 0.00912526 0.69224923 -0.02408253]. -2024-12-18 16:33:07,683 - INFO - Moving arm to joint values: [-0.16928958 -0.5169392 -0.45921082 -0.40812183 -0.58592702 0.36059332] -2024-12-18 16:33:15,338 - INFO - Checking for faults on the robot... -2024-12-18 16:33:18,340 - INFO - Successfully initialize the robot -2024-12-18 16:33:18,342 - INFO - Current end effector pose: -2024-12-18 16:33:18,343 - INFO - Current joint values: [-0.16929012537002563, -0.516936182975769, -0.4592105746269226, -0.4081290066242218, -0.585932731628418, 0.36059093475341797] -2024-12-18 16:33:18,345 - INFO - Converted joint values [-0.16929012537002563, -0.516936182975769, -0.4592105746269226, -0.4081290066242218, -0.585932731628418, 0.36059093475341797] to Cartesian Pose [ 0.32847731 -0.00905009 0.48028277] and [ 0.72323905 0.00976548 0.69001115 -0.0267306 ]. -2024-12-18 16:33:18,400 - INFO - Moving arm to joint values: [-0.17476017 -0.50605171 -0.46373149 -0.42520966 -0.58952449 0.3760937 ] -2024-12-18 16:33:24,726 - INFO - Checking for faults on the robot... -2024-12-18 16:33:27,730 - INFO - Successfully initialize the robot -2024-12-18 16:33:27,732 - INFO - Current end effector pose: -2024-12-18 16:33:27,733 - INFO - Current joint values: [-0.1747664362192154, -0.5060487389564514, -0.463731974363327, -0.42521756887435913, -0.5895337462425232, 0.3760937452316284] -2024-12-18 16:33:27,734 - INFO - Converted joint values [-0.1747664362192154, -0.5060487389564514, -0.463731974363327, -0.42521756887435913, -0.5895337462425232, 0.3760937452316284] to Cartesian Pose [ 0.33058143 -0.00931579 0.47829782] and [ 0.72529393 0.01034688 0.68773421 -0.02938234]. -2024-12-18 16:33:27,813 - INFO - Moving arm to joint values: [-0.18196304 -0.49928816 -0.46568393 -0.44594024 -0.59216837 0.39483737] -2024-12-18 16:34:13,434 - INFO - Checking for faults on the robot... -2024-12-18 16:34:16,438 - INFO - Successfully initialize the robot -2024-12-18 16:34:16,439 - INFO - Current end effector pose: -2024-12-18 16:34:16,440 - INFO - Current joint values: [-0.18196846544742584, -0.49928581714630127, -0.4656839668750763, -0.44594356417655945, -0.5921702980995178, 0.39483514428138733] -2024-12-18 16:34:16,441 - INFO - Converted joint values [-0.18196846544742584, -0.49928581714630127, -0.4656839668750763, -0.44594356417655945, -0.5921702980995178, 0.39483514428138733] to Cartesian Pose [ 0.33166406 -0.00962803 0.47630497] and [ 0.72742454 0.0108522 0.68535496 -0.03200545]. -2024-12-18 16:35:51,584 - INFO - Checking for faults on the robot... -2024-12-18 16:35:54,587 - INFO - Successfully initialize the robot -2024-12-18 16:35:54,589 - INFO - Current end effector pose: -2024-12-18 16:35:54,590 - INFO - Current joint values: [-0.18196655809879303, -0.49928581714630127, -0.4656878113746643, -0.4459473788738251, -0.5921741127967834, 0.39483514428138733] -2024-12-18 16:35:54,592 - INFO - Converted joint values [-0.18196655809879303, -0.49928581714630127, -0.4656878113746643, -0.4459473788738251, -0.5921741127967834, 0.39483514428138733] to Cartesian Pose [ 0.33166402 -0.00962691 0.47630677] and [ 0.72742225 0.01085547 0.68535732 -0.03200601]. -2024-12-18 16:35:54,940 - INFO - Moving arm to joint values: [-0.15140485 -0.41769749 -0.5152589 -0.3872068 -0.60674441 0.3440546 ] -2024-12-18 16:36:12,100 - INFO - Checking for faults on the robot... -2024-12-18 16:36:15,104 - INFO - Successfully initialize the robot -2024-12-18 16:36:15,106 - INFO - Current end effector pose: -2024-12-18 16:36:15,106 - INFO - Current joint values: [-0.027920367196202278, -0.47473636269569397, -0.03839937224984169, -0.02268374152481556, -1.0576778650283813, 0.003482136409729719] -2024-12-18 16:36:15,106 - INFO - Converted joint values [-0.027920367196202278, -0.47473636269569397, -0.03839937224984169, -0.02268374152481556, -1.0576778650283813, 0.003482136409729719] to Cartesian Pose [ 0.25014449 -0.00422151 0.36013582] and [ 7.07139593e-01 -1.78710583e-04 7.07051830e-01 5.59236235e-03]. -2024-12-18 16:36:15,159 - INFO - Moving arm to joint values: [-0.04150686 -0.37307685 -0.10900079 -0.04580389 -1.08914953 0.02123348] -2024-12-18 16:36:28,995 - INFO - Checking for faults on the robot... -2024-12-18 16:36:31,998 - INFO - Successfully initialize the robot -2024-12-18 16:36:31,999 - INFO - Current end effector pose: -2024-12-18 16:36:32,000 - INFO - Current joint values: [-0.041505686938762665, -0.37307947874069214, -0.1090046763420105, -0.04580466449260712, -1.0891493558883667, 0.021228376775979996] -2024-12-18 16:36:32,001 - INFO - Converted joint values [-0.041505686938762665, -0.37307947874069214, -0.1090046763420105, -0.04580466449260712, -1.0891493558883667, 0.021228376775979996] to Cartesian Pose [ 0.27043482 -0.00567766 0.35987804] and [ 7.07104589e-01 -3.22816393e-04 7.07108824e-01 3.26870088e-04]. -2024-12-18 16:36:32,054 - INFO - Moving arm to joint values: [-0.05037627 -0.28696404 -0.17889024 -0.0631509 -1.10506496 0.03647826] -2024-12-18 16:36:37,808 - INFO - Checking for faults on the robot... -2024-12-18 16:36:40,811 - INFO - Successfully initialize the robot -2024-12-18 16:36:40,813 - INFO - Current end effector pose: -2024-12-18 16:36:40,814 - INFO - Current joint values: [-0.05038359761238098, -0.28696754574775696, -0.1788966804742813, -0.06315974146127701, -1.1050740480422974, 0.03647806495428085] -2024-12-18 16:36:40,816 - INFO - Converted joint values [-0.05038359761238098, -0.28696754574775696, -0.1788966804742813, -0.06315974146127701, -1.1050740480422974, 0.03647806495428085] to Cartesian Pose [ 0.29066552 -0.00702262 0.35972585] and [ 0.70733923 -0.0007357 0.70685617 -0.00500344]. -2024-12-18 16:36:40,878 - INFO - Moving arm to joint values: [-0.05653082 -0.21081827 -0.24860129 -0.07734193 -1.11098133 0.05078134] -2024-12-18 16:36:43,490 - INFO - Checking for faults on the robot... -2024-12-18 16:36:46,493 - INFO - Successfully initialize the robot -2024-12-18 16:36:46,494 - INFO - Current end effector pose: -2024-12-18 16:36:46,494 - INFO - Current joint values: [-0.05652719363570213, -0.21082456409931183, -0.2485988438129425, -0.07733947783708572, -1.1109778881072998, 0.050778597593307495] -2024-12-18 16:36:46,494 - INFO - Converted joint values [-0.05652719363570213, -0.21082456409931183, -0.2485988438129425, -0.07733947783708572, -1.1109778881072998, 0.050778597593307495] to Cartesian Pose [ 0.31085532 -0.00825336 0.35957856] and [ 0.70766509 -0.00132531 0.70647113 -0.01033982]. -2024-12-18 16:36:46,542 - INFO - Moving arm to joint values: [-0.06093932 -0.14172297 -0.31845313 -0.08960802 -1.109535 0.06478463] -2024-12-18 16:36:48,879 - INFO - Checking for faults on the robot... -2024-12-18 16:36:51,882 - INFO - Successfully initialize the robot -2024-12-18 16:36:51,884 - INFO - Current end effector pose: -2024-12-18 16:36:51,885 - INFO - Current joint values: [-0.06093738600611687, -0.14172640442848206, -0.31845441460609436, -0.08960748463869095, -1.1095378398895264, 0.06478192657232285] -2024-12-18 16:36:51,887 - INFO - Converted joint values [-0.06093738600611687, -0.14172640442848206, -0.31845441460609436, -0.08960748463869095, -1.1095378398895264, 0.06478192657232285] to Cartesian Pose [ 0.3310143 -0.0093756 0.35943286] and [ 0.70804854 -0.00203371 0.70598757 -0.01564233]. -2024-12-18 16:36:51,941 - INFO - Moving arm to joint values: [-0.06415481 -0.07771941 -0.3887083 -0.10063938 -1.10246046 0.07878964] -2024-12-18 16:36:57,867 - INFO - Checking for faults on the robot... -2024-12-18 16:37:00,871 - INFO - Successfully initialize the robot -2024-12-18 16:37:00,873 - INFO - Current end effector pose: -2024-12-18 16:37:00,874 - INFO - Current joint values: [-0.06415874511003494, -0.07771913707256317, -0.38871073722839355, -0.10064639896154404, -1.102460503578186, 0.0787871703505516] -2024-12-18 16:37:00,876 - INFO - Converted joint values [-0.06415874511003494, -0.07771913707256317, -0.38871073722839355, -0.10064639896154404, -1.102460503578186, 0.0787871703505516] to Cartesian Pose [ 0.35114812 -0.01039105 0.35927266] and [ 0.70849179 -0.00282239 0.70540428 -0.02088591]. -2024-12-18 16:37:00,942 - INFO - Moving arm to joint values: [-0.06651505 -0.01744301 -0.45958402 -0.11086321 -1.09092479 0.09294348] -2024-12-18 16:41:29,206 - INFO - Checking for faults on the robot... -2024-12-18 16:41:32,210 - INFO - Successfully initialize the robot -2024-12-18 16:41:32,212 - INFO - Current end effector pose: -2024-12-18 16:41:32,213 - INFO - Current joint values: [-0.043637920171022415, -0.28797996044158936, -0.21119080483913422, -3.834952167380834e-06, 0.4991611838340759, 0.04014044255018234] -2024-12-18 16:41:32,214 - INFO - Converted joint values [-0.043637920171022415, -0.28797996044158936, -0.21119080483913422, -3.834952167380834e-06, 0.4991611838340759, 0.04014044255018234] to Cartesian Pose [ 0.27822749 -0.01214917 0.19437073] and [ 9.99122847e-01 -4.18752499e-02 4.78268035e-06 4.59945902e-06]. -2024-12-18 16:41:32,267 - INFO - Moving arm to joint values: [-0.05277871 -0.18336451 -0.19883586 -0.01302102 0.38214986 0.04091448] -2024-12-18 16:41:36,971 - INFO - Checking for faults on the robot... -2024-12-18 16:41:39,975 - INFO - Successfully initialize the robot -2024-12-18 16:41:39,977 - INFO - Current end effector pose: -2024-12-18 16:41:39,978 - INFO - Current joint values: [-0.052786197513341904, -0.1833624690771103, -0.1988326758146286, -0.013029249384999275, 0.38213953375816345, 0.040913183242082596] -2024-12-18 16:41:39,980 - INFO - Converted joint values [-0.052786197513341904, -0.1833624690771103, -0.1988326758146286, -0.013029249384999275, 0.38213953375816345, 0.040913183242082596] to Cartesian Pose [ 0.27601605 -0.01413562 0.17060509] and [ 9.99164632e-01 -4.07935847e-02 1.33361110e-05 2.43329214e-03]. -2024-12-18 16:41:40,047 - INFO - Moving arm to joint values: [-0.06469863 -0.06751967 -0.19611212 -0.03225192 0.26365645 0.04633606] -2024-12-18 16:42:11,943 - INFO - Checking for faults on the robot... -2024-12-18 16:42:14,947 - INFO - Successfully initialize the robot -2024-12-18 16:42:14,950 - INFO - Current end effector pose: -2024-12-18 16:42:14,951 - INFO - Current joint values: [0.8804762363433838, -0.4831387400627136, -0.43810874223709106, 1.4105087518692017, -0.8393310308456421, -0.9861693978309631] -2024-12-18 16:42:14,953 - INFO - Converted joint values [0.8804762363433838, -0.4831387400627136, -0.43810874223709106, 1.4105087518692017, -0.8393310308456421, -0.9861693978309631] to Cartesian Pose [0.2548577 0.11798433 0.40900307] and [0.80912723 0.00425489 0.58760083 0.00450348]. -2024-12-18 16:42:15,324 - INFO - Moving arm to joint values: [ 0.79357938 -0.42553265 -0.43929476 1.26982862 -0.79572055 -0.86372619] -2024-12-18 16:42:19,369 - INFO - Checking for faults on the robot... -2024-12-18 16:42:22,373 - INFO - Successfully initialize the robot -2024-12-18 16:42:22,375 - INFO - Current end effector pose: -2024-12-18 16:42:22,376 - INFO - Current joint values: [0.793576180934906, -0.42553776502609253, -0.43929949402809143, 1.2698273658752441, -0.7957180142402649, -0.8637290000915527] -2024-12-18 16:42:22,378 - INFO - Converted joint values [0.793576180934906, -0.42553776502609253, -0.43929949402809143, 1.2698273658752441, -0.7957180142402649, -0.8637290000915527] to Cartesian Pose [0.27701032 0.11478789 0.40380408] and [ 8.03998745e-01 -2.80538309e-04 5.94630817e-01 3.61946020e-04]. -2024-12-18 16:42:22,444 - INFO - Moving arm to joint values: [ 0.70922947 -0.36312311 -0.44843231 1.12093982 -0.76635632 -0.73652297] -2024-12-18 16:45:18,128 - INFO - Checking for faults on the robot... -2024-12-18 16:45:21,131 - INFO - Successfully initialize the robot -2024-12-18 16:45:21,133 - INFO - Current end effector pose: -2024-12-18 16:45:21,134 - INFO - Current joint values: [-0.02792803756892681, -0.47473445534706116, -0.038393620401620865, -0.02269524522125721, -1.0576759576797485, 0.003489806316792965] -2024-12-18 16:45:21,136 - INFO - Converted joint values [-0.02792803756892681, -0.47473445534706116, -0.038393620401620865, -0.02269524522125721, -1.0576759576797485, 0.003489806316792965] to Cartesian Pose [ 0.25014373 -0.00422201 0.36013323] and [ 7.07143014e-01 -1.78656694e-04 7.07048421e-01 5.59074264e-03]. -2024-12-18 16:45:21,202 - INFO - Moving arm to joint values: [-0.04363769 -0.40080548 -0.0883018 -0.04838655 -1.08217452 0.02273059] -2024-12-18 16:53:15,627 - INFO - Checking for faults on the robot... -2024-12-18 16:53:18,631 - INFO - Successfully initialize the robot -2024-12-18 16:53:18,632 - INFO - Current end effector pose: -2024-12-18 16:53:18,632 - INFO - Current joint values: [-0.027926120907068253, -0.47473254799842834, -0.03839745745062828, -0.022689493373036385, -1.0576682090759277, 0.003482136409729719] -2024-12-18 16:53:18,632 - INFO - Converted joint values [-0.027926120907068253, -0.47473254799842834, -0.03839745745062828, -0.022689493373036385, -1.0576682090759277, 0.003482136409729719] to Cartesian Pose [ 0.25014521 -0.00422227 0.36013329] and [ 7.07145052e-01 -1.78054523e-04 7.07046361e-01 5.59357786e-03]. -2024-12-18 16:53:38,079 - INFO - Checking for faults on the robot... -2024-12-18 16:53:41,082 - INFO - Successfully initialize the robot -2024-12-18 16:53:41,083 - INFO - Current end effector pose: -2024-12-18 16:53:41,083 - INFO - Current joint values: [-0.027922285720705986, -0.47473445534706116, -0.038393620401620865, -0.022685658186674118, -1.0576682090759277, 0.0034840537700802088] -2024-12-18 16:53:41,084 - INFO - Converted joint values [-0.027922285720705986, -0.47473445534706116, -0.038393620401620865, -0.022685658186674118, -1.0576682090759277, 0.0034840537700802088] to Cartesian Pose [ 0.25014434 -0.00422175 0.3601325 ] and [ 7.07145720e-01 -1.79236937e-04 7.07045704e-01 5.59204112e-03]. -2024-12-18 16:53:41,142 - INFO - Moving arm to joint values: [-0.04363769 -0.40080548 -0.0883018 -0.04838655 -1.08217452 0.02273059] -2024-12-18 16:54:00,768 - INFO - Checking for faults on the robot... -2024-12-18 16:54:03,772 - INFO - Successfully initialize the robot -2024-12-18 16:54:03,774 - INFO - Current end effector pose: -2024-12-18 16:54:03,775 - INFO - Current joint values: [-0.043639834970235825, -0.40081000328063965, -0.08829785138368607, -0.04839134216308594, -1.0821774005889893, 0.0227278433740139] -2024-12-18 16:54:03,777 - INFO - Converted joint values [-0.043639834970235825, -0.40081000328063965, -0.08829785138368607, -0.04839134216308594, -1.0821774005889893, 0.0227278433740139] to Cartesian Pose [ 0.2644329 -0.00567246 0.35987626] and [ 7.07105636e-01 -3.22962991e-04 7.07107777e-01 3.26516657e-04]. -2024-12-18 16:54:03,829 - INFO - Moving arm to joint values: [-0.06234195 -0.39871487 -0.08953733 -0.07749853 -1.08306928 0.04434266] -2024-12-18 16:54:09,537 - INFO - Checking for faults on the robot... -2024-12-18 16:54:12,540 - INFO - Successfully initialize the robot -2024-12-18 16:54:12,542 - INFO - Current end effector pose: -2024-12-18 16:54:12,543 - INFO - Current joint values: [-0.06234673038125038, -0.3987199664115906, -0.08954229205846786, -0.07750438153743744, -1.0830748081207275, 0.04433779790997505] -2024-12-18 16:54:12,545 - INFO - Converted joint values [-0.06234673038125038, -0.3987199664115906, -0.08954229205846786, -0.07750438153743744, -1.0830748081207275, 0.04433779790997505] to Cartesian Pose [ 0.26466588 -0.0071116 0.35971177] and [ 7.07355201e-01 -6.73306417e-04 7.06840368e-01 -4.98612488e-03]. -2024-12-18 16:54:12,595 - INFO - Moving arm to joint values: [-0.08041102 -0.39663926 -0.09074135 -0.10582517 -1.08388427 0.06553109] -2024-12-18 16:54:21,296 - INFO - Checking for faults on the robot... -2024-12-18 16:54:24,300 - INFO - Successfully initialize the robot -2024-12-18 16:54:24,302 - INFO - Current end effector pose: -2024-12-18 16:54:24,303 - INFO - Current joint values: [-0.08041702210903168, -0.39663374423980713, -0.0907483845949173, -0.10583317279815674, -1.0838935375213623, 0.06552974134683609] -2024-12-18 16:54:24,305 - INFO - Converted joint values [-0.08041702210903168, -0.39663374423980713, -0.0907483845949173, -0.10583317279815674, -1.0838935375213623, 0.06552974134683609] to Cartesian Pose [ 0.26485144 -0.00849048 0.35948423] and [ 0.70775517 -0.00104751 0.70638219 -0.01028215]. -2024-12-18 16:54:24,354 - INFO - Moving arm to joint values: [-0.09786297 -0.39458264 -0.09190178 -0.13338775 -1.0846115 0.08630933] -2024-12-18 16:54:25,586 - INFO - Checking for faults on the robot... -2024-12-18 16:54:28,590 - INFO - Successfully initialize the robot -2024-12-18 16:54:28,592 - INFO - Current end effector pose: -2024-12-18 16:54:28,593 - INFO - Current joint values: [-0.09785646945238113, -0.39458972215652466, -0.09189503639936447, -0.13338346779346466, -1.0846068859100342, 0.0863017588853836] -2024-12-18 16:54:28,595 - INFO - Converted joint values [-0.09785646945238113, -0.39458972215652466, -0.09189503639936447, -0.13338346779346466, -1.0846068859100342, 0.0863017588853836] to Cartesian Pose [ 0.26498807 -0.00980834 0.35919848] and [ 0.7082982 -0.0014492 0.70574047 -0.01555474]. -2024-12-18 16:54:28,646 - INFO - Moving arm to joint values: [-0.11471778 -0.39255499 -0.09300055 -0.16021478 -1.08523811 0.10668974] -2024-12-18 16:54:29,659 - INFO - Checking for faults on the robot... -2024-12-18 16:54:32,662 - INFO - Successfully initialize the robot -2024-12-18 16:54:32,664 - INFO - Current end effector pose: -2024-12-18 16:54:32,665 - INFO - Current joint values: [-0.11471492052078247, -0.39255911111831665, -0.09299758821725845, -0.16021662950515747, -1.085235834121704, 0.10668452829122543] -2024-12-18 16:54:32,667 - INFO - Converted joint values [-0.11471492052078247, -0.39255911111831665, -0.09299758821725845, -0.16021662950515747, -1.085235834121704, 0.10668452829122543] to Cartesian Pose [ 0.26508296 -0.01107026 0.35885742] and [ 0.70897243 -0.00188201 0.70492679 -0.02080322]. -2024-12-18 16:54:32,717 - INFO - Moving arm to joint values: [-0.13100874 -0.39055011 -0.09403675 -0.18634651 -1.08576727 0.12669831] -2024-12-18 16:55:23,068 - INFO - Checking for faults on the robot... -2024-12-18 16:55:26,071 - INFO - Successfully initialize the robot -2024-12-18 16:55:26,073 - INFO - Current end effector pose: -2024-12-18 16:55:26,074 - INFO - Current joint values: [-0.027922285720705986, -0.47473061084747314, -0.038393620401620865, -0.022691410034894943, -1.057666301727295, 0.003482136409729719] -2024-12-18 16:55:26,077 - INFO - Converted joint values [-0.027922285720705986, -0.47473061084747314, -0.038393620401620865, -0.022691410034894943, -1.057666301727295, 0.003482136409729719] to Cartesian Pose [ 0.25014485 -0.00422106 0.36013134] and [ 7.07147774e-01 -1.75824068e-04 7.07043652e-01 5.59190996e-03]. -2024-12-18 16:55:53,090 - INFO - Checking for faults on the robot... -2024-12-18 16:55:56,094 - INFO - Successfully initialize the robot -2024-12-18 16:55:56,096 - INFO - Current end effector pose: -2024-12-18 16:55:56,098 - INFO - Current joint values: [-0.027926120907068253, -0.47473061084747314, -0.038393620401620865, -0.022689493373036385, -1.0576701164245605, 0.003482136409729719] -2024-12-18 16:55:56,100 - INFO - Converted joint values [-0.027926120907068253, -0.47473061084747314, -0.038393620401620865, -0.022689493373036385, -1.0576701164245605, 0.003482136409729719] to Cartesian Pose [ 0.25014454 -0.00422225 0.36013171] and [ 7.07146418e-01 -1.78087706e-04 7.07044994e-01 5.59351919e-03]. -2024-12-18 16:55:56,164 - INFO - Moving arm to joint values: [-0.04983681 -0.47098664 -0.03980306 -0.05606362 -1.06067688 0.02740035] -2024-12-18 16:56:48,361 - INFO - Checking for faults on the robot... -2024-12-18 16:56:51,364 - INFO - Successfully initialize the robot -2024-12-18 16:56:51,366 - INFO - Current end effector pose: -2024-12-18 16:56:51,367 - INFO - Current joint values: [0.6476946473121643, -0.4308626055717468, -0.891028106212616, 1.9065041542053223, -1.011008381843567, -1.7163710594177246] -2024-12-18 16:56:51,369 - INFO - Converted joint values [0.6476946473121643, -0.4308626055717468, -0.891028106212616, 1.9065041542053223, -1.011008381843567, -1.7163710594177246] to Cartesian Pose [0.2758677 0.06362008 0.50262956] and [ 0.83171395 -0.09450834 0.5224177 0.16248024]. -2024-12-18 16:56:51,439 - INFO - Moving arm to joint values: [ 0.63201973 -0.43794378 -0.87549602 1.88070895 -0.99998811 -1.68199854] -2024-12-18 16:56:53,738 - INFO - Checking for faults on the robot... -2024-12-18 16:56:56,741 - INFO - Successfully initialize the robot -2024-12-18 16:56:56,743 - INFO - Current end effector pose: -2024-12-18 16:56:56,744 - INFO - Current joint values: [0.6320192813873291, -0.43794384598731995, -0.8754984140396118, 1.8807045221328735, -0.9999924898147583, -1.681996464729309] -2024-12-18 16:56:56,746 - INFO - Converted joint values [0.6320192813873291, -0.43794384598731995, -0.8754984140396118, 1.8807045221328735, -0.9999924898147583, -1.681996464729309] to Cartesian Pose [0.27723977 0.05818542 0.50186701] and [ 0.82589571 -0.10364114 0.53019566 0.16139192]. -2024-12-18 16:56:56,816 - INFO - Moving arm to joint values: [ 0.6160001 -0.44486083 -0.86028297 1.85435242 -0.98895687 -1.64747968] -2024-12-18 16:56:58,868 - INFO - Checking for faults on the robot... -2024-12-18 16:57:01,871 - INFO - Successfully initialize the robot -2024-12-18 16:57:01,873 - INFO - Current end effector pose: -2024-12-18 16:57:01,873 - INFO - Current joint values: [0.6159987449645996, -0.44486019015312195, -0.8602851629257202, 1.8543468713760376, -0.9889612197875977, -1.6474781036376953] -2024-12-18 16:57:01,874 - INFO - Converted joint values [0.6159987449645996, -0.44486019015312195, -0.8602851629257202, 1.8543468713760376, -0.9889612197875977, -1.6474781036376953] to Cartesian Pose [0.27857261 0.05274606 0.50121839] and [ 0.81991375 -0.11245792 0.53797986 0.16022588]. -2024-12-18 16:57:01,930 - INFO - Moving arm to joint values: [ 0.59963698 -0.45160961 -0.84538244 1.82741974 -0.97790559 -1.61278117] -2024-12-18 16:57:03,697 - INFO - Checking for faults on the robot... -2024-12-18 16:57:06,701 - INFO - Successfully initialize the robot -2024-12-18 16:57:06,703 - INFO - Current end effector pose: -2024-12-18 16:57:06,704 - INFO - Current joint values: [0.5996369123458862, -0.4516077935695648, -0.8453863859176636, 1.8274178504943848, -0.9779108166694641, -1.6127833127975464] -2024-12-18 16:57:06,706 - INFO - Converted joint values [0.5996369123458862, -0.4516077935695648, -0.8453863859176636, 1.8274178504943848, -0.9779108166694641, -1.6127833127975464] to Cartesian Pose [0.27986468 0.04730814 0.50068285] and [ 0.81378055 -0.12095384 0.54576366 0.15897616]. -2024-12-18 16:57:06,779 - INFO - Moving arm to joint values: [ 0.5829297 -0.45818858 -0.83079205 1.79989237 -0.96683475 -1.57787748] -2024-12-18 16:58:03,489 - INFO - Checking for faults on the robot... -2024-12-18 16:58:06,493 - INFO - Successfully initialize the robot -2024-12-18 16:58:06,495 - INFO - Current end effector pose: -2024-12-18 16:58:06,496 - INFO - Current joint values: [-0.027926120907068253, -0.47473061084747314, -0.038393620401620865, -0.022691410034894943, -1.0576778650283813, 0.0034878887236118317] -2024-12-18 16:58:06,498 - INFO - Converted joint values [-0.027926120907068253, -0.47473061084747314, -0.038393620401620865, -0.022691410034894943, -1.0576778650283813, 0.0034878887236118317] to Cartesian Pose [ 0.25014395 -0.004222 0.36013246] and [ 7.07143685e-01 -1.79167388e-04 7.07047746e-01 5.59121898e-03]. -2024-12-18 16:58:06,563 - INFO - Moving arm to joint values: [-0.04150686 -0.37307685 -0.10900079 -0.04580389 -1.08914953 0.02123348] -2024-12-18 16:58:21,794 - INFO - Checking for faults on the robot... -2024-12-18 16:58:24,798 - INFO - Successfully initialize the robot -2024-12-18 16:58:24,800 - INFO - Current end effector pose: -2024-12-18 16:58:24,802 - INFO - Current joint values: [-0.027920367196202278, -0.47473061084747314, -0.03839937224984169, -0.02268374152481556, -1.057664394378662, 0.003482136409729719] -2024-12-18 16:58:24,803 - INFO - Converted joint values [-0.027920367196202278, -0.47473061084747314, -0.03839937224984169, -0.02268374152481556, -1.057664394378662, 0.003482136409729719] to Cartesian Pose [ 0.25014605 -0.00422155 0.36013308] and [ 7.07146388e-01 -1.78805511e-04 7.07045034e-01 5.59232149e-03]. -2024-12-18 16:58:24,864 - INFO - Moving arm to joint values: [-0.04150686 -0.37307685 -0.10900079 -0.04580389 -1.08914953 0.02123348] -2024-12-18 17:01:19,020 - INFO - Checking for faults on the robot... -2024-12-18 17:01:22,024 - INFO - Successfully initialize the robot -2024-12-18 17:01:22,026 - INFO - Current end effector pose: -2024-12-18 17:01:22,027 - INFO - Current joint values: [-0.04150952026247978, -0.3730756342411041, -0.10900275409221649, -0.04580850154161453, -1.0891531705856323, 0.021232211962342262] -2024-12-18 17:01:22,029 - INFO - Converted joint values [-0.04150952026247978, -0.3730756342411041, -0.10900275409221649, -0.04580850154161453, -1.0891531705856323, 0.021232211962342262] to Cartesian Pose [ 0.2704345 -0.00567822 0.35987674] and [ 7.07105303e-01 -3.23751621e-04 7.07108110e-01 3.26184172e-04]. -2024-12-18 17:01:22,087 - INFO - Moving arm to joint values: [-0.05037699 -0.28696352 -0.17889097 -0.06315169 -1.10506539 0.03647968] -2024-12-18 17:03:24,234 - INFO - Checking for faults on the robot... -2024-12-18 17:03:27,237 - INFO - Successfully initialize the robot -2024-12-18 17:03:27,239 - INFO - Current end effector pose: -2024-12-18 17:03:27,240 - INFO - Current joint values: [-0.027920367196202278, -0.47473061084747314, -0.03839937224984169, -0.02268374152481556, -1.057664394378662, 0.0034840537700802088] -2024-12-18 17:03:27,242 - INFO - Converted joint values [-0.027920367196202278, -0.47473061084747314, -0.03839937224984169, -0.02268374152481556, -1.057664394378662, 0.0034840537700802088] to Cartesian Pose [ 0.25014605 -0.00422155 0.36013308] and [ 7.07146388e-01 -1.79483438e-04 7.07045039e-01 5.59164366e-03]. -2024-12-18 17:03:27,304 - INFO - Moving arm to joint values: [-0.04150686 -0.37307685 -0.10900079 -0.04580389 -1.08914953 0.02123348] -2024-12-18 17:08:03,237 - INFO - Checking for faults on the robot... -2024-12-18 17:08:06,240 - INFO - Successfully initialize the robot -2024-12-18 17:08:06,242 - INFO - Current end effector pose: -2024-12-18 17:08:06,243 - INFO - Current joint values: [-0.027922285720705986, -0.47473445534706116, -0.03839745745062828, -0.022685658186674118, -1.0576701164245605, 0.003482136409729719] -2024-12-18 17:08:06,245 - INFO - Converted joint values [-0.027922285720705986, -0.47473445534706116, -0.03839745745062828, -0.022685658186674118, -1.0576701164245605, 0.003482136409729719] to Cartesian Pose [ 0.2501449 -0.00422176 0.36013396] and [ 7.07143690e-01 -1.78524181e-04 7.07047729e-01 5.59275430e-03]. -2024-12-18 17:08:06,314 - INFO - Moving arm to joint values: [-0.04150686 -0.37307685 -0.10900079 -0.04580389 -1.08914953 0.02123348] -2024-12-18 17:08:51,120 - INFO - Checking for faults on the robot... -2024-12-18 17:08:54,124 - INFO - Successfully initialize the robot -2024-12-18 17:08:54,126 - INFO - Current end effector pose: -2024-12-18 17:08:54,127 - INFO - Current joint values: [-0.041507601737976074, -0.37307754158973694, -0.1090046763420105, -0.04580274969339371, -1.0891531705856323, 0.021230293437838554] -2024-12-18 17:08:54,129 - INFO - Converted joint values [-0.041507601737976074, -0.37307754158973694, -0.1090046763420105, -0.04580274969339371, -1.0891531705856323, 0.021230293437838554] to Cartesian Pose [ 0.2704347 -0.00567841 0.35987789] and [ 7.07103911e-01 -3.25083301e-04 7.07109501e-01 3.27099538e-04]. -2024-12-18 17:08:54,194 - INFO - Moving arm to joint values: [-0.05037576 -0.28696443 -0.17888977 -0.06315035 -1.10506454 0.03647797] -2024-12-18 17:09:02,047 - INFO - Checking for faults on the robot... -2024-12-18 17:09:05,050 - INFO - Successfully initialize the robot -2024-12-18 17:09:05,051 - INFO - Current end effector pose: -2024-12-18 17:09:05,051 - INFO - Current joint values: [-0.027920367196202278, -0.47473061084747314, -0.03839937224984169, -0.022685658186674118, -1.0576740503311157, 0.0034840537700802088] -2024-12-18 17:09:05,053 - INFO - Converted joint values [-0.027920367196202278, -0.47473061084747314, -0.03839937224984169, -0.022685658186674118, -1.0576740503311157, 0.0034840537700802088] to Cartesian Pose [ 0.25014532 -0.0042213 0.36013401] and [ 7.07142981e-01 -1.78521585e-04 7.07048449e-01 5.59137485e-03]. -2024-12-18 17:09:05,106 - INFO - Moving arm to joint values: [-0.04150686 -0.37307685 -0.10900079 -0.04580389 -1.08914953 0.02123348] -2024-12-18 17:11:18,559 - INFO - Checking for faults on the robot... -2024-12-18 17:11:21,562 - INFO - Successfully initialize the robot -2024-12-18 17:11:21,564 - INFO - Current end effector pose: -2024-12-18 17:11:21,565 - INFO - Current joint values: [-0.027924202382564545, -0.47473061084747314, -0.03839745745062828, -0.022685658186674118, -1.057666301727295, 0.0034840537700802088] -2024-12-18 17:11:21,567 - INFO - Converted joint values [-0.027924202382564545, -0.47473061084747314, -0.03839745745062828, -0.022685658186674118, -1.057666301727295, 0.0034840537700802088] to Cartesian Pose [ 0.25014554 -0.00422226 0.36013262] and [ 7.07146397e-01 -1.79922350e-04 7.07045022e-01 5.59272081e-03]. -2024-12-18 17:11:21,627 - INFO - Moving arm to joint values: [-0.04150686 -0.37307685 -0.10900079 -0.04580389 -1.08914953 0.02123348] -2024-12-18 17:12:42,354 - INFO - Checking for faults on the robot... -2024-12-18 17:12:45,359 - INFO - Successfully initialize the robot -2024-12-18 17:12:45,361 - INFO - Current end effector pose: -2024-12-18 17:12:45,362 - INFO - Current joint values: [-0.04150952026247978, -0.37307947874069214, -0.10900275409221649, -0.04580466449260712, -1.0891493558883667, 0.021232211962342262] -2024-12-18 17:12:45,424 - INFO - Moving arm to joint values: [-0.05037898 -0.28696336 -0.17889121 -0.06315503 -1.10506556 0.0364812 ] -2024-12-18 17:13:32,940 - INFO - Checking for faults on the robot... -2024-12-18 17:13:35,943 - INFO - Successfully initialize the robot -2024-12-18 17:13:35,945 - INFO - Current end effector pose: -2024-12-18 17:13:35,946 - INFO - Current joint values: [-0.027926120907068253, -0.47473061084747314, -0.03839745745062828, -0.022691410034894943, -1.0576720237731934, 0.0034840537700802088] -2024-12-18 17:13:36,013 - INFO - Moving arm to joint values: [-0.04150686 -0.37307685 -0.10900079 -0.04580389 -1.08914953 0.02123348] -2024-12-18 17:20:06,589 - INFO - Checking for faults on the robot... -2024-12-18 17:20:09,593 - INFO - Successfully initialize the robot -2024-12-18 17:20:09,595 - INFO - Current end effector pose: -2024-12-18 17:20:09,596 - INFO - Current joint values: [-0.027922285720705986, -0.47473445534706116, -0.03839745745062828, -0.022689493373036385, -1.057666301727295, 0.003482136409729719] -2024-12-18 17:20:09,651 - INFO - Moving arm to joint values: [-0.04046228 -0.37310569 -0.10897496 -0.04462554 -1.08912463 0.02068773] -2024-12-18 17:20:42,784 - INFO - Checking for faults on the robot... -2024-12-18 17:20:45,788 - INFO - Successfully initialize the robot -2024-12-18 17:20:45,790 - INFO - Current end effector pose: -2024-12-18 17:20:45,791 - INFO - Current joint values: [-0.027922285720705986, -0.47473061084747314, -0.03839937224984169, -0.02268374152481556, -1.0576740503311157, 0.0034840537700802088] -2024-12-18 17:20:45,842 - INFO - Moving arm to joint values: [-0.04046228 -0.37310569 -0.10897496 -0.04462554 -1.08912463 0.02068773] -2024-12-18 17:21:59,689 - INFO - Checking for faults on the robot... -2024-12-18 17:22:02,692 - INFO - Successfully initialize the robot -2024-12-18 17:22:02,694 - INFO - Current end effector pose: -2024-12-18 17:22:02,695 - INFO - Current joint values: [-0.027924202382564545, -0.47473061084747314, -0.03839937224984169, -0.022691410034894943, -1.0576740503311157, 0.003485971363261342] -2024-12-18 17:22:02,761 - INFO - Moving arm to joint values: [-0.04150686 -0.37307685 -0.10900079 -0.04580389 -1.08914953 0.02123348] -2024-12-18 17:22:50,097 - INFO - Checking for faults on the robot... -2024-12-18 17:22:53,101 - INFO - Successfully initialize the robot -2024-12-18 17:22:53,103 - INFO - Current end effector pose: -2024-12-18 17:22:53,104 - INFO - Current joint values: [-0.04150952026247978, -0.3730756342411041, -0.1090046763420105, -0.04580466449260712, -1.0891493558883667, 0.021228376775979996] -2024-12-18 17:22:53,171 - INFO - Moving arm to joint values: [-0.0494794 -0.28699423 -0.17885989 -0.06214749 -1.10504035 0.03602928] -2024-12-18 17:22:59,000 - INFO - Checking for faults on the robot... -2024-12-18 17:23:02,003 - INFO - Successfully initialize the robot -2024-12-18 17:23:02,005 - INFO - Current end effector pose: -2024-12-18 17:23:02,006 - INFO - Current joint values: [-0.027922285720705986, -0.47472870349884033, -0.03839937224984169, -0.022685658186674118, -1.0576740503311157, 0.0034840537700802088] -2024-12-18 17:23:02,071 - INFO - Moving arm to joint values: [-0.04046228 -0.37310569 -0.10897496 -0.04462554 -1.08912463 0.02068773] -2024-12-18 17:23:39,092 - INFO - Checking for faults on the robot... -2024-12-18 17:23:42,096 - INFO - Successfully initialize the robot -2024-12-18 17:23:42,098 - INFO - Current end effector pose: -2024-12-18 17:23:42,099 - INFO - Current joint values: [-0.027920367196202278, -0.47473636269569397, -0.03839937224984169, -0.02268374152481556, -1.057664394378662, 0.0034840537700802088] -2024-12-18 17:23:42,180 - INFO - Moving arm to joint values: [-0.04150686 -0.37307685 -0.10900079 -0.04580389 -1.08914953 0.02123348] -2024-12-18 17:24:01,186 - INFO - Checking for faults on the robot... -2024-12-18 17:24:04,190 - INFO - Successfully initialize the robot -2024-12-18 17:24:04,192 - INFO - Current end effector pose: -2024-12-18 17:24:04,193 - INFO - Current joint values: [-0.027922285720705986, -0.47472870349884033, -0.03839937224984169, -0.022687576711177826, -1.0576701164245605, 0.0034840537700802088] -2024-12-18 17:24:04,245 - INFO - Moving arm to joint values: [-0.0425513 -0.37304728 -0.10902728 -0.04698205 -1.08917507 0.02177911] -2024-12-18 17:36:09,224 - INFO - Checking for faults on the robot... -2024-12-18 17:36:12,228 - INFO - Successfully initialize the robot -2024-12-18 17:36:12,230 - INFO - Current end effector pose: -2024-12-18 17:36:12,231 - INFO - Current joint values: [-0.28328216075897217, -0.03905706852674484, -0.7451618313789368, -0.6515238285064697, -0.9098423719406128, 0.4350043535232544] -2024-12-18 17:36:12,300 - INFO - Moving arm to joint values: [-0.25766706 0.01593008 -0.80556114 -0.63292942 -0.88808863 0.43347777] -2024-12-18 17:36:42,951 - INFO - Checking for faults on the robot... -2024-12-18 17:36:45,954 - INFO - Successfully initialize the robot -2024-12-18 17:36:45,956 - INFO - Current end effector pose: -2024-12-18 17:36:45,957 - INFO - Current joint values: [-0.4824925661087036, -0.004425534512847662, -0.9801389575004578, -1.1519774198532104, -1.0745344161987305, 0.848483145236969] -2024-12-18 17:36:46,325 - INFO - Moving arm to joint values: [-0.44317461 0.03085386 -1.01770369 -1.13326286 -1.03301439 0.85269725] -2024-12-18 17:39:47,217 - INFO - Checking for faults on the robot... -2024-12-18 17:39:50,221 - INFO - Successfully initialize the robot -2024-12-18 17:39:50,223 - INFO - Current end effector pose: -2024-12-18 17:39:50,224 - INFO - Current joint values: [-0.027922285720705986, -0.47473636269569397, -0.038393620401620865, -0.022689493373036385, -1.0576778650283813, 0.0034840537700802088] -2024-12-18 17:39:50,282 - INFO - Moving arm to joint values: [-0.04150686 -0.37307685 -0.10900079 -0.04580389 -1.08914953 0.02123348] -2024-12-18 17:41:08,160 - INFO - Checking for faults on the robot... -2024-12-18 17:41:11,163 - INFO - Successfully initialize the robot -2024-12-18 17:41:11,164 - INFO - Current end effector pose: -2024-12-18 17:41:11,164 - INFO - Current joint values: [-0.027926120907068253, -0.47473061084747314, -0.038393620401620865, -0.022689493373036385, -1.0576701164245605, 0.0034840537700802088] -2024-12-18 17:41:11,515 - INFO - Moving arm to joint values: [-0.04150701 -0.37306356 -0.10899858 -0.04580481 -1.08916503 0.02123328] -2024-12-18 17:43:16,081 - INFO - Checking for faults on the robot... -2024-12-18 17:43:19,085 - INFO - Successfully initialize the robot -2024-12-18 17:43:19,085 - INFO - Current end effector pose: -2024-12-18 17:43:19,086 - INFO - Current joint values: [-0.027922285720705986, -0.47473636269569397, -0.03839745745062828, -0.022687576711177826, -1.057666301727295, 0.0034840537700802088] -2024-12-18 17:43:19,134 - INFO - Moving arm to joint values: [-0.04983681 -0.47098664 -0.03980306 -0.05606362 -1.06067688 0.02740035] -2024-12-18 17:43:31,190 - INFO - Checking for faults on the robot... -2024-12-18 17:43:34,194 - INFO - Successfully initialize the robot -2024-12-18 17:43:34,195 - INFO - Current end effector pose: -2024-12-18 17:43:34,195 - INFO - Current joint values: [-0.027920367196202278, -0.47473061084747314, -0.03839937224984169, -0.02268374152481556, -1.057664394378662, 0.003482136409729719] -2024-12-18 17:43:34,244 - INFO - Moving arm to joint values: [-0.04983681 -0.47098664 -0.03980306 -0.05606362 -1.06067688 0.02740035] -2024-12-18 17:44:03,601 - INFO - Checking for faults on the robot... -2024-12-18 17:44:06,604 - INFO - Successfully initialize the robot -2024-12-18 17:44:06,605 - INFO - Current end effector pose: -2024-12-18 17:44:06,605 - INFO - Current joint values: [-0.304119348526001, -0.20815928280353546, -0.6484577655792236, -0.6779735088348389, -0.8492079377174377, 0.4868184030056] -2024-12-18 17:44:06,956 - INFO - Moving arm to joint values: [-0.30850413 -0.20789832 -0.64596958 -0.69104339 -0.84686527 0.50116539] -2024-12-18 17:46:28,433 - INFO - Checking for faults on the robot... -2024-12-18 17:46:31,437 - INFO - Successfully initialize the robot -2024-12-18 17:46:31,438 - INFO - Current end effector pose: -2024-12-18 17:46:31,438 - INFO - Current joint values: [0.6209592819213867, -0.5777067542076111, -0.8901441097259521, -0.5743895173072815, 0.8544675707817078, 0.8672053813934326] -2024-12-18 17:46:31,795 - INFO - Moving arm to joint values: [ 0.78174218 -0.10175415 -1.48666094 2.51729017 -1.05037598 -2.19329855] -2024-12-18 17:46:57,555 - INFO - Checking for faults on the robot... -2024-12-18 17:47:00,558 - INFO - Successfully initialize the robot -2024-12-18 17:47:00,559 - INFO - Current end effector pose: -2024-12-18 17:47:00,559 - INFO - Current joint values: [0.8922533392906189, 0.14861972630023956, -0.6966497302055359, 0.391579270362854, -0.17768099904060364, -0.3300743103027344] -2024-12-18 17:47:00,713 - INFO - Moving arm to joint values: [ 0.86560539 0.15387242 -0.68913773 0.26579369 -0.18926605 -0.22577908] -2024-12-18 17:47:29,359 - INFO - Checking for faults on the robot... -2024-12-18 17:47:32,363 - INFO - Successfully initialize the robot -2024-12-18 17:47:32,363 - INFO - Current end effector pose: -2024-12-18 17:47:32,364 - INFO - Current joint values: [-0.44624075293540955, -0.050492893904447556, -1.2351939678192139, -1.9963475465774536, -0.5926649570465088, 1.7032344341278076] -2024-12-18 17:47:32,717 - INFO - Moving arm to joint values: [-0.45202711 -0.05292395 -1.22189663 -2.00784815 -0.58092481 1.71558292] -2024-12-18 17:47:53,225 - INFO - Checking for faults on the robot... -2024-12-18 17:47:56,229 - INFO - Successfully initialize the robot -2024-12-18 17:47:56,230 - INFO - Current end effector pose: -2024-12-18 17:47:56,230 - INFO - Current joint values: [-0.5194442272186279, 0.04593697190284729, -1.2915294170379639, -1.9207030534744263, -0.7628237009048462, 2.8348021507263184] -2024-12-18 17:47:56,587 - INFO - Moving arm to joint values: [-0.20058717 -0.28596695 -0.90354865 1.23583557 0.45019349 -0.18732727] -2024-12-18 17:48:07,660 - INFO - Checking for faults on the robot... -2024-12-18 17:48:10,662 - INFO - Successfully initialize the robot -2024-12-18 17:48:10,663 - INFO - Current end effector pose: -2024-12-18 17:48:10,664 - INFO - Current joint values: [-0.2005852460861206, -0.2859666049480438, -0.9035511016845703, 1.2358362674713135, 0.4501945972442627, -0.1873316466808319] -2024-12-18 17:48:10,721 - INFO - Moving arm to joint values: [-0.20524228 -0.28121923 -0.90903472 1.24630233 0.44700989 -0.20543593] -2024-12-18 17:48:15,845 - INFO - Checking for faults on the robot... -2024-12-18 17:48:18,848 - INFO - Successfully initialize the robot -2024-12-18 17:48:18,849 - INFO - Current end effector pose: -2024-12-18 17:48:18,849 - INFO - Current joint values: [-0.20524662733078003, -0.28122279047966003, -0.9090331792831421, 1.2463018894195557, 0.44700199365615845, -0.20543837547302246] -2024-12-18 17:48:18,905 - INFO - Moving arm to joint values: [-0.21001787 -0.27644871 -0.91464805 1.25692719 0.44397547 -0.22369523] -2024-12-18 17:48:32,830 - INFO - Checking for faults on the robot... -2024-12-18 17:48:35,833 - INFO - Successfully initialize the robot -2024-12-18 17:48:35,834 - INFO - Current end effector pose: -2024-12-18 17:48:35,834 - INFO - Current joint values: [-0.21001730859279633, -0.2764482796192169, -0.914649486541748, 1.2569266557693481, 0.4439723789691925, -0.22369849681854248] -2024-12-18 17:48:35,887 - INFO - Moving arm to joint values: [-0.217715 -0.27792241 -0.9165143 1.2635044 0.44428207 -0.238592 ] -2024-12-18 17:48:46,501 - INFO - Checking for faults on the robot... -2024-12-18 17:48:49,504 - INFO - Successfully initialize the robot -2024-12-18 17:48:49,505 - INFO - Current end effector pose: -2024-12-18 17:48:49,505 - INFO - Current joint values: [-0.21771980822086334, -0.27792665362358093, -0.9165132641792297, 1.2634978294372559, 0.4442753493785858, -0.2385953664779663] -2024-12-18 17:48:49,561 - INFO - Moving arm to joint values: [-0.22270591 -0.27310809 -0.92231641 1.2742957 0.44149997 -0.25702508] -2024-12-18 17:49:02,924 - INFO - Checking for faults on the robot... -2024-12-18 17:49:05,927 - INFO - Successfully initialize the robot -2024-12-18 17:49:05,928 - INFO - Current end effector pose: -2024-12-18 17:49:05,928 - INFO - Current joint values: [0.15631839632987976, 0.09691306948661804, -1.2473909854888916, 1.0285283327102661, 0.09583161771297455, 0.14001217484474182] -2024-12-18 17:49:05,977 - INFO - Moving arm to joint values: [ 0.15362745 0.10221369 -1.25871881 1.05996707 0.09302904 0.10540742] -2024-12-18 17:49:10,270 - INFO - Checking for faults on the robot... -2024-12-18 17:49:13,274 - INFO - Successfully initialize the robot -2024-12-18 17:49:13,274 - INFO - Current end effector pose: -2024-12-18 17:49:13,275 - INFO - Current joint values: [0.15362434089183807, 0.10220914334058762, -1.2587194442749023, 1.0599615573883057, 0.09301868081092834, 0.10540173947811127] -2024-12-18 17:49:13,325 - INFO - Moving arm to joint values: [ 0.15065801 0.10757326 -1.27050894 1.09204042 0.09041081 0.0700941 ] -2024-12-18 17:49:15,696 - INFO - Checking for faults on the robot... -2024-12-18 17:49:18,700 - INFO - Successfully initialize the robot -2024-12-18 17:49:18,701 - INFO - Current end effector pose: -2024-12-18 17:49:18,701 - INFO - Current joint values: [0.1506580114364624, 0.10756848752498627, -1.270508050918579, 1.092035174369812, 0.09041091054677963, 0.07008758187294006] -2024-12-18 17:49:18,756 - INFO - Moving arm to joint values: [ 0.14739752 0.1129905 -1.28274663 1.12471126 0.08798822 0.03410787] -2024-12-18 17:49:27,277 - INFO - Checking for faults on the robot... -2024-12-18 17:49:30,281 - INFO - Successfully initialize the robot -2024-12-18 17:49:30,281 - INFO - Current end effector pose: -2024-12-18 17:49:30,282 - INFO - Current joint values: [0.14739254117012024, 0.11298343539237976, -1.282751202583313, 1.1247090101242065, 0.08798146992921829, 0.034104228019714355] -2024-12-18 17:49:30,336 - INFO - Moving arm to joint values: [ 0.14382989 0.11846045 -1.29541483 1.15792432 0.08576044 -0.00249958] -2024-12-18 17:49:37,044 - INFO - Checking for faults on the robot... -2024-12-18 17:49:40,047 - INFO - Successfully initialize the robot -2024-12-18 17:49:40,048 - INFO - Current end effector pose: -2024-12-18 17:49:40,048 - INFO - Current joint values: [0.14382220804691315, 0.11846166849136353, -1.2954161167144775, 1.1579272747039795, 0.08574952930212021, -0.0025042237248271704] -2024-12-18 17:49:40,098 - INFO - Moving arm to joint values: [ 0.13993424 0.12398452 -1.30850123 1.19163928 0.08371969 -0.03968941] -2024-12-18 17:49:49,556 - INFO - Checking for faults on the robot... -2024-12-18 17:49:52,560 - INFO - Successfully initialize the robot -2024-12-18 17:49:52,561 - INFO - Current end effector pose: -2024-12-18 17:49:52,561 - INFO - Current joint values: [0.13992589712142944, 0.12398399412631989, -1.308506727218628, 1.1916403770446777, 0.08371508121490479, -0.03969558700919151] -2024-12-18 17:49:52,613 - INFO - Moving arm to joint values: [ 0.13341854 0.09541533 -1.28931201 1.21709916 0.08436061 -0.06951949] -2024-12-18 17:50:01,896 - INFO - Checking for faults on the robot... -2024-12-18 17:50:04,899 - INFO - Successfully initialize the robot -2024-12-18 17:50:04,900 - INFO - Current end effector pose: -2024-12-18 17:50:04,900 - INFO - Current joint values: [0.1334160566329956, 0.09540785104036331, -1.2893089056015015, 1.2171006202697754, 0.08435551822185516, -0.06952192634344101] -2024-12-18 17:50:04,954 - INFO - Moving arm to joint values: [ 0.12778593 0.08392466 -1.28662287 1.24705149 0.08393361 -0.10351617] -2024-12-18 17:50:16,827 - INFO - Checking for faults on the robot... -2024-12-18 17:50:19,830 - INFO - Successfully initialize the robot -2024-12-18 17:50:19,831 - INFO - Current end effector pose: -2024-12-18 17:50:19,831 - INFO - Current joint values: [0.12778443098068237, 0.08392217010259628, -1.2866244316101074, 1.247047781944275, 0.08393367379903793, -0.10351686179637909] -2024-12-18 17:50:19,885 - INFO - Moving arm to joint values: [ 0.12302339 0.08954584 -1.30051414 1.28230125 0.08241507 -0.14248518] -2024-12-18 17:50:36,379 - INFO - Checking for faults on the robot... -2024-12-18 17:50:39,383 - INFO - Successfully initialize the robot -2024-12-18 17:50:39,384 - INFO - Current end effector pose: -2024-12-18 17:50:39,384 - INFO - Current joint values: [0.12301567196846008, 0.08954229205846786, -1.30051851272583, 1.2822986841201782, 0.0824054479598999, -0.14249147474765778] -2024-12-18 17:50:39,436 - INFO - Moving arm to joint values: [ 0.11828983 0.10203954 -1.32136368 1.31938294 0.08065141 -0.18322646] -2024-12-18 17:50:52,964 - INFO - Checking for faults on the robot... -2024-12-18 17:50:55,968 - INFO - Successfully initialize the robot -2024-12-18 17:50:55,969 - INFO - Current end effector pose: -2024-12-18 17:50:55,969 - INFO - Current joint values: [0.11828909069299698, 0.10203848779201508, -1.3213653564453125, 1.319376826286316, 0.08064328879117966, -0.1832263320684433] -2024-12-18 17:50:56,023 - INFO - Moving arm to joint values: [ 0.11330268 0.11799715 -1.34597013 1.35677606 0.07893131 -0.22430512] -2024-12-18 17:51:15,427 - INFO - Checking for faults on the robot... -2024-12-18 17:51:18,430 - INFO - Successfully initialize the robot -2024-12-18 17:51:18,431 - INFO - Current end effector pose: -2024-12-18 17:51:18,431 - INFO - Current joint values: [0.43319618701934814, -0.5327189564704895, -0.7772431373596191, 2.1834797859191895, -0.6401167511940002, -1.2369810342788696] -2024-12-18 17:51:18,786 - INFO - Moving arm to joint values: [ 0.42127368 -0.53691912 -0.76062479 2.13942845 -0.62793763 -1.18875835] -2024-12-18 17:51:35,137 - INFO - Checking for faults on the robot... -2024-12-18 17:51:38,141 - INFO - Successfully initialize the robot -2024-12-18 17:51:38,142 - INFO - Current end effector pose: -2024-12-18 17:51:38,142 - INFO - Current joint values: [0.421267569065094, -0.5369182229042053, -0.760624349117279, 2.139425754547119, -0.6279369592666626, -1.1887621879577637] -2024-12-18 17:51:38,197 - INFO - Moving arm to joint values: [ 0.40891928 -0.54098801 -0.7440127 2.09377054 -0.6159024 -1.13928423] -2024-12-18 17:51:58,407 - INFO - Checking for faults on the robot... -2024-12-18 17:52:01,411 - INFO - Successfully initialize the robot -2024-12-18 17:52:01,412 - INFO - Current end effector pose: -2024-12-18 17:52:01,412 - INFO - Current joint values: [0.4089113473892212, -0.5409947633743286, -0.7440113425254822, 2.0937647819519043, -0.6158971190452576, -1.1392855644226074] -2024-12-18 17:52:01,465 - INFO - Moving arm to joint values: [ 0.39609739 -0.54493152 -0.72738504 2.04640678 -0.60401727 -1.08846185] -2024-12-18 17:52:04,038 - INFO - Checking for faults on the robot... -2024-12-18 17:52:07,042 - INFO - Successfully initialize the robot -2024-12-18 17:52:07,042 - INFO - Current end effector pose: -2024-12-18 17:52:07,043 - INFO - Current joint values: [0.396091103553772, -0.5449294447898865, -0.727390706539154, 2.0464069843292236, -0.6040241122245789, -1.0884648561477661] -2024-12-18 17:52:07,096 - INFO - Moving arm to joint values: [ 0.38277689 -0.54874779 -0.7107292 1.99723009 -0.59229567 -1.03619923] -2024-12-18 17:57:44,137 - INFO - Checking for faults on the robot... -2024-12-18 17:57:47,140 - INFO - Successfully initialize the robot -2024-12-18 17:57:47,141 - INFO - Current end effector pose: -2024-12-18 17:57:47,141 - INFO - Current joint values: [0.3827703893184662, -0.5487509369850159, -0.7107259035110474, 1.9972257614135742, -0.5922929644584656, -1.0361963510513306] -2024-12-18 17:57:47,194 - INFO - Moving arm to joint values: [ 0.36890599 -0.55244647 -0.69402382 1.9461187 -0.58073648 -0.9823948 ] -2024-12-18 17:58:00,202 - INFO - Checking for faults on the robot... -2024-12-18 17:58:03,205 - INFO - Successfully initialize the robot -2024-12-18 17:58:03,206 - INFO - Current end effector pose: -2024-12-18 17:58:03,206 - INFO - Current joint values: [-0.02792995609343052, -0.47473254799842834, -0.03839937224984169, -0.022697163745760918, -1.0576778650283813, 0.0034878887236118317] -2024-12-18 17:58:03,258 - INFO - Moving arm to joint values: [-0.04983681 -0.47098664 -0.03980306 -0.05606362 -1.06067688 0.02740035] -2024-12-18 17:58:06,583 - INFO - Checking for faults on the robot... -2024-12-18 17:58:09,587 - INFO - Successfully initialize the robot -2024-12-18 17:58:09,588 - INFO - Current end effector pose: -2024-12-18 17:58:09,588 - INFO - Current joint values: [-0.04983711987733841, -0.4709915518760681, -0.03980104997754097, -0.056068915873765945, -1.060680627822876, 0.027396896854043007] -2024-12-18 17:58:09,637 - INFO - Moving arm to joint values: [-0.07088566 -0.46821012 -0.04111678 -0.08834501 -1.06229194 0.05072978] -2024-12-18 17:59:53,811 - INFO - Checking for faults on the robot... -2024-12-18 17:59:56,814 - INFO - Successfully initialize the robot -2024-12-18 17:59:56,815 - INFO - Current end effector pose: -2024-12-18 17:59:56,815 - INFO - Current joint values: [-0.07088908553123474, -0.46820929646492004, -0.041120272129774094, -0.08834770321846008, -1.0622950792312622, 0.05073066055774689] -2024-12-18 17:59:56,815 - INFO - Converted joint values [-0.07088908553123474, -0.46820929646492004, -0.041120272129774094, -0.08834770321846008, -1.0622950792312622, 0.05073066055774689] to Cartesian Pose [ 0.25066787 -0.00703155 0.35963558] and [ 7.07401113e-01 -5.09030242e-04 7.06794962e-01 -4.92821815e-03]. -2024-12-18 17:59:56,866 - INFO - Moving arm to joint values: [-0.09110488 -0.46545679 -0.04240048 -0.11957486 -1.0638388 0.0734598 ] -2024-12-18 18:00:50,621 - INFO - Checking for faults on the robot... -2024-12-18 18:00:53,624 - INFO - Successfully initialize the robot -2024-12-18 18:00:53,625 - INFO - Current end effector pose: -2024-12-18 18:00:53,625 - INFO - Current joint values: [-0.027920367196202278, -0.47473061084747314, -0.03839937224984169, -0.022685658186674118, -1.057664394378662, 0.0034840537700802088] -2024-12-18 18:00:53,625 - INFO - Converted joint values [-0.027920367196202278, -0.47473061084747314, -0.03839937224984169, -0.022685658186674118, -1.057664394378662, 0.0034840537700802088] to Cartesian Pose [ 0.25014605 -0.00422131 0.36013308] and [ 7.07146395e-01 -1.78560461e-04 7.07045035e-01 5.59138589e-03]. -2024-12-18 18:00:53,678 - INFO - Moving arm to joint values: [-0.04999365 -0.47258667 -0.03875973 -0.0562608 -1.06012536 0.02752394] -2024-12-18 18:01:26,871 - INFO - Checking for faults on the robot... -2024-12-18 18:01:29,875 - INFO - Successfully initialize the robot -2024-12-18 18:01:29,876 - INFO - Current end effector pose: -2024-12-18 18:01:29,876 - INFO - Current joint values: [-0.019567841663956642, -0.5318924784660339, -0.4564666748046875, 0.0016988837160170078, 0.3419454097747803, 0.0018024274613708258] -2024-12-18 18:01:29,877 - INFO - Converted joint values [-0.019567841663956642, -0.5318924784660339, -0.4564666748046875, 0.0016988837160170078, 0.3419454097747803, 0.0018024274613708258] to Cartesian Pose [ 0.34950472 -0.00690589 0.38400231] and [ 0.94816015 -0.0108 0.317601 0.00230044]. -2024-12-18 18:01:30,230 - INFO - Moving arm to joint values: [-0.0232185 -0.5289069 -0.44926211 0.0006699 0.33173039 -0.0035986 ] -2024-12-18 18:33:09,086 - INFO - Checking for faults on the robot... -2024-12-18 18:33:12,089 - INFO - Successfully initialize the robot -2024-12-18 18:33:12,092 - INFO - Current end effector pose: -2024-12-18 18:33:12,092 - INFO - Current joint values: [-0.02792803756892681, -0.47473061084747314, -0.038393620401620865, -0.02269524522125721, -1.0576740503311157, 0.0034917236771434546] -2024-12-18 18:33:12,158 - INFO - Moving arm to joint values: [-0.04999365 -0.47258667 -0.03875973 -0.0562608 -1.06012536 0.02752394] -2024-12-18 18:33:14,141 - INFO - Checking for faults on the robot... -2024-12-18 18:33:17,144 - INFO - Successfully initialize the robot -2024-12-18 18:33:17,146 - INFO - Current end effector pose: -2024-12-18 18:33:17,147 - INFO - Current joint values: [-0.04998859763145447, -0.47259455919265747, -0.038757942616939545, -0.05625874549150467, -1.0601226091384888, 0.02751769870519638] -2024-12-18 18:33:17,213 - INFO - Moving arm to joint values: [-0.07134971 -0.4713872 -0.03903698 -0.08893489 -1.06121621 0.0510933 ] -2024-12-18 18:46:15,766 - INFO - Checking for faults on the robot... -2024-12-18 18:46:18,770 - INFO - Successfully initialize the robot -2024-12-18 18:46:18,773 - INFO - Current end effector pose: -2024-12-18 18:46:18,774 - INFO - Current joint values: [-0.07135311514139175, -0.47138655185699463, -0.039041727781295776, -0.08894212543964386, -1.0612213611602783, 0.05109114944934845] -2024-12-18 18:46:18,775 - INFO - Current joint values: [-0.07135311514139175, -0.47138655185699463, -0.039041727781295776, -0.08894212543964386, -1.0612213611602783, 0.05109114944934845] -2024-12-18 18:46:18,785 - INFO - Converted Cartesian position [0.42023294099999997, -0.0060612750000000005, 0.358956207] to joint values [ 0. 0. -0.05260681 -0.46451922 -0.0398952 -0.07482018 - -1.0654767 0.05109115]. -2024-12-18 18:46:18,801 - INFO - Moving arm to joint values: [-0.0398952 -0.07482018 -1.0654767 0.05109115] -2024-12-18 18:46:53,951 - INFO - Checking for faults on the robot... -2024-12-18 18:46:56,955 - INFO - Successfully initialize the robot -2024-12-18 18:46:56,957 - INFO - Current end effector pose: -2024-12-18 18:46:56,958 - INFO - Current joint values: [-0.03989308699965477, -0.07481799274682999, -1.0654820203781128, 0.05109114944934845, -1.0612232685089111, 0.05108923092484474] -2024-12-18 18:46:56,959 - INFO - Current joint values: [-0.03989308699965477, -0.07481799274682999, -1.0654820203781128, 0.05109114944934845, -1.0612232685089111, 0.05108923092484474] -2024-12-18 18:46:56,976 - INFO - Converted Cartesian position [0.544744263, -0.035832783, 0.702229614] to joint values [ 0. 0. -0.02914492 -0.07779642 -1.06724875 0.05369141 - -1.05746224 0.05108923]. -2024-12-18 18:46:56,993 - INFO - Moving arm to joint values: [-0.02914492 -0.07779642 -1.06724875 0.05369141 -1.05746224 0.05108923] -2024-12-18 18:47:01,336 - INFO - Checking for faults on the robot... -2024-12-18 18:47:04,340 - INFO - Successfully initialize the robot -2024-12-18 18:47:04,342 - INFO - Current end effector pose: -2024-12-18 18:47:04,343 - INFO - Current joint values: [-0.029141800478100777, -0.0777958333492279, -1.067247986793518, 0.05368932709097862, -1.0574573278427124, 0.05108347907662392] -2024-12-18 18:47:04,344 - INFO - Current joint values: [-0.029141800478100777, -0.0777958333492279, -1.067247986793518, 0.05368932709097862, -1.0574573278427124, 0.05108347907662392] -2024-12-18 18:47:04,371 - INFO - Converted Cartesian position [0.544324036, -0.030616127, 0.704100769] to joint values [ 0. 0. -0.0184399 -0.08076505 -1.06907166 0.0562189 - -1.05371494 0.05108348]. -2024-12-18 18:47:04,386 - INFO - Moving arm to joint values: [-0.0184399 -0.08076505 -1.06907166 0.0562189 -1.05371494 0.05108348] -2024-12-18 18:49:03,469 - INFO - Checking for faults on the robot... -2024-12-18 18:49:06,473 - INFO - Successfully initialize the robot -2024-12-18 18:49:06,475 - INFO - Current end effector pose: -2024-12-18 18:49:06,476 - INFO - Current joint values: [-0.018438449129462242, -0.080769844353199, -1.069075345993042, 0.05621464177966118, -1.0537163019180298, 0.05107772350311279] -2024-12-18 18:49:06,478 - INFO - Current joint values: [-0.018438449129462242, -0.080769844353199, -1.069075345993042, 0.05621464177966118, -1.0537163019180298, 0.05107772350311279] -2024-12-18 18:49:06,494 - INFO - Converted Cartesian position [0.543839844, -0.025415237, 0.705993835] to joint values [ 0. 0. -0.00778084 -0.0837264 -1.0709453 0.05867344 - -1.04997243 0.05107772]. -2024-12-18 18:49:06,504 - INFO - Moving arm to joint values: [-0.00778084 -0.0837264 -1.0709453 0.05867344 -1.04997243 0.05107772] -2024-12-18 19:03:30,498 - INFO - Checking for faults on the robot... -2024-12-18 19:03:33,501 - INFO - Successfully initialize the robot -2024-12-18 19:03:33,503 - INFO - Current end effector pose: -2024-12-18 19:03:33,504 - INFO - Current joint values: [-0.0077811176888644695, -0.08373042196035385, -1.0709487199783325, 0.058669012039899826, -1.0499733686447144, 0.05107197165489197] -2024-12-18 19:03:33,506 - INFO - Converted joint values [-0.0077811176888644695, -0.08373042196035385, -1.0709487199783325, 0.058669012039899826, -1.0499733686447144, 0.05107197165489197] to Cartesian Pose [ 0.40711183 -0.01031775 0.60591553] and [ 0.4502702 -0.04255652 0.8916342 -0.02083634]. -2024-12-18 19:03:33,523 - INFO - Moved end effector to pose: [0.544156680181676, -0.020046261931804074, 0.7065443288649818], [0.4502701979781566, -0.04255652157821327, 0.8916341952137145, -0.02083634347640151] -2024-12-18 19:03:35,248 - INFO - Checking for faults on the robot... -2024-12-18 19:03:38,252 - INFO - Successfully initialize the robot -2024-12-18 19:03:38,254 - INFO - Current end effector pose: -2024-12-18 19:03:38,255 - INFO - Current joint values: [0.0008705341024324298, -0.08028855174779892, -1.0714375972747803, 0.07522833347320557, -1.05265212059021, 0.029323959723114967] -2024-12-18 19:03:38,257 - INFO - Converted joint values [0.0008705341024324298, -0.08028855174779892, -1.0714375972747803, 0.07522833347320557, -1.05265212059021, 0.029323959723114967] to Cartesian Pose [ 0.40795468 -0.00880593 0.6046628 ] and [ 0.45074449 -0.04397601 0.8914375 -0.01531992]. -2024-12-18 19:03:38,275 - INFO - Moved end effector to pose: [0.5450408113034085, -0.019806113524565863, 0.7051042093298315], [0.4507444869504017, -0.043976013435077045, 0.8914375007472969, -0.015319921265793164] -2024-12-18 19:04:07,824 - INFO - Checking for faults on the robot... -2024-12-18 19:04:10,827 - INFO - Successfully initialize the robot -2024-12-18 19:04:10,829 - INFO - Current end effector pose: -2024-12-18 19:04:10,830 - INFO - Current joint values: [0.009619977325201035, -0.07683710008859634, -1.0719534158706665, 0.09182984381914139, -1.0552924871444702, 0.007551020476967096] -2024-12-18 19:04:10,832 - INFO - Converted joint values [0.009619977325201035, -0.07683710008859634, -1.0719534158706665, 0.09182984381914139, -1.0552924871444702, 0.007551020476967096] to Cartesian Pose [ 0.40879107 -0.00724129 0.60338181] and [ 0.45134669 -0.04538176 0.89114013 -0.00979443]. -2024-12-18 19:04:10,845 - INFO - Moved end effector to pose: [0.40879107281287214, -0.007241288573654841, 0.603381813188234], [0.45134669297750507, -0.04538176065002378, 0.8911401279310248, -0.009794433605394741] -2024-12-18 19:06:01,384 - INFO - Checking for faults on the robot... -2024-12-18 19:06:04,387 - INFO - Successfully initialize the robot -2024-12-18 19:06:04,389 - INFO - Current end effector pose: -2024-12-18 19:06:04,390 - INFO - Current joint values: [0.08845508098602295, -0.43194982409477234, -0.4311617314815521, 0.15491288900375366, -1.3452225923538208, -0.04047024995088577] -2024-12-18 19:06:04,392 - INFO - Converted joint values [0.08845508098602295, -0.43194982409477234, -0.4311617314815521, 0.15491288900375366, -1.3452225923538208, -0.04047024995088577] to Cartesian Pose [0.27241607 0.00688281 0.50179729] and [ 0.45243818 -0.04609228 0.89059914 -0.00289224]. -2024-12-18 19:06:04,411 - INFO - Moved end effector to pose: [0.40946135764922154, -0.006629192710045183, 0.6014768568030846], [0.45243818384761103, -0.04609227938818567, 0.8905991390772001, -0.0028922400361672523] -2024-12-18 19:28:02,212 - INFO - Checking for faults on the robot... -2024-12-18 19:28:05,216 - INFO - Successfully initialize the robot -2024-12-18 19:28:05,218 - INFO - Current end effector pose: -2024-12-18 19:28:05,219 - INFO - Current joint values: [0.10339414328336716, -0.42639681696891785, -0.4312556982040405, 0.17492558062076569, -1.35066819190979, -0.06476274877786636] -2024-12-18 19:28:05,220 - INFO - Current joint values: [0.10339414328336716, -0.42639681696891785, -0.4312556982040405, 0.17492558062076569, -1.35066819190979, -0.06476274877786636] -2024-12-18 19:28:05,259 - INFO - Converted Cartesian position [0.409461365, -0.0066291909999999996, 0.601476868] to joint values [ 0. 0. 0.11791164 -0.43554719 -0.43271841 0.17974884 - -1.34381266 -0.06476275]. -2024-12-18 19:28:05,276 - INFO - Moving arm to joint values: [ 0.11791164 -0.43554719 -0.43271841 0.17974884 -1.34381266 -0.06476275] -2024-12-18 19:28:05,277 - INFO - Converted joint values [0.10339414328336716, -0.42639681696891785, -0.4312556982040405, 0.17492558062076569, -1.35066819190979, -0.06476274877786636] to Cartesian Pose [0.27292347 0.00885446 0.50022169] and [ 0.45369526 -0.0467749 0.88991891 0.00412945]. -2024-12-18 19:28:05,302 - INFO - Moved end effector to pose: [0.4101334719036873, -0.005935331581744431, 0.599492418416786], [0.4536952636757494, -0.04677490147015898, 0.8899189086344537, 0.004129450994019601] -2024-12-18 19:29:25,721 - INFO - Checking for faults on the robot... -2024-12-18 19:29:28,725 - INFO - Successfully initialize the robot -2024-12-18 19:29:45,737 - INFO - Checking for faults on the robot... -2024-12-18 19:29:48,741 - INFO - Successfully initialize the robot -2024-12-18 19:33:22,153 - INFO - Checking for faults on the robot... -2024-12-18 19:33:25,157 - INFO - Successfully initialize the robot -2024-12-18 19:33:25,524 - INFO - Moving arm to joint values: [] -2024-12-18 19:33:43,747 - INFO - Checking for faults on the robot... -2024-12-18 19:33:46,751 - INFO - Successfully initialize the robot -2024-12-18 19:33:46,765 - ERROR - Failed to move arm to joint values: [0.6422811647339133, -0.42062434973063345, 0.4310963252425994, 0.1954768762233649, -1.356120828799594, -0.0890117918517108]. Error code: -8 -2024-12-18 19:34:10,371 - INFO - Checking for faults on the robot... -2024-12-18 19:34:13,373 - INFO - Successfully initialize the robot -2024-12-18 19:34:13,385 - ERROR - Failed to move arm to joint values: [0.2932153143350474, -0.42062434973063345, 0.4310963252425994, 0.1954768762233649, -1.356120828799594, -0.0890117918517108]. Error code: -8 -2024-12-18 19:38:13,960 - INFO - Checking for faults on the robot... -2024-12-18 19:38:16,963 - INFO - Successfully initialize the robot -2024-12-18 19:38:16,981 - INFO - Moving arm to joint values: [0.2932153143350474, -0.42062434973063345, -0.4310963252425994, 0.1954768762233649, -1.356120828799594, -0.0890117918517108] -2024-12-18 19:43:41,648 - INFO - Checking for faults on the robot... -2024-12-18 19:43:44,652 - INFO - Successfully initialize the robot -2024-12-18 19:43:44,654 - INFO - Robot states updated: {'joint_angles': [0.29321083426475525, -0.4206232726573944, -0.43110036849975586, 0.19547133147716522, -1.3561214208602905, -0.08901306986808777], 'joint_velocities': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 'tcp_pose': [404.918396, 65.631699, 599.238892, -0.135326, -0.941132, -2.907577]} -2024-12-18 19:43:44,655 - INFO - Current end effector pose: -2024-12-18 19:43:44,656 - INFO - Current joint values: [0.29321083426475525, -0.4206232726573944, -0.43110036849975586, 0.19547133147716522, -1.3561214208602905, -0.08901306986808777] -2024-12-18 19:43:44,657 - INFO - Current joint values: [0.29321083426475525, -0.4206232726573944, -0.43110036849975586, 0.19547133147716522, -1.3561214208602905, -0.08901306986808777] -2024-12-18 19:43:44,679 - INFO - Converted Cartesian position [0.40491839599999996, 0.065631699, 0.5992388919999999] to joint values [ 0. 0. 0.30733251 -0.43007508 -0.43256943 0.19979759 - -1.34929972 -0.08901307]. -2024-12-18 19:43:44,690 - INFO - Moving arm to joint values: [ 0.30733251 -0.43007508 -0.43256943 0.19979759 -1.34929972 -0.08901307] -2024-12-18 19:43:44,691 - INFO - Converted joint values [0.29321083426475525, -0.4206232726573944, -0.43110036849975586, 0.19547133147716522, -1.3561214208602905, -0.08901306986808777] to Cartesian Pose [0.2673795 0.05821301 0.49846018] and [ 0.45774199 -0.00768479 0.88657217 -0.0663552 ]. -2024-12-18 19:43:44,714 - INFO - Moved end effector to pose: [0.4055321134177814, 0.06622355938701624, 0.5972006695927298], [0.45774198538199745, -0.007684789479678209, 0.8865721665786355, -0.06635519781648087] -2024-12-18 19:48:39,844 - INFO - Checking for faults on the robot... -2024-12-18 19:48:42,849 - INFO - Successfully initialize the robot -2024-12-18 19:48:42,851 - INFO - Robot states updated: {'joint_angles': [0.3071259558200836, -0.4149245321750641, -0.4313860833644867, 0.21474197506904602, -1.3614270687103271, -0.11202086508274078], 'joint_velocities': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 'tcp_pose': {'position': [405.532104, 66.223572, 597.200684], 'orientation_quat': [-0.4577420809017105, 0.007684608623454013, -0.8865720689905066, 0.06635586370448886]}} -2024-12-18 19:48:42,852 - INFO - Current end effector pose: -2024-12-18 19:48:42,853 - INFO - Current joint values: [0.3071259558200836, -0.4149245321750641, -0.4313860833644867, 0.21474197506904602, -1.3614270687103271, -0.11202086508274078] -2024-12-18 19:48:42,854 - INFO - Current joint values: [0.3071259558200836, -0.4149245321750641, -0.4313860833644867, 0.21474197506904602, -1.3614270687103271, -0.11202086508274078] -2024-12-18 19:48:42,881 - INFO - Converted Cartesian position [0.405532104, 0.06622357200000001, 0.597200684] to joint values [ 0. 0. 0.32103894 -0.42479738 -0.43299597 0.21893948 - -1.35442554 -0.11202087]. -2024-12-18 19:48:42,898 - INFO - Moving arm to joint values: [ 0.32103894 -0.42479738 -0.43299597 0.21893948 -1.35442554 -0.11202087] -2024-12-18 19:48:42,899 - INFO - Converted joint values [0.3071259558200836, -0.4149245321750641, -0.4313860833644867, 0.21474197506904602, -1.3614270687103271, -0.11202086508274078] to Cartesian Pose [0.26755402 0.06011599 0.49685333] and [ 0.45931898 -0.00825258 0.88625121 -0.05930228]. -2024-12-18 19:48:42,923 - INFO - Moved end effector to pose: [0.4061248925566705, 0.06689042357420937, 0.5950990421364744], [0.45931898213059913, -0.008252576345193158, 0.8862512099715579, -0.05930228041253287] -2024-12-18 19:50:05,551 - INFO - Checking for faults on the robot... -2024-12-18 19:50:08,554 - INFO - Successfully initialize the robot -2024-12-18 19:50:08,555 - INFO - Robot states updated: {'joint_angles': [0.3212558329105377, -0.40918171405792236, -0.4317369759082794, 0.23415449261665344, -1.36670982837677, -0.13509002327919006], 'joint_velocities': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 'tcp_pose': {'position': [406.124878, 66.890427, 595.09906], 'orientation_quat': [-0.4593230646886693, 0.008253882092979885, -0.8862502553281563, 0.05928474176532797]}} -2024-12-18 19:50:08,556 - INFO - Current end effector pose: -2024-12-18 19:50:08,556 - INFO - Current joint values: [0.3212558329105377, -0.40918171405792236, -0.4317369759082794, 0.23415449261665344, -1.36670982837677, -0.13509002327919006] -2024-12-18 19:50:08,556 - INFO - Current joint values: [0.3212558329105377, -0.40918171405792236, -0.4317369759082794, 0.23415449261665344, -1.36670982837677, -0.13509002327919006] -2024-12-18 19:50:08,566 - INFO - Converted Cartesian position [0.406124878, 0.066890427, 0.59509906] to joint values [ 0. 0. 0.33491388 -0.41949522 -0.4334871 0.23820072 - -1.35952535 -0.13509002]. -2024-12-18 19:50:08,581 - INFO - Moving arm to joint values: [ 0.33491388 -0.41949522 -0.4334871 0.23820072 -1.35952535 -0.13509002] -2024-12-18 19:50:08,581 - INFO - Converted joint values [0.3212558329105377, -0.40918171405792236, -0.4317369759082794, 0.23415449261665344, -1.36670982837677, -0.13509002327919006] to Cartesian Pose [0.26769183 0.0620906 0.49522377] and [ 0.46103698 -0.00873728 0.88580106 -0.05220196]. -2024-12-18 19:50:08,606 - INFO - Moved end effector to pose: [0.4066985024879677, 0.06764196020182099, 0.5929290852952557], [0.4610369792157506, -0.008737284618235619, 0.8858010608341843, -0.05220195665058314] -2024-12-18 19:50:20,862 - INFO - Checking for faults on the robot... -2024-12-18 19:50:23,866 - INFO - Successfully initialize the robot -2024-12-18 19:50:23,869 - INFO - Robot states updated: {'joint_angles': [0.33550843596458435, -0.4033794105052948, -0.4321492314338684, 0.25356510281562805, -1.3719943761825562, -0.1580326110124588], 'joint_velocities': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 'tcp_pose': {'position': [406.698517, 67.641968, 592.929077], 'orientation_quat': [-0.4610324747336115, 0.008736304333459954, -0.8858023358721154, 0.05222026418067217]}} -2024-12-18 19:51:45,453 - INFO - Checking for faults on the robot... -2024-12-18 19:51:48,456 - INFO - Successfully initialize the robot -2024-12-18 19:51:48,826 - INFO - Moved end effector to pose: [0.8121770143508911, 0.04117409512400627, 0.2725994288921356], [-0.4610324747336115, 0.008736304333459954, -0.8858023358721154, 0.05222026418067217] -2024-12-18 19:52:20,156 - INFO - Checking for faults on the robot... -2024-12-18 19:52:23,157 - INFO - Successfully initialize the robot -2024-12-18 19:52:23,521 - INFO - Moved end effector to pose: [0.6121770143508911, 0.04117409512400627, 0.2725994288921356], [-0.4610324747336115, 0.008736304333459954, -0.8858023358721154, 0.05222026418067217] -2024-12-18 19:54:11,057 - INFO - Checking for faults on the robot... -2024-12-18 19:54:14,060 - INFO - Successfully initialize the robot -2024-12-18 19:54:14,063 - INFO - Robot states updated: {'joint_angles': [-0.027931872755289078, -0.47473636269569397, -0.03839745745062828, -0.022691410034894943, -1.057666301727295, 0.0034840537700802088], 'joint_velocities': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 'tcp_pose': {'position': [420.434235, -5.81615, 359.876038], 'orientation_quat': [0.7071068218077394, -0.00032550013355177364, 0.7071065907288765, 0.0003255002399235735]}} -2024-12-18 19:55:01,266 - INFO - Checking for faults on the robot... -2024-12-18 19:55:04,269 - INFO - Successfully initialize the robot -2024-12-18 19:55:04,272 - ERROR - Error updating robot states: unsupported operand type(s) for /: 'list' and 'int' -2024-12-18 19:56:13,014 - INFO - Checking for faults on the robot... -2024-12-18 19:56:16,017 - INFO - Successfully initialize the robot -2024-12-18 19:56:16,020 - INFO - Robot states updated: {'joint_angles': [-0.02792995609343052, -0.47473061084747314, -0.038393620401620865, -0.02269332855939865, -1.0576682090759277, 0.0034840537700802088], 'joint_velocities': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 'tcp_pose': {'position': [0.420434235, -0.00581615, 0.359876038], 'orientation_quat': [0.7071068218077394, -0.00032550013355177364, 0.7071065907288765, 0.0003255002399235735]}} -2024-12-18 19:56:58,801 - INFO - Checking for faults on the robot... -2024-12-18 19:57:01,805 - INFO - Successfully initialize the robot -2024-12-18 19:57:01,818 - INFO - Moved end effector to pose: [0.420434235, -0.00581615, 0.459876038], [0.7071068218077394, -0.00032550013355177364, 0.7071065907288765, 0.0003255002399235735] -2024-12-18 19:57:11,086 - INFO - Checking for faults on the robot... -2024-12-18 19:57:14,090 - INFO - Successfully initialize the robot -2024-12-18 19:57:14,109 - INFO - Moved end effector to pose: [0.420434235, -0.00581615, 0.359876038], [0.7071068218077394, -0.00032550013355177364, 0.7071065907288765, 0.0003255002399235735] -2024-12-18 19:57:45,530 - INFO - Checking for faults on the robot... -2024-12-18 19:57:48,533 - INFO - Successfully initialize the robot -2024-12-18 19:57:48,551 - INFO - Moved end effector to pose: [0.420434235, -0.00581615, 0.459876038], [0.7071068218077394, -0.00032550013355177364, 0.7071065907288765, 0.0003255002399235735] diff --git a/Install/install.md b/Install/install.md new file mode 100644 index 0000000..3a87b51 --- /dev/null +++ b/Install/install.md @@ -0,0 +1,49 @@ +# Installation + +## Install with conda (Linux) + +### Basic Env + +> ***Note**: This will only install the basic module. For other algorithm submodules, please follow the instructions [Algorithm Submodule Env](#algorithm-submodule-env) to install as needed.* + +1. Pull the repository and update the submodule +``` +cd /home/$(whoami) +git clone https://github.com/yding25/BestMan_Xarm.git +``` + +2. Install xArm SDK +``` +cd ~ +git clone https://github.com/xArm-Developer/xArm-Python-SDK.git +python3 setup.py install +``` + +3. Run the following script to add the project to the PYTHON search path + +``` +/home/$(whoami)/BestMan_Ｘarm/Install +chmod 777 pythonpath.sh +bash pythonpath.sh +source ~/.bashrc +``` + +4. Create basic conda environment +``` +cd /home/$(whoami)/BestMan_Xarm/Install +conda env create -f basic_environment.yaml +``` + +5. Install ROS environment (optional) +``` +cd /home/$(whoami)/BestMan_Flexiv/Install +chmod 777 install_ros_noetic.sh +bash install_ros_noetic.sh +source ~/.bashrc +``` + +6. Check ROS environment (optional) +``` +roscore +echo $ROS_DISTRO +``` diff --git a/README.md b/README.md index c4bada1..3e8eca1 100644 --- a/README.md +++ b/README.md @@ -15,26 +15,7 @@ Welcome to the BestMan_Xarm repository, a codebase dedicated to the XArm 6 robot ## 💻 Installation -- (Optional) Create conda environment - - ``` - conda env create -f basic_environment.yaml - ``` - -- Install xArm-Python-SDK - - ```bash - cd ~ - git clone https://github.com/xArm-Developer/xArm-Python-SDK.git - python3 setup.py install - ``` - -- Clone the BestMan_Xarm Repository - - ```bash - git clone https://github.com/yding25/BestMan_Xarm.git - ``` - +We provide the installation guide [here](Install/install.md). You can install locally or use docker and verify the installation easily. ## 🔎 Project Structure @@ -52,14 +33,6 @@ firefox /home/$(whoami)/BestMan_Xarm/docs/html/index.html # API document cd ~/BestMan_Xarm/Examples/ # Demo: Go Home python3 move_arm_to_home.py 192.168.1.208 - -# Demo: Move Joints -python3 move_arm_to_joint_angles.py 192.168.1.208 - -# Demo: Move gripper -python3 close_gripper.py 192.168.1.208 -python3 open_gripper.py 192.168.1.208 -python3 open_gripper_width.py 192.168.1.208 ``` (We are fixing other demos.) diff --git a/Robotics_API/Bestman_Real_Xarm6.py b/Robotics_API/Bestman_Real_Xarm6.py index b3721b2..2e0cf10 100644 --- a/Robotics_API/Bestman_Real_Xarm6.py +++ b/Robotics_API/Bestman_Real_Xarm6.py @@ -21,7 +21,6 @@ from ikpy.link import URDFLink from xarm.wrapper import XArmAPI import logging -import math class Bestman_Real_Xarm6: @@ -38,6 +37,7 @@ class Bestman_Real_Xarm6: """ def __init__(self, robot_ip, local_ip=None, frequency=None): + # DONE """ Initializes the xArm robot and related components. @@ -54,18 +54,13 @@ def __init__(self, robot_ip, local_ip=None, frequency=None): # Parse URDF for kinematic chain current_dir = os.path.dirname(os.path.abspath(__file__)) urdf_file = os.path.join(current_dir, "../Asset/xarm6_robot.urdf") - if not os.path.exists(urdf_file): raise FileNotFoundError(f"URDF file not found: {urdf_file}") - - # Parse the URDF file and configure the kinematic chain - self.robot_chain = Chain.from_urdf_file(urdf_file, base_elements=["world"]) + self.robot_chain = Chain.from_urdf_file(urdf_file, base_elements=["world"]) # Parse the URDF file and configure the kinematic chain # Define active links mask for xArm6 (Base + 6 DOF + End Effector) - active_links_mask = [False] + [True] * 6 + [False] # Adjust for 6-DOF robot + active_links_mask = [False] + [False] + [True] * 6 # Adjust for 6-DOF robot self.robot_chain.active_links_mask = active_links_mask - - # Filter active joints (revolute or prismatic) self.active_joints = [ joint for joint in self.robot_chain.links @@ -86,7 +81,7 @@ def __init__(self, robot_ip, local_ip=None, frequency=None): format="%(asctime)s - %(levelname)s - %(message)s", handlers=[ logging.StreamHandler(), # Output to console - logging.FileHandler("xarm_command_log.log"), # Output to file + # logging.FileHandler("xarm_command_log.log"), # Output to file ], ) self.log = logging.getLogger("XArmRobotController") @@ -95,9 +90,8 @@ def __init__(self, robot_ip, local_ip=None, frequency=None): # Device Initialization and State Management # ---------------------------------------------------------------- - - def initialize_robot(self): + # DONE """ Initializes the robot by clearing faults and enabling it. @@ -105,11 +99,10 @@ def initialize_robot(self): bool: True if the robot is successfully initialized, False otherwise. """ try: - # Step 1: Clear faults and warnings self.log.info("Checking for faults on the robot...") - self.robot.clean_error() # Clear errors - self.robot.clean_warn() # Clear warnings - time.sleep(3) # Wait for a short duration to allow the robot to reset + self.robot.clean_error() + self.robot.clean_warn() + time.sleep(2) self.log.info(f"Successfully initialize the robot") return True @@ -118,90 +111,88 @@ def initialize_robot(self): return False def update_robot_states(self): + # DONE """ - Fetches and updates the current robot states (joint angles and TCP pose in quaternion format). + Fetches and returns the current robot states (joint angles and TCP pose in quaternion format). """ try: # Retrieve joint states joint_states = self.robot.get_joint_states(is_radian=True) - joint_angles = joint_states[1][0][:6] - joint_velocities = joint_states[1][1][:6] - + joint_values = joint_states[1][0][:6] + joint_velocities = joint_states[1][1][:6] # TODO + # Retrieve end effector pose (Euler angles) - success, tcp_pose_raw = self.robot.get_position(is_radian=True) + success, tcp_pose = self.robot.get_position(is_radian=True) if success != 0: raise ValueError("Failed to retrieve end-effector pose") - - # Convert position from millimeters to meters - position = [value / 1000 for value in tcp_pose_raw[:3]] # x, y, z in meters - - # Convert orientation from Euler angles to quaternion - roll, pitch, yaw = tcp_pose_raw[3:6] # Extract Euler angles - orientation_quat = R.from_euler('xyz', [roll, pitch, yaw]).as_quat() - - # Store updated robot states - self.robot_states = { - "joint_angles": joint_angles, - "joint_velocities": joint_velocities, - "tcp_pose": { + + position = [value / 1000 for value in tcp_pose[:3]] # x, y, z in meters # ! + orientation = R.from_euler('xyz', tcp_pose[3:6]).as_quat() # ! + + robot_states = { + "joint_values": joint_values, + "tcp_pose (quaternion)": { "position": position, # Position in meters - "orientation_quat": orientation_quat.tolist(), # Quaternion (x, y, z, w) + "orientation": orientation.tolist(), # Quaternion (x, y, z, w) }, } - + # Log and display TCP pose in quaternion format - formatted_quaternion = ["%.2f" % i for i in orientation_quat] - self.log.info(f"Robot states updated: {self.robot_states}") - print("tcp_pose(quaternion):", formatted_quaternion) + self.log.info(f"Robot states updated: {robot_states}") + + # Return the robot states + return robot_states except Exception as e: self.log.error(f"Error updating robot states: {str(e)}") def go_home(self): + # DONE """ Moves the robot arm to its initial (home) pose. Converts input position (in meters) to millimeters for the xArm API, and quaternion orientation to Euler angles. """ - position = [0.420, -0.0055, 0.36] # x, y, z in meters - orientation_quat = [ - 0.7071065119402606, - -0.0006170669964236503, - 0.7071065119402605, - 0.0006170669964236504, - ] # Identity quaternion (no rotation) - pose = Pose(position, orientation_quat) - - position_mm = [ - p * 1000 for p in position - ] # Convert position to millimeters for xArm API - - euler_orientation = pose.get_orientation( - type="euler" - ) # Convert to [roll, pitch, yaw] - euler_orientation_degree = np.degrees( - euler_orientation - ) # Convert radians to degrees for xArm API - - # Command the robot to move to the home pose - self.robot.set_position( - x=position_mm[0], - y=position_mm[1], - z=position_mm[2], - roll=euler_orientation_degree[0], - pitch=euler_orientation_degree[1], - yaw=euler_orientation_degree[2], - ) + try: + # Set robot to online tcp control mode + if self.robot.set_mode(0) != 0: + self.log.error( + "Failed to set robot mode to 'online joint control mode'." + ) + return + if self.robot.set_state(0) != 0: + self.log.error("Failed to set robot state to operational.") + return + + position = [0.420, -0.0055, 0.36] + orientation = [0.7071, -0.0006, 0.7071, 0.0006] + pose = Pose(position, orientation) + orientation = pose.get_orientation(type="euler") + + # Command the robot to move to the home pose + self.robot.set_position( + x=position[0] * 1000, + y=position[1] * 1000, + z=position[2] * 1000, + roll=orientation[0], + pitch=orientation[1], + yaw=orientation[2], + is_radian=True + ) + self.log.info(f"Robot moved to home position") + except Exception as e: + self.log.error(f"Cannot go home: {str(e)}") - self.log.info(f"Robot moved to home position") def set_pay_load(self, payload_weight, payload_cog): + # TODO self.robot.set_tcp_load(payload_weight, payload_cog) # ---------------------------------------------------------------- # Arm Functions # ---------------------------------------------------------------- def get_joint_bounds(self): + # TODO """ Retrieves the joint limits of the xArm6 robot arm. @@ -214,6 +205,7 @@ def get_joint_bounds(self): return list(zip(joint_min, joint_max)) def print_arm_jointInfo(self): + # TODO """ Prints the joint and link information of the robot's arm. @@ -221,13 +213,11 @@ def print_arm_jointInfo(self): None """ print("Arm joint and link information:") - for i, link in enumerate( - self.robot_chain.links[1:] - ): # Assuming arm starts at the second link - print(f"Link {i + 1}: {link.name}") - self.log.info(f"Link {i + 1}: {link.name}") + for i, link in enumerate(self.robot_chain.links): + print(f"Link {i} attributes: {vars(link)}") def get_joint_idx(self): + # TODO """ Retrieves the indices of the joints in the robot arm. @@ -237,6 +227,7 @@ def get_joint_idx(self): return list(range(len(self.active_joints))) def get_DOF(self): + # DONE """ Retrieves the degree of freedom (DOF) of the xArm robot. @@ -248,6 +239,7 @@ def get_DOF(self): return dof def get_tcp_link(self): + # TODO """ Retrieves the name of the TCP (Tool Center Point) link. @@ -263,6 +255,7 @@ def get_tcp_link(self): return None def get_current_joint_values(self): + # DONE """ Retrieves the current joint angles of the robot arm. @@ -297,6 +290,7 @@ def get_current_joint_velocities(self): return [] def get_current_eef_pose(self): + # DONE """ Retrieves the current pose of the robot arm's end effector. @@ -311,21 +305,20 @@ def get_current_eef_pose(self): ) # Convert position values from millimeters to meters position = [ - raw_pose[0] / 1000, # x in meters - raw_pose[1] / 1000, # y in meters - raw_pose[2] / 1000, # z in meters + raw_pose[0] / 1000, + raw_pose[1] / 1000, + raw_pose[2] / 1000, ] - orientation = raw_pose[3:] # roll, pitch, yaw (already in radians) + orientation = raw_pose[3:] pose = Pose(position, orientation) - self.log.info(f"Current end effector pose: {pose}") + self.log.info(f"Current end effector pose: {pose.position} and {pose.orientation}") return pose except Exception as e: self.log.error(f"Failed to retrieve end effector pose: {str(e)}") return None - def move_arm_to_joint_values( - self, joint_values, target_vel=None, target_acc=None, max_vel=None, max_acc=None - ): + def move_arm_to_joint_values(self, joint_values, target_vel=None, target_acc=None, max_vel=None, max_acc=None): + # DONE """ Moves the robotic arm to a specific set of joint values. @@ -363,8 +356,8 @@ def move_arm_to_joint_values( f"An error occurred while moving arm to joint values: {str(e)}" ) - # TODO def get_current_tcp_speed(self): + # TODO """ Retrieves the current tcp velocities of the robot arm. @@ -411,8 +404,8 @@ def move_eef_to_goal_pose(self, goal_pose, max_linear_vel=100, max_angular_vel=5 yaw=orientation[2], speed=max_linear_vel, mvacc=max_angular_vel, - is_radian=True, - wait=False, + is_radian=True, # ! + wait=False, # ! ) self.log.info(f"Moved end effector to pose: {goal_pose.position}, {goal_pose.orientation}") @@ -420,7 +413,7 @@ def move_eef_to_goal_pose(self, goal_pose, max_linear_vel=100, max_angular_vel=5 self.log.error(f"Failed to move end effector to goal pose: {str(e)}") def move_eef_to_tcp_velocity(self, _velocity_setpoint, _duration): - #TODO: No test + #TODO """ Move arm's end effector to a target tcp velocity. @@ -446,7 +439,7 @@ def move_eef_to_tcp_velocity(self, _velocity_setpoint, _duration): ) def rotate_eef_tcp(self, axis, angle): - # TODO: No Test + # TODO """ Rotate the end effector of the robot arm by a specified angle by joint. @@ -468,60 +461,8 @@ def rotate_eef_tcp(self, axis, angle): # ---------------------------------------------------------------- # Functions for IK # ---------------------------------------------------------------- - - # def joints_to_cartesian(self, joint_values): - # """ - # Converts the robot's joint angles to its Cartesian coordinates. - - # Args: - # joint_values (list[float]): A list of joint angles for the robot arm. - - # Returns: - # Pose: A Pose object representing the Cartesian position and quaternion orientation. - - # Raises: - # ValueError: If the number of joint values does not match the number of active joints. - # """ - # try: - # if len(joint_values) != len(self.active_joints): - # raise ValueError( - # "The number of joint values does not match the number of active joints." - # ) - - # full_joint_values = np.zeros(len(self.robot_chain.links)) - # active_joint_indices = [ - # self.robot_chain.links.index(joint) for joint in self.active_joints - # ] - - # for i, joint_value in enumerate(joint_values): - # full_joint_values[active_joint_indices[i]] = joint_value - - # cartesian_matrix = self.robot_chain.forward_kinematics(full_joint_values) - # position = cartesian_matrix[:3, 3] - # orientation_matrix = cartesian_matrix[:3, :3] - # quaternion = R.from_matrix(orientation_matrix).as_quat() - - # self.log.info(f"Converted joint values {joint_values} to Cartesian Pose {position} and {quaternion}.") - - # def calculate_new_pose(x, y, z, quaternion, distance = -0.1703): - # # Step 1: 根据 roll, pitch, yaw 计算旋转矩阵 - # rotation = R.from_quat(quaternion) - # rotation_matrix = rotation.as_matrix() - # # Step 2: 提取旋转矩阵的 z 轴方向 - # z_axis = rotation_matrix[:, 2] # 第三列就是 z 轴方向 - # new_position = np.array([x, y, z]) - distance * z_axis - # # Step 3: 返回新的位姿 (新位置 + 原来的姿态) - # return [new_position[0], new_position[1], new_position[2]], quaternion - - # position, quaternion= calculate_new_pose(position[0],position[1],position[2], quaternion) - # print('nnn',position) - - # return Pose(position, quaternion) - # except Exception as e: - # self.log.error(f"Error converting joint values to Cartesian: {str(e)}") - # raise - def joints_to_cartesian(self, joint_values): + # DONE """ Converts the robot's joint angles to its Cartesian coordinates. @@ -563,45 +504,9 @@ def joints_to_cartesian(self, joint_values): except Exception as e: self.log.error(f"Error converting joint values to Cartesian: {str(e)}") raise - - # def cartesian_to_joints(self, pose, initial_joint_angles=None): - # position = pose.position - # orientation = pose.orientation - - # def calculate_new_pose(x, y, z, quaternion, distance = 0.1703): - # # Step 1: 根据 roll, pitch, yaw 计算旋转矩阵 - # rotation = R.from_quat(quaternion) - # rotation_matrix = rotation.as_matrix() - # # Step 2: 提取旋转矩阵的 z 轴方向 - # z_axis = rotation_matrix[:, 2] # 第三列就是 z 轴方向 - # new_position = np.array([x, y, z]) - distance * z_axis - # # Step 3: 返回新的位姿 (新位置 + 原来的姿态) - # return [new_position[0], new_position[1], new_position[2]], quaternion - - # position, quaternion= calculate_new_pose(position[0],position[1],position[2], orientation) - - # my_chain = ikpy.chain.Chain.from_urdf_file("../Asset/xarm6_robot.urdf", base_elements=['world']) - # """ - # 将机械臂的笛卡尔坐标转换为关节角度。 - # """ - # rotation = R.from_quat(orientation) - # rotation_matrix = rotation.as_matrix() - - # if initial_joint_angles is None: - # # initial_joint_angles = [0, 0, -0.02792803756892681, -0.47473636269569397, -0.0384012907743454, -0.022689493373036385, -1.057666301727295, 0.0034878887236118317] - # initial_joint_angles = [0, 0] + self.get_current_joint_values() - - # joint_angles = my_chain.inverse_kinematics( - # position, - # rotation_matrix, - # orientation_mode='all', - # initial_position=initial_joint_angles) - - # print('4',joint_angles) - # return joint_angles - def cartesian_to_joints(self, pose, initial_joint_values=None): + # DONE """ Converts the robot's Cartesian coordinates to its joint angles. @@ -702,7 +607,30 @@ def get_gripper_position_xarm(self): return gripper_pos[1] + def gripper_goto_xarm(self, value, speed=5000, force=None): + """ + Moves the gripper to a specified position with given speed. + + Args: + value (int): Position of the gripper. Integer between 0 and 800. + 0 represents the open position, and 255 represents the closed position. + speed (int): Speed of the gripper movement, between 0 and 8000. + force (int): Not applicable for xarm gripper + + Note: + - 0 means fully closed. + - 800 means fully open. + """ + self.robot.set_gripper_position( + pos=value, speed=speed, wait=False, timeout=1, auto_enable=True + ) + + def open_gripper_xarm(self): + """Opens the gripper to its maximum position with maximum speed and force.""" + self.gripper_goto(value=850, speed=5000, force=None) + def get_gripper_position_robotiq(self, number_of_registers=3): + # TODO """ Reading the status of robotiq gripper @@ -723,31 +651,12 @@ def get_gripper_position_robotiq(self, number_of_registers=3): gripper_width = status[1][-2] return gripper_width - def gripper_goto_xarm(self, value, speed=5000, force=None): - """ - Moves the gripper to a specified position with given speed. - - Args: - value (int): Position of the gripper. Integer between 0 and 800. - 0 represents the open position, and 255 represents the closed position. - speed (int): Speed of the gripper movement, between 0 and 8000. - force (int): Not applicable for xarm gripper - - Note: - - 0 means fully closed. - - 800 means fully open. - """ - self.robot.set_gripper_position( - pos=value, speed=speed, wait=False, timeout=1, auto_enable=True - ) - - def gripper_goto_robotiq( - self, pos, speed=0xFF, force=0xFF, wait=False, timeout=5, **kwargs - ): + def gripper_goto_robotiq(self, width, speed=0xFF, force=0xFF, wait=False, timeout=5, **kwargs): + # DONE """ Go to the position with determined speed and force. - :param pos: position of the gripper. Integer between 0 and 255. 0 being the open position and 255 being the close position. + :param width: position of the gripper. Integer between 0 and 255. 0 being the open position and 255 being the close position. :param speed: gripper speed between 0 and 255 :param force: gripper force between 0 and 255 :param wait: whether to wait for the robotion motion complete, default is True @@ -757,17 +666,16 @@ def gripper_goto_robotiq( code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details. robotiq_response: See the robotiq documentation """ - return self.robot.robotiq_set_position( - pos, speed=speed, force=force, wait=wait, timeout=timeout, **kwargs - ) - - def open_gripper_xarm(self): - """Opens the gripper to its maximum position with maximum speed and force.""" - self.gripper_goto(value=850, speed=5000, force=None) + return self.robot.robotiq_set_position(width, speed=speed, force=force, wait=wait, timeout=timeout, **kwargs) + + def close_gripper_xarm(self): + """Closes the gripper to its minimum position with maximum speed and force.""" + self.gripper_goto(value=0, speed=5000, force=None) def open_gripper_robotiq( self, speed=0xFF, force=0xFF, wait=False, timeout=5, **kwargs ): + # DONE """ Open the robotiq gripper @@ -783,14 +691,11 @@ def open_gripper_robotiq( return self.robot.robotiq_open( speed=speed, force=force, wait=wait, timeout=timeout, **kwargs ) - - def close_gripper_xarm(self): - """Closes the gripper to its minimum position with maximum speed and force.""" - self.gripper_goto(value=0, speed=5000, force=None) - + def close_gripper_robotiq( self, speed=0xFF, force=0xFF, wait=False, timeout=5, **kwargs ): + # DONE """ Close the robotiq gripper diff --git a/Robotics_API/__pycache__/Bestman_Real_Xarm6.cpython-38.pyc b/Robotics_API/__pycache__/Bestman_Real_Xarm6.cpython-38.pyc index 4078589e256add3c1e3ebfcbd24318c18bbcef81..93a9c818036545a199fcb3f51f1c78f5729073d1 100644 GIT binary patch delta 3168 zcmai0Yj6|S72bPywOXwnmSy>YF^-JEcx~QK+koGX@~|)lgAoLetQA5wmdw=}u#hpq zPUwpUxFl_+FfpYsI-Mk~m@=iECZ$bgGW;lQe`IDeohFk>Q#zSW^P|k9O>oaynMd-c z((Jc)@BQvM_n!NmvwQs}dGiMG^ts)t0KY%oAB=g1E_lkwnG3ZkPnV!Obah(L)hQbC z{p>~!U9(PKKTSeq`UbsjS_m!Bx9bi1M(8QmyY(i$8Cp;2EqW`A1@xfarngU%Y`*wI z>bA#7v7jq4qC5OT_V>1SN}96{TASK zfs(G+#b=#G+;ClYKaCUpI8mX5*SK5xj~m?bzmWBFo?lVOFS++k8q$nScV=w5s>{#P zdBRmXBVQFJ>=~MoGxiz#RY7-Mrn>twolr8ukTT=YJsIV@!WDXz5Fw-N6?O@dV5q|? z`v(yw97IT$T0EzoXSgzsDe{oJS6C`;6 zxkEJmT;4MB48M{0mBD)=EH@;c7*A`|R7R7jxDih#OlOkCqX{F74=~^wO2!k$z_IX= zv1lq?XhcQ_fEo={9~}!DQIkR>Z&l@0auI6v$uQmpH z9@@&$SOd5>y>sTHN0JfnA5EF|)L0}EO{GH0Ltm^8n5-UG>tg6=AZLMBG!onb7`jcDb$d*lAi4q~R&+Hc zO;E#@AsISFC&Y|6BnvuSw7IC6LIwd`l3+znur*HF?B_(`m!B6-{ zONJ+7@2CB*biHpj#cpkJ(b}`eUXQ8oTpQi2zS9(&?|yID`I)~p#loxmR@Pi^d;MXf z$+ShsjzrmYVoJvNXq2skz%tAqRs*nGvzz3ENybf*HYJuka>TTa#*N5fQydQ;XKV2k z%5rMrANz|Y)84jlA_3ZpB?E_(BhiL{=43v!R)|oAumZqzq@r`WV5n;|F9y#~NZE8D z;i1Y|ElymHuo6JCF)Rg-g=R`9a<+<3X$2kjSaJY;zW|ifY5>7bK}j;9vyvziFH{Se zs(^68vugcw)r5X3JG{V1mxAj2e75j;=daKT#zUYySzefZrRW^Vzm1(Tb}B%p=}Mp% z^GHcmGiHe)=wu4?We_~@A#sYd3r`0Q&{+ zmGu+ZloH{Q=*9AW=O;+=00>GpQvLxU8~INg7r3R|*)&5xYv6Ymm+*^$c~01(kn&pO zYrhCkUksYslhhWA`rg6?q<}wI7?IxsLi!AkRSvBlk|ADDn1}2bEli$_ZLZiVkUnof z`pA$z0f@E)McHMj{L@MeDp}>C|9ZcuXSRVaUtA&o#})`DbRJw>qs+;36F3WEn0-zT4-&HLZB2vi83qMEKPkb*-&F;*@~(= z#L3!$6=WTNytn!-8{2`cO0AkS@aldG3=Id-Ho;TseR52 zpIKJb-3k4y3*n2q+})|EYzy{pMc9V09ibZ`h>!pX*+!0qBM~c$S&|nlKTLM;%<_4q zn~>3iuoJ;bzPZr6y1bLrTSw(tQACbq7q57h%2+8NvahYG0R!)@8X+%dx2&EdbC2s?8Kl8+3+1L~1A*@Y~%z%JH~v9Bv4V^3B_pXFG$<)N3u`5*kh-aQvWC2R>dq%Qnd%I}imt7!hhJUcT+IbC4 zSyji&i~8!a8~2=|eLk+MS$%h=l}Ka zXZCHL#19p=3!x98AK@TE7-10M5W-=EIKnVO3c)}aN5~+YLYPMQ8p78R&LNyfxPb5i z!gmlZBU}|cfZxOB_Yr=;w}r|kOQD&<%vOlxQr9bfWsy>)*cG?pQdGFSiciT`%G4!F zo~kLOO0lw3tx-JCSEwkePm$CjHDAFmG5pP{g}H_>%M@s71?)Q9SP_6Jrp8ljDYQa1 jcv+2(vonyu_d|1gV3@o`@>edzJER+v-HCTij} zLxj7ydya7TEQ{B^@XZdk@lL*Zj>en$9^TEjfYicAcrV`u(sJI%x6hGyE06IVydN|X zKEMa(NZF-4%e;Dua#x7d6YfmW@+*!(Mt7E}{#n-64gpwqVi>xcYWB}iLe$3$>Qicz zFx8dvlkVp!Rph8~&CAg|gyv4hghZ9VO$OagZD$S#}Vf?jzBf!jwC5^V((R$B&ytJZHgCNRlSCJeY$ zXxe_Pf)6yp5Aap^TPlD?7?FWMW4I1oLQrNq#$bvpgcIE=+XE5mm%9UZcf}2nJDSTU zEC|)K#AAAsSL@s1D={ z!#%7r${3^Cf7So!ZXETo8o6y-SU%~mof!wt-MXA0>F~GNI0ph&6IW77iE@=YxCRBF zmRKH=Tx7GXq;TD)7_}*;7L^PI%6cx)k|WRNm7-&TmNejynsF3WZZr}=uZ=!H4K%Cp zhANGNiu*5Ul)waS-pK>VP##2L$tyKKm9Nz{xAn*V@vAHIZ}g>3E&OKqK=mMi+2Zf$3*DrE*aRQ(;q{;Txb^=wm{r%GGE!JCe?An_a*5-)NmF*|p-mNsIXz*D ztF)r#&rX>Fb0!$TTL@9DLeEs_*$OR)oj9@!VMuNdt(b{Lb)mpx#h)_si6zvou7Vpw3ZSO*^vD(Vz#&>_;pCPPIc8mfx$pK!dNLT0^xnKm)AMW>5H5eRSq1=<-0dS?ov@R-a`Qq#bs?S)kUZ zwjW_7inQZu5eQjNEZpbuoHiO#ZE*BlJ257Iqfm z)FETO8-~(k7uVLtH|asy*0ja@2AbFjaTX%@)#XxCBlWzE>S{vD3(KadLmUB(Y;HN| zc^8MUIT2@hrsYqR_R4H;i@d+p8Nkw9?1!h46Vs9g>*X`8jnpNBD;vwNx4uJbj)6^7 ztpH5v*M( zMNeLr;kGC>rp4Aja$M#J754q?m3 zg_F;Z)!p*9x}%#pCtx5ZCZ)M<^~l{Ii+d10{OS95McpEX`g;*Z5tcLne4GM^J0?#h z$H&DdP;pYezV0{`az|%llbxQusJ#yXcaLh7XJo!}7tPpN_(f+}D`BkNlGZhr_4O&H z-OT%RR|j~S@0z5~mVI~3P@0vGY-nx~=*I$xYBmSrYVu6l5;@SsLy)nM{N08y&D)u9 zZ1mF~mFqTE6c%CfojZ5?aU1#bPyW?@k}h#_vSmC1&6(n%bCsn17aWgaTH9E zv&3?cMFin6YHMJZI-L|eaUz{HVY^C?TcQsKJjX)Y;uhd*tEn zr#%K#hlLv^@s?d%)+)hsAja$if|vyn!r0k!Kb@1;d$z2|pu+a*vc2N|9CszHMfdw; zq*pZW2dj7}orMEJaw2i_!~pdI%{C*S>s=nIc=AbsO&Oc1IFC+KknnY$BNz;3f%wYx z+ip;LzFfckRZ9EiZ+CQ4S+41?r}Uic7+6C;DfbP0j($y62419($S)1vvb<*~_$oTI3q9zg1})rfcfQtC$ zo&*ww0s~7FitvvFoqCE z_%uQi;V6QMa2z3xa01~J!fAve!VJO#2oEAWjBo+rF$9V5WrT|ePaw=AJWZgTi)T=L z7UApi#=+*9WguFg0nn*xxO?=F-mb6KHQmsCx=Z)yemw}kW-!3(hC8bJ;NPV0(MM5+ zu{vFM*STw}24E4=9k=A#4`hL-x@bsz18yP=P*JS2mVD@tm+q4n53SL#p2S1)+MymT Vu0ow2mo(lEyG2L5Nr7|Z{{RBUMBxAc diff --git a/Robotics_API/Utils.py b/TODO/Utils.py similarity index 100% rename from Robotics_API/Utils.py rename to TODO/Utils.py From 74e64f8cfd040a4e99a8be42c5e9e622ba3432c5 Mon Sep 17 00:00:00 2001 From: yding25 Date: Thu, 19 Dec 2024 00:40:39 +0800 Subject: [PATCH 06/24] update --- README.md | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/README.md b/README.md index 3e8eca1..48eb6ce 100644 --- a/README.md +++ b/README.md @@ -12,6 +12,16 @@ Welcome to the BestMan_Xarm repository, a codebase dedicated to the XArm 6 robotic arm. +## 🔥 News +- [2024-12-18] We update API names, such as "joint angles" -> "joint values", and revise some function "joints_to_cartesian", "cartesian_to_joints". + +### 🏠 Prerequisites + +> ***Note**: We recommand Ubuntu 22.04 and python version deault to 3.8.* + +- Ubuntu 22.04 +- Conda + - Python 3.8 ## 💻 Installation From 8702b1e43038e5e5abb9cf3809c64fe676c7421e Mon Sep 17 00:00:00 2001 From: Tong Date: Fri, 18 Jul 2025 11:05:23 +0800 Subject: [PATCH 07/24] feat: refactor Xarm7 api --- .vscode/launch.json | 15 + Asset/xarm7_robot.urdf | 359 ++++++++++++++++++ Examples/display_states.py | 4 +- Examples/move_arm_to_home.py | 4 +- Examples/move_eef_to_pose.py | 11 +- Robotics_API/Bestman_Real_Xarm6.py | 56 +-- Robotics_API/Bestman_Real_Xarm7.py | 85 +++++ Robotics_API/__init__.py | 1 + .../Bestman_Real_Xarm6.cpython-310.pyc | Bin 0 -> 22292 bytes .../Bestman_Real_Xarm6.cpython-38.pyc | Bin 23109 -> 22319 bytes .../Bestman_Real_Xarm6.cpython-39.pyc | Bin 0 -> 22372 bytes .../Bestman_Real_Xarm7.cpython-310.pyc | Bin 0 -> 2443 bytes .../Bestman_Real_Xarm7.cpython-38.pyc | Bin 0 -> 2437 bytes .../Bestman_Real_Xarm7.cpython-39.pyc | Bin 0 -> 2435 bytes Robotics_API/__pycache__/Pose.cpython-310.pyc | Bin 0 -> 3463 bytes Robotics_API/__pycache__/Pose.cpython-38.pyc | Bin 3496 -> 3490 bytes Robotics_API/__pycache__/Pose.cpython-39.pyc | Bin 0 -> 3482 bytes .../__pycache__/__init__.cpython-310.pyc | Bin 0 -> 250 bytes .../__pycache__/__init__.cpython-38.pyc | Bin 213 -> 248 bytes .../__pycache__/__init__.cpython-39.pyc | Bin 0 -> 248 bytes Sensor/force.py | 58 +++ 21 files changed, 559 insertions(+), 34 deletions(-) create mode 100644 .vscode/launch.json create mode 100644 Asset/xarm7_robot.urdf create mode 100644 Robotics_API/Bestman_Real_Xarm7.py create mode 100644 Robotics_API/__pycache__/Bestman_Real_Xarm6.cpython-310.pyc create mode 100644 Robotics_API/__pycache__/Bestman_Real_Xarm6.cpython-39.pyc create mode 100644 Robotics_API/__pycache__/Bestman_Real_Xarm7.cpython-310.pyc create mode 100644 Robotics_API/__pycache__/Bestman_Real_Xarm7.cpython-38.pyc create mode 100644 Robotics_API/__pycache__/Bestman_Real_Xarm7.cpython-39.pyc create mode 100644 Robotics_API/__pycache__/Pose.cpython-310.pyc create mode 100644 Robotics_API/__pycache__/Pose.cpython-39.pyc create mode 100644 Robotics_API/__pycache__/__init__.cpython-310.pyc create mode 100644 Robotics_API/__pycache__/__init__.cpython-39.pyc create mode 100644 Sensor/force.py diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..05c523d --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,15 @@ +{ + // 使用 IntelliSense 了解相关属性。 + // 悬停以查看现有属性的描述。 + // 欲了解更多信息，请访问: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + { + "name": "Python 调试程序: 当前文件", + "type": "debugpy", + "request": "launch", + "program": "${file}", + "console": "integratedTerminal" + } + ] +} \ No newline at end of file diff --git a/Asset/xarm7_robot.urdf b/Asset/xarm7_robot.urdf new file mode 100644 index 0000000..6874b74 --- /dev/null +++ b/Asset/xarm7_robot.urdf @@ -0,0 +1,359 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 100 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 100 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 100 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 100 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 100 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 100 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 100 + + + + true + + + true + + + true + + + true + + + true + + + true + + + true + + + true + + + + + /xarm + + gazebo_ros_control/DefaultRobotHWSim + true + + + + + diff --git a/Examples/display_states.py b/Examples/display_states.py index 645db11..e27d532 100644 --- a/Examples/display_states.py +++ b/Examples/display_states.py @@ -11,7 +11,7 @@ import argparse import time -from Robotics_API import Bestman_Real_Xarm6, Pose +from Robotics_API import Bestman_Real_Xarm7, Pose def main(): # Parse Arguments @@ -24,7 +24,7 @@ def main(): try: # Instantiate the robot interface - bestman = Bestman_Real_Xarm6(args.robot_ip) + bestman = Bestman_Real_Xarm7(args.robot_ip) # Initialize robot if not bestman.initialize_robot(): diff --git a/Examples/move_arm_to_home.py b/Examples/move_arm_to_home.py index c736c76..fcaea3d 100644 --- a/Examples/move_arm_to_home.py +++ b/Examples/move_arm_to_home.py @@ -11,7 +11,7 @@ import argparse import time -from Robotics_API import Bestman_Real_Xarm6 +from Robotics_API import Bestman_Real_Xarm7 def main(): # Parse Arguments @@ -24,7 +24,7 @@ def main(): try: # Instantiate the robot interface - bestman = Bestman_Real_Xarm6(args.robot_ip) + bestman = Bestman_Real_Xarm7(args.robot_ip) # Initialize robot if not bestman.initialize_robot(): diff --git a/Examples/move_eef_to_pose.py b/Examples/move_eef_to_pose.py index 17085a7..ef1d14e 100644 --- a/Examples/move_eef_to_pose.py +++ b/Examples/move_eef_to_pose.py @@ -11,7 +11,8 @@ import argparse import time -from Robotics_API import Bestman_Real_Xarm6, Pose +from Robotics_API import Bestman_Real_Xarm7, Pose +import numpy as np def main(): # Parse Arguments @@ -24,17 +25,19 @@ def main(): try: # Instantiate the robot interface - bestman = Bestman_Real_Xarm6(args.robot_ip) + bestman = Bestman_Real_Xarm7(args.robot_ip) # Initialize robot if not bestman.initialize_robot(): return # Exit if initialization fails # Define the target trajectory - target_pose = Pose([0.420434235, -0.00581615, 0.459876038], [0.7071068218077394, -0.00032550013355177364, 0.7071065907288765, 0.0003255002399235735]) # 四元数 + angles_rad = np.radians([-84.09874516930229, -83.31184493346971, -98.6411015590766]) + mvpose = Pose([0.5414959720000001, -0.0031613120000000003, 0.302334076], angles_rad) + # target_pose = Pose([0.420434235, -0.00581615, 0.459876038], [0.7071068218077394, -0.00032550013355177364, 0.7071065907288765, 0.0003255002399235735]) # 四元数 # Move the arm to follow the target trajectory - bestman.move_eef_to_goal_pose(target_pose) + bestman.move_eef_to_goal_pose(mvpose) except Exception as e: # Log any exceptions that occur diff --git a/Robotics_API/Bestman_Real_Xarm6.py b/Robotics_API/Bestman_Real_Xarm6.py index 2e0cf10..a50d268 100644 --- a/Robotics_API/Bestman_Real_Xarm6.py +++ b/Robotics_API/Bestman_Real_Xarm6.py @@ -50,30 +50,34 @@ def __init__(self, robot_ip, local_ip=None, frequency=None): self.local_ip = local_ip self.frequency = frequency self.robot.set_mode(0) # Default mode + self.robot.set_tgpio_modbus_baudrate(115200) + self.robot.set_tcp_load(1.917, [9.12, -2.28, 85.08]) + self.robot.set_tcp_offset([0, 0, 318.1, 0, 0, 0]) + self.robot.save_conf() - # Parse URDF for kinematic chain - current_dir = os.path.dirname(os.path.abspath(__file__)) - urdf_file = os.path.join(current_dir, "../Asset/xarm6_robot.urdf") - if not os.path.exists(urdf_file): - raise FileNotFoundError(f"URDF file not found: {urdf_file}") - self.robot_chain = Chain.from_urdf_file(urdf_file, base_elements=["world"]) # Parse the URDF file and configure the kinematic chain - - # Define active links mask for xArm6 (Base + 6 DOF + End Effector) - active_links_mask = [False] + [False] + [True] * 6 # Adjust for 6-DOF robot - self.robot_chain.active_links_mask = active_links_mask - self.active_joints = [ - joint - for joint in self.robot_chain.links - if isinstance(joint, URDFLink) - and (joint.joint_type == "revolute" or joint.joint_type == "prismatic") - ] - - # Validate the number of active joints for xArm6 - if len(self.active_joints) != 6: # xArm6 is a 6-DOF robot - raise ValueError( - f"Expected 6 active joints, but found {len(self.active_joints)}. " - f"Check the URDF file and active_links_mask." - ) + # # Parse URDF for kinematic chain + # current_dir = os.path.dirname(os.path.abspath(__file__)) + # urdf_file = os.path.join(current_dir, "../Asset/xarm6_robot.urdf") + # if not os.path.exists(urdf_file): + # raise FileNotFoundError(f"URDF file not found: {urdf_file}") + # self.robot_chain = Chain.from_urdf_file(urdf_file, base_elements=["world"]) # Parse the URDF file and configure the kinematic chain + + # # Define active links mask for xArm6 (Base + 6 DOF + End Effector) + # active_links_mask = [False] + [False] + [True] * 6 # Adjust for 6-DOF robot + # self.robot_chain.active_links_mask = active_links_mask + # self.active_joints = [ + # joint + # for joint in self.robot_chain.links + # if isinstance(joint, URDFLink) + # and (joint.joint_type == "revolute" or joint.joint_type == "prismatic") + # ] + + # # Validate the number of active joints for xArm6 + # if len(self.active_joints) != 6: # xArm6 is a 6-DOF robot + # raise ValueError( + # f"Expected 6 active joints, but found {len(self.active_joints)}. " + # f"Check the URDF file and active_links_mask." + # ) # Configure the logger (simplified setup) logging.basicConfig( @@ -588,9 +592,9 @@ def find_gripper_robotiq(self): # TCP Payload and offset # Robotiq 2F/85 Gripper - self.robot.set_tcp_load(0.925, [0, 0, 58]) - self.robot.set_tcp_offset([0, 0, 174, 0, 0, 0]) - self.robot.save_conf() + # self.robot.set_tcp_load(0.925, [0, 0, 58]) + # self.robot.set_tcp_offset([0, 0, 174, 0, 0, 0]) + # Self-Collision Prevention Model # Robotiq 2F/85 Gripper diff --git a/Robotics_API/Bestman_Real_Xarm7.py b/Robotics_API/Bestman_Real_Xarm7.py new file mode 100644 index 0000000..0855625 --- /dev/null +++ b/Robotics_API/Bestman_Real_Xarm7.py @@ -0,0 +1,85 @@ +# !/usr/bin/env python +# -*- coding: utf-8 -*- +""" +# @FileName : Bestman_real_xarm7.py +# @Description : Minimal extension for xArm7 based on Bestman_Real_Xarm6 +""" + +import os +from .Bestman_Real_Xarm6 import Bestman_Real_Xarm6 +from ikpy.link import URDFLink +from scipy.spatial.transform import Rotation as R + +class Bestman_Real_Xarm7(Bestman_Real_Xarm6): + def __init__(self, robot_ip, local_ip=None, frequency=None): + # Call parent constructor + super().__init__(robot_ip, local_ip, frequency) + + # Override: Load xArm7 URDF + # current_dir = os.path.dirname(os.path.abspath(__file__)) + + # urdf_file = os.path.join(current_dir, "../Asset/xarm7_robot.urdf") + # if not os.path.exists(urdf_file): + # raise FileNotFoundError(f"URDF file not found: {urdf_file}") + # self.robot_chain = self.robot_chain.__class__.from_urdf_file(urdf_file, base_elements=["world"]) + + # Set active joints mask for 7-DOF + # active_links_mask = [False] + [False] + [True] * 7 + # self.robot_chain.active_links_mask = active_links_mask + + # # Update active joints + # self.active_joints = [ + # joint + # for joint in self.robot_chain.links + # if isinstance(joint, URDFLink) + # and (joint.joint_type == "revolute" or joint.joint_type == "prismatic") + # ] + + # Validate DOF + # if len(self.active_joints) != 7: + # raise ValueError( + # f"Expected 7 active joints, but found {len(self.active_joints)}. " + # f"Check the URDF file and active_links_mask." + # ) + # Override Dofs! + def update_robot_states(self): + """ + Fetches and returns the current robot states (joint angles and TCP pose in quaternion format) for xArm7. + """ + try: + joint_states = self.robot.get_joint_states(is_radian=True) + joint_values = joint_states[1][0][:7] # xArm7: 7 joints + joint_velocities = joint_states[1][1][:7] # xArm7: 7 velocities + + success, tcp_pose = self.robot.get_position(is_radian=True) + if success != 0: + raise ValueError("Failed to retrieve end-effector pose") + + position = [value / 1000 for value in tcp_pose[:3]] + orientation = R.from_euler('xyz', tcp_pose[3:6]).as_quat() + + robot_states = { + "joint_values": joint_values, + "tcp_pose (quaternion)": { + "position": position, + "orientation": orientation.tolist(), + }, + } + + self.log.info(f"[XArm7] Robot states updated: {robot_states}") + return robot_states + + except Exception as e: + self.log.error(f"[XArm7] Error updating robot states: {str(e)}") + + def get_DOF(self): + """Override DOF to 7.""" + dof = 7 + self.log.info(f"Robot DOF: {dof}") + return dof + + def get_joint_bounds(self): + """Override joint limits for 7-DOF.""" + joint_min = [-3.14] * 7 + joint_max = [3.14] * 7 + return list(zip(joint_min, joint_max)) diff --git a/Robotics_API/__init__.py b/Robotics_API/__init__.py index 042dd72..a2e3d1e 100644 --- a/Robotics_API/__init__.py +++ b/Robotics_API/__init__.py @@ -1,2 +1,3 @@ from .Bestman_Real_Xarm6 import Bestman_Real_Xarm6 +from .Bestman_Real_Xarm7 import Bestman_Real_Xarm7 from .Pose import Pose \ No newline at end of file diff --git a/Robotics_API/__pycache__/Bestman_Real_Xarm6.cpython-310.pyc b/Robotics_API/__pycache__/Bestman_Real_Xarm6.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..0934c1148f09b06f6c61e1b1f50c23363eba828d GIT binary patch literal 22292 zcmeHvYmgk*bzXN*&rHwk>|l4XAOR8tn=f#O+6MrNBD3ZL&;lSKQcDO}NE9S`s&{Vh z>@4QR?j8W!o<(SaidBgb$En0F+j44QId;L7N>b%Nc2X{>l*Gvo|41dOQqrkBREedm zm2zBB*@;;(k?)+_ubG_%U?)FPi5HmLkGc2ueVp^1XHRE*yr|%>@qB0f2OEm=AL%0f zm%+tJeB2LE2<5UORG~H1%PJqW%Nidumot3SFK0zY=*?U!cR8n0-E1@8GAm-t@3Io>K?F7vt3+}E18JQ3ENygd23A_}7Tp(2VmwaZgNyQoyg{NFD= zWu83SXxhtG%QofTqWNjt^;%ZD>eyDZdfjqbpPKJ(Qq7e{T#M>1nkSB(_~hL26LZI3 zG*6sZr2o%Ty;D7Jz2k)aQN7FS*7b&Y)cm5=HmSmymepv+-=l(075w#Hr)AAsn^wDi zvQAazYn>LFZHh*_e&U6bt2nG%Ew9s_w?&V8I&Hf(r_uEqowgj(JMX+hPq^e-^=L*s zRJy-|byt>mRFoAp(9d^V8%N_p$FrydoM)HTtp=SKmoA(>`&OfU1?Ty9PC2bp=g-xs zdHOHj&?D&5R=)BGmc#+^5RT*GVQ~=0vN$9j!Ev8>R2;@}LcAd!6G!l*N%6Qaac4@L z6Hke!ac94HMjXYRY4NO>!SR5oi05#8NPI?oLd>GZ!(vX%8;x) zZNR5Yr?cAe8Z{Fw=ZmtH=Q)klo@cv@d`&%ZtwnPsZibyRJF6RZ%|q7>uVFPCzAci@ zyH4k7L*UN2^QI+)W4rEr+}oOCzt^+dwN2S$t<(0LPScEUn`b4U-9+SfjvxO&sI$drn?A2bq4#GCAwlG^AjIiV2GUp2ytFf7=YHeJnM)@tIc~v4{pWuUf8TXaCb#&^uRJqjxit@PUUAJi^O>2ZebsKZ zAq2wfkVGqPB@<-%%OHo%ZdqPntbQ!Yuiz}?D(lgEIAEK_Ze4+$nP znw=WBzR?W|G4BM1YkO6qw%`z*dG&6iL)WW4x4LTe1nAKYO8ix=TWxkM5tO5=&e|GI zgMw>awX2}PwIEN-1}Y53RxP(t!{N{GtbG)_Ow@Jt)*|?=5D{AcIyF zJ*abdmFvKJtG>I(#9pM`)V3|rs@Xw=*)aE8Aj z(V0LTL}y@BtAsq&>a*y{HBl4`IhFpE@NY_;P>ZVmsIF>iPSft@{$#@NAKeX>^HDD~ zB8liv!wIP!%uYT|{3Bo#WgE;CvVpdpAvVBWZChuC$ZqFimZF->QeRMneo@gCFTeS+ zs%(LU{3-6q5LvH4Ecy$|FES&3LHQ!sY)ieNiu}d!?3#K_sTls>i+gfD+X?Q6RP05=gMDJ*1XZ<9+R8 z|EUo~H{8nr@e$$0#PsQkg}zP_vn6f4_J>@ ztpDCpzpQQPTiL!Y^eY)>&dZ4WP4xp{wkUk4^s{JFq<&5-H`T42r$ai7;cij(ep}nl z-Bz~pkRiE#emj3#5v8wbV*G2`mVsv&+XYeX8xbM*jV~xn^2N<-AbDukO!CI|nAqo) z`h_8q&u$fN6hsyz&u2{X6Mi=)`LniHTZdF7LGDP}<-Lz2tYiTx$jzCJ4ip{KtvACa zD@*6iE>u#GuK8XM(C4&CtCg@@NkqcR9W^L4+^S=VhShe6I2qNKcXG4?PbV~pK{yQ{ zJIUm^NG9_$26mBpAgO#2A9or>9boY;7#usLJgWS(_8wUa>dt?LzRLtw(E@G$SdhJH zHG8}0uK04Z;d&$(U;PSoNWD)h{^AJQ>U9OCDXBA5aOPiTLE*lNlc<{=(zAoi^-UiS zHn2qCtWG-^gPsJ*D^+(bDDi?;c}%W|K$XyIdbSEBB@$x zVhjS#Q<#`DLq&y(=THD2n4!bz$mcjO;hSJ0oS}^+!V=hSkPjg*Frw)?pQ4Ulr0OM} zM7XZbC+I#&vJ^z@AF%;FRB>rYTDGiBtJCUK#!wCQA?={5Wi&0P>L2Gmo+{)rW$kG~ zjl2HQ2iw^Jk|PB$29E!M3;$h~H7vD+3dSed1IZ=j6l7E8B1tf1TPL~3HJ%D<=p^T4 z4XF-psZtHx(!6Y6?Q1u+EvSDEWHhB=Y3J5ApL;V!R_-rgZ0~j4r`Wk(M zlJ1to_^o^>M-5R+*!(d7h#mq6jeDP0?CV1SlG)O4=rj|-XCZ)mD+Z9ycCLm> zxuid+7c5F3B|{iCXGjaFFb^k)09EIz?QpGb&!aDyL(CZ&M$vd8P!ypF=!i`CRTGE%I|mm>A(B>cZ1jJ|KMCn`@~ON{YIU&;o^5*tLWc< zbo@We{k84yy;iqQTsrpLouzL$Z-Bg_tJfeqX$F8$-P?p&a|~Zf2z?S~E0uhpUJumG zK=lJ1hF&wsb{jzTAhT&*bLNRq4@(GXVXs)w(}u)L?oTXP?RLjA>m8H60CtQqb~sb? zAd>vd>d;#O96UjX(QO|ANouX7+8ga;3Md#OvWyWaUe^igM)G*R-l@_^eh!az2^_Ti z1h`mJ^}D&u1Zx0>W=LHiBeeplnnD@<}w*nun6A z!Ceh-VQ6?0YlTjWl*@w5!I6qBNdSv^>r5!HMLJf@FRD16EGJFT|L;S}~^n3UK~3Zlfu77?o3$lXy*p{RSS|J7OF?e65)klCFBW3=Gh69^JR_g#V5O;Ybjj=8CkCaURZ^i2| z$YG7PXu!IQ7MZt!#A0GyJ;;I⋙ItHZj@VQQguO(qBb)s+b4-8_?Wscn|D$FmAz> z0xyId(;ZcU^N_AJo)B(BT>tm9q6}GvAN$)=PJ@BU1FJ}l;#GVE7lOj-U4-z)*aBYmnbU8dtwgJv_C%$7*lmu3?4&yjl(!1a06ZZ3=A8;V zPr%1nz=-`ZDMIl?$VV+vg(G7&BRXrtbeX3mK@0NXGe+pfl&Y)#NVz1d4y+qteXMZ` z8pVEEH8NEX0UCG>Gte#v)-W^=vzy1E-9dV>d6{GYwY#kU zL3;gC5`&y=(Q=dJH?soAhPecT2CI0UY{Lp8%>Dc!FeTtt80w4|J6ZIdyZG~`=_&LD z*`_=YHhdDrE4pn)713|SaMrL^P8~&{)0mUp3X132!oJStPRf1>@nxJW5_(*fTa7Y5 z*m#Pn{UM&f`hsSZ;Sz&iLXG_rql7w3`X1XK`2-?y!d`&10C`X} z`_dY8Chy$=%^~I0zi0n!AV@cYn|3fbbF}A#G}5skx%Q!@FuWl9l!i8J=alXqlhEMw zqN;cDQtSH*)HCbCPtnJV{@X-8)jPJxKc>J|#5C09OKND54w@YM0PnV>s+yhZneh;eBL#pB7D-AOf1$th^tp zkeqxV%DTrh($rnu9vPyscm|-exe-t`L}#h$&UKIuq%}EHoRd_X zqGA*d9UegeaZeA&^pqb#Poo_E_v&qcrbttbk?Nbc@K2?YN@}U(ErmPefCtec#XEUJ z;gVqg>7(SF$iX?0<+Hpzt7qWw(8y!Tbf|k2P1E59$)bPqm=^oljT|&zcuqI+r1z3D zqn`~C&)BR&+#%AqYaUEL&~<(Fb{0;z%H~#`zH{NTo@^XzckL z2GqP{f={D5B3#xr_KPOY1LqR;Gm0^$)1++MYus>#7~&|JXi&fmI1&sP1jQ`54)i}M z;dppZct3j|$c_4~k>MBpk+Bi8#mst&P&SD>yo!3#W)Ytg$d#|0nqX$_I zVz74%!KyCoi9o+*HN12vg4|`K($I|3;|H#n=S}H-myF~e#!?U$kFNd6Q|*xNIyFXr zWOA${=!UF?y<1fZqnvQg1xzK_$DTObx!S49c_cc?bNC(_FpCiOz!yDUwJM}u5S?4K zT9Ak9g>Ui+-N^xs5Z)ZgRtIS@*uwTY1G>v;Vl4s=%^3F4vhaaWe?DeDpIYf#c!QK)+ZV3gm2}kePB>#F$q=P1TBz&QUoA=NIQY4D@ zyWxD;CMWcM;I7#yC%ara6oV;*`I{6rW#fQ}F|;b3_c60!tMWh^;mDle7FTE=$c``! zSeP^S0kF8)4w;oZU)`@L0|_rA=4n-Vd6Au``0KR5|GQX5u?;VGQ*&p%ynt|qQlM@d zw-JTts~d$d)Og3>U7oZIYX#^4n zCG*9iJVjT#p|9RSwh1d6Go&VjIXA*~8xGv|{6%PO4M=80Olr*@s+q`YS&tt@$k>uj zRt_JeJ@u7Y6JA*hsz?tXiJ$Cz4Yds9bF~vV-63iihErb-sMUk#cNOz*<9Ub;v}>#? z(fFNn)a)}<3`Hy`q2MjL|5+-Qsd$@;^Hh9}iuX|j+16FS5@WRU@Q)e)pR4MoPUIrP&-{U?kN85FGdF6W_m#k?f9< z6cHmaG~`QYgE5j2lmF<+QXCH*g%=|qw<~T!S`VWHZC-*A%z?_bmgMYgWT3+k&vJSN zN+ejI%yJ2H0O#;()nRyw{4I*Q7y$Bu^)r~?pNiE|$i8_`cB4HYTXVAtY1$?4)gl;X zL3&c*DwWFjEU|~YnNq>y5fPSGMNeiSE}Cn-HvFpat0Qp?J6%WaWT$Hqw#O}&I|Heo z)WN#vb=}2fIGJxDnbuyLyIP}(Rcn@m2O{eto-d`1&9TsM=i;1F@WX#zVWh7e-O$!7#c)WlR32K zC?lu;R6Oo@pMpZXM0?Ukd%uy`Rl|d2Qjcxi86?C&_ber+DDpE{O|sry2yy$ z2H1d%0QxW+5GeGOZ3RLN7VNgl!VL)kSp_7ny{KfAml3FAtCj>#lJJHgJ6p6+={Go& zi1UVWw-BJGlrAoc)au^-${SE+dt4*Q`hv2XeBqNH}x3>n)WPx>8R?a#%a9Bp7=O&JUUh8x>he*Ns0N6>1dayI@8M5SU=w%dsx{jq4(1ExG>n3EcD@Gfn8Z3hs!VK zi7>r#pVG@B7xTW&pM>r-4hvGYoPfh+vOk_!kRMP?Vf@Cpbh21Kiv^15H%|T_3T6i% zlqYFoG2J0-iJW>wFP9?lg2w`o)Mz74c<;33G(im8kH~Eg1Jwi1iUMg5>}e$J=DyCH zWa!XO5w(L5(JNR!x=oQw1%o1z0fkdir&_Ds=ttZ!0N z#{A?$r`{Lv&#nwq`?Geq%RnOm0m}m1fr1nC3R^1Z4D`N@nAL4nrs*-u0`AvPOGa(7 zh;iE2VFVq=tsu(-ays-iM<`7?dmaV77Q=zHoKjYEcSk1dabuEei6k7Au(!;j>-+!c z4`;soQ{Vb0uQ?>m{D(tA1#}K9ioc%-G}&Py2-Ep7Gzrvse>QRraE23U0RxTGE#V@Y za#}QvTFej5KhLVml3f5>G|?XM%}Dw+Nq2if}NMQk1Ili=dd$ zeNYY)?5hYw*S+=RI?Rusw8RF?3R}ckiYb=_Q-Mj3**{`uzfA;j0^e%oDfT?*cc*kX z`9K^KdRZ+2@~7d4&@Y=ssf`?PXUiEC%5CiG*i zr#P>`ADF8n6M+(jf!~^EY!qNMD;w|wk`9f$L7fwb3DuDc&(3q;KO;K(7aqD1cY z(1~8&^nmGr~7Vj42_D6#6Dp8>f_)N_6r zr;K>4&IcJ7FL=KsD>HFQp{lj!Ae0^)fRXP|#G=|p48fKDM>m*AEgHWb<3E;Q=ZJms zCus>J1%#r@`P&%9PJx(cH-aWz3^fCSPIB(BL_)l&gZyOzo1lR7#NzokufmUk(8dsJ4hH>)KKH z0w$p9(knaCCwn+oz90UL=zakINVot5B>s`v$1kO6)(Zp3U^Q#-ap3hnBKdG}%|wQo zGw0r{gbs4j#bEH_fIPgq*=^LW%pv?W*KN|vm3U35(WS1!)HQgW!nY)QaM-~89=+~1 zkjAzfP$KUMt7S45C~~t!9wMo+MXI6Pk?>&&4-u|#Un%Mz(Ov4>IEi_W9+x9!h|Qq! zfq@1S!_OKvcBbHVkV{=X_ojR~=Q*MVGFx=Re`bV%v%AHB8b=#UpCx=;(k!Hl2Leqd zaA`Qh+mAx+sm>vC;m{H9zu?|Ya^fJj1XrA+$fS|=ez3=(Or6{YK?L+(}+3BY0qg#r0S-+3{zDZi{(qB z^i~#P{%yPrYKvI{P|A3uhmcwwJ}43>%l_mWc$v?nx9ZGr_$&TH@bSo+17C#RkAOpz zkq#6|u3$g8^`pClNuY6uyz`MGL??A9(BaW=hWCAP4N6+vm>fYb(YCXNnlGvoZ~*FH ze*^}e9zg=T8V^tav3E=X-nchMbp<9ZOa`;{E7&42Tpaa>!xBW802*ZcMj{Ys2S{^rxOkK zB88_TDu~vEyz?#Oj?C^J>d;t$t`ME!#0iMeaY-gWf|H2J zh05=&!BRy87fD~_3MWBe9YggP=#V=uZ6OlLP=c)Cmyt?1@7?peLH;qPuZdD_e8?dH0RxHO?QXpwurZVB5CG%PVm|i&9NolrJF7%uu2ZVtM>?EUX{E5=V@^5;vgf$ZVdA zc*}qU@R#UUSeDSvJl(0`C6(~K-FIi^<5(LaW%%6@1l$mJo1^mzF)T<9zjo40xmbCv zhMtXIGzlLRkvt-K$z$SyRn%br2vTJv-T4@4@}cxc9!dBF648>OB8;SWHxQ3wCJV+R zw7Av~-uf_Ha%O2%BSI(nzH{TfeuSLmsS;F5f3IB9z1r{G#X!zFe-q_E?i{HK^m!8> zH;p7~NE@XH2)ttI*Ytkwm({;YZ;5EuH!+fUiC%{{MLJZX0ZZKg`(TqH3Gq3Ms0mwy zxWbFE7|ZP;dApZp0Kq;FxwvYxQwKytu$gp1BucVB`JSa_Tp^K+;V5{RixK#RqaIX7 z&|f_9uoVl5q!nKd=>;QNQNG(E89*R*yh_wD=?2p;QNJW6gtA-s0V9fN0!99*Fpv9)4705f&JbRUi!|1mLd9Pv zekLX0$i9)r&8RkeDnib`rqO5pHUJ( zK#Bb}#cVa8-+Ud@a{e6}jCOm_5C`hlNS}-TNI8t~=!fL$lbVe{4vWMiMx$(qM!NMx z8jVQjV*wK3dboz*`K}qh&y}dhV~%m!`TY^pd|(j5LvsFygyMgI>LXK{M>eTw%Sbd^#vdf_YE)dIqDzHC#WgA@_T>0fY*F!5D*iGRe}jszQE{7!U#H?9 zQ}Iu!_#G<#ITinciho7Lzop_oQ9+Ss=Lb~$S1SG+6)YT+!lZlT%p_-=dlrSE=xVWK zEE?0s)5cLFZ;TryqlnLhF=b2^4;#-G_Zb!A5#!cxkQNiAEv zs~srcoj*d4fd&V4#@%$CqjZ&p)q*syU%|P|qJ7w{HM*PgZWjR@{49z`NtQs_ zmP3vNj^8=IMGcD-nw#h7qeD^|KOIFk=*?UwN7p5CV9Z~0;K_nBoCLM=FcmbvQ4 literal 0 HcmV?d00001 diff --git a/Robotics_API/__pycache__/Bestman_Real_Xarm6.cpython-38.pyc b/Robotics_API/__pycache__/Bestman_Real_Xarm6.cpython-38.pyc index 93a9c818036545a199fcb3f51f1c78f5729073d1..4dc7b57b5a4b515ac3ce104b24e8ee86befd5c63 100644 GIT binary patch delta 3224 zcmai0YfK#173R*)uJ>tPi%B39Ft%YZHm@3}Yrq5>>|n2xn3#I~2lBX)PScaUgP(rLPbe&xbce3}wy(G?Jkpt|* z$lj4i)^JbQ*{ngmK+eu8vL`!6Zq^G^`<(4+W}e1*B%0u`RzZ{e>)!{v=j?f-+qJDM z`@^>0uCS(=mg#C*J2>6Lp&}9`A@LV)l!;6vDfET(-%xm^k&TVi7XBLj3yD{AHBUCJ zS)0b#FXH31H4qR(#PFf+SBcNawvR%5VPzNVZLVeU%Be1JP$W;<-{S=r_1r?41$;yg z?pBHxwS98;OEobW)B|vM~PwQG^rHuYm zcZHA|ma1v@STFyr@$LuHc`To(yqWl6UUeMfxVO9HhFH zHFyUZmY2}euDNJvqI}L8@lz?K@<2n39N5Y2R$>pM%UX_{nmE{^NPa<33jU0+Y9M3J zmW6z-@d`O}f-ljN96Z_5=Z&J77d>uej3u$hB^&DJpe96pcoLeh+FZ;+y6Fa~Zh>#H zXM{hc=uVbwK7K^(*X270^nQ?&M)feG;M_-!`|(he)20* zi<*vU@u^{@4Y5gHWO_O8PbDh0ZwyZE2Z*GLTOp>Dm=1i?*mw5zG!)mi)ZndsH>6NK zytK20o|agV&tZ0>rK-Cfo~UyQEd;H`~yw>`jpY}_3(hKed{iv}(0 zga3@}CyJ9cB5qm4-yFFrg>Yi|DV>~b8iA#nM9>loUKiWin8>@K3=1#FTZW%m z5qCZKE0gaGUf^q~wE(@VNT1`9HKN8hTxM<8E@|X-+grzuk@mvBj$IE-3Kliu1~xBh zn5+0zvEXcQcoE@4zD!MQxV>a%+_^G3@12+<`NI7tBY|0wBZ$xp;Cmc+ar|^Q0wHZ) zAU|FI9`J&`*Do>mLKpaf$(GfVz4qYWT0{ZpRfKsSuz8CLuRQci>F5ti82CS(c&c{Z zu-5$1;ScF@#`K5qS5oi-oBo}&x71Z)Bbjs#zmKkMGGI^7oKKyH5G)mv;zbaA6(Wuu zuQ0nzzpC~0Olus0`MeY4qyUo00p>! z6~HRsGT;i}S-^Jz-v_)zw{~xOq#~T0u delta 3885 zcmai0du&@*8TYxiuV1n2yxTNwoh41{G_l*})r2-n*ECzo;=a zn=}b-(Ay)Xl<0N*x zN;tZ|oco>ceCPK)&UfzXuaURDOZ*r7ey<4=2Y>VB9EyEaxaQ<<6*=cBF%;9UIC9RHZTEy_vP8_l)V7>caVCi( z-V$;)DX6$$3{SmHal^<{W2?7a6LVtDHWidA6|drZ!G`0hsv+AlffdzeP42vHs(Kle zr|tA@vsS495tqDdQ~dYre51Oldb4h7yAm+#R(7n~02`ETt0XBX!CQ8M?>m*sRW#VB zkn^^16?@TncX3A~5@o6k(Ny@5w=@i&CoK2DQ6NY z#cku>DP7B$k~yDM*91$59E>};WX(qXF#qqt=%k)jqw{hm`lMz~&WuN&Pz`gGg3iQ$n`75puNJT5! zb7+|fomMf$bD|neT0||~GXJLM??j*;_L?uzid4b^tw1&BEIMDR3R1cafE}y&DXC&U zf3uS|<9olqDT;g10|$8VY zUJ{o4ul)}NUpGqeY|=LNhu~wRi@7R$f>kJ}Mc@pgY_zhor^tg-aB>QCb2}(D-B4-y zHtc(qcfhYRRW)QEQ>&VMr4(*gEt6LCB3&73aRWwsyr{) zL}O4&f89AU#PjkAoZ6&VUKx&pXM?EJkMJ@3xkf0oP5WRPv=$mpu#a}tGrIfmfh4?# zOU)dAlo^m&jqLfxYSzBTlmGMXzYx;Neib?vEU745*UMU)&M$J}NAVqNo7xeIE^O9N zxihYUGf#jzbbtA(#7nHGeh%%FI|wqX2KtD!(H~Sb|F5PGNgFy!F`RS=AVyCDjwL7f z@BPPz*Bq7;$^_a_7Ynr9t1UkSxbQUlQA=%o*-3C0SoX6ITY73A#b!IZw|9H~^xl2~ zC*fvmLoF5+%^;Lk;P+c^2(EQ%Y%tt9I*b}mB5V!n*f2DJ5tN@o_$0#92%`uw1T-FZ zrf20uf_9-mXZi5hGmrEMr}3WSX|X(GaBmh$mnZBD&YVTyZWn{kTM};tkS3OFJJ?X1 zE003{LEDNTwxN|?cBvy&(qT@6FusHRzI}%SPcysU(Y&QcyxZ{-8Rn_mJPu>SGzF_- zVUbS4f6I~3v-7m*8=9n908rrP*CE6GM$_zmG#b2ulciyQFZu&}2MS>O;*oZ0c!a4B zwJ&cj3E*fO`(&|$YkTr1(oVX z(L9aA>Ub60Ay;LpNYh$I<)1SKJ%$rLe&2$GYIAe|_beV3es*~5i=8E~UV*r_lI`l= zPo8DN-Oc0(o9ez4EGFYJnhNCK>5kbeKLg_!MK9M!0SficVJ71JEU2aZb|C<0}N z+$YbNx};|0arl4&D{$itH8$~cj$a%6JX%i3H+-a)?tW|N7rnEf5L>g;O-1@8kgSn- z#RiL{Zt5jf$PEP8-IGs|-u!zf@7kfQTpwu=`F3~Mt0OyuC3_&xMhIRl`^`v* zdF=Jv1>A=FK^crV>(SB1PBddVuWE`pS!hyQ^(=83iZ;0=F6@=jcS76``V}<#RfNk3 zB}DSKV&4|ruY#0sb^L5im@|F^7rIiu_WIc$3I{jV!erdfzizZEQX+-l9p5Nh0Iyap zdt!FF z!Z#6$LFc0Ydw4->T*H|<0LyO78x#z{4Uw5iL+e~(OBeTv*m~$^+3OdNG_>H*jqov3 H|(K4fCK@O%^SEw?Jh19Mb?@W0O3MHq?Qn{6sfV~N$;HA z*;&jD-8}%dYZj>xR7rk-E&IVPS(3dFeS_`9iJb>IsYEIz@q^2iN>x(js(wjSsaQsw z%7YUpG0PV6{r~CP%r-8T^N>osz?{C!Ij7I%|G)p*V@*z$75uec?5+RlhNApOx=8=! zaB%`3_k9#XsVPDgT3fBDeAH?hA9J-FAN5*ZWXbPBbCN_F$?V#lbJ_*`q3Yh~Ok zv?n^1T7~Zw+moHC+7zD~?OmPe+H_cVcWw9UiYSTl2Z|`)&}uV6yQowr{Qp{h#yoMh z)wY+dj%~`nCG%6Z>vgPd-Lb89{hH-;KC#%}q?#+OxE9r2GLIcO_VIsre^WPaZ2npELT$7;3X?@__03jX?_*Rd9@O{?2H(WEMi zja~=MHbtx3JoeIwRUFo>j@Roh+G4;xownVE)9QPzURMt3z4zXuC%ofY&1gnERJwl$ z>#i){QBhXaKtJDeZ5)jYJL8Y|0)Ry{_l<+Gc#)TmzBQ5c#o5yLPX+gh}Dh zw4Lbjm>h1H9XtV)>2h0PuMV0`5VdJ_h1uz0ggpnBNmtx+-35JOnzT?D0|*q|(z9Jz zoyX7^IF8-*a1V9O-dfhcIz3@8nHSLt1E7INje33CVHUt3gq=LT3D&|_a{l2(G$9&5 zxFl_w0`+HO8ur>6mb>T3UX~lgB+(z8=kYm(kNXuAo-$N#s6(X=3`D67HI%tw4y8U+ zHq`CB&~B-!!si8%i_eQfZz>}HE%lZberw>mfa|Ds>6W4@UU_2zpNc4Ydfa z%^cm|o)V>7^vqpC1s0b5pNaSVTudo)m9BvT_zt_c@gJj~iC!J=B z9%GuUxBBLs>p4}qlnJ#(JC!^mS^Z^)km`=|JymIL-Z^yC_^lrvujZPsmH+6&{%cR4 z2v4-{Hox?zpZ)d|XWltcEpQ7in!oTh{C(d&k=)`lzxv#q_Tx{aUR3rqou>^y^ehta;JHifb266j)Y-{c>!7tw+@`) zZ%Aw=5C^dt81*_KPrd#;dU8z^N~x?C_+Le>s59!cT0Wqwx~ge~yV_qB4FAARuw0CK zX%I<7e;Q6mO_hg8?S0Wbuy7EnC#CMg?gUz#8VT49|`zM{ouvGW_3;8DX4>cy-Oy zO<=uk5y+Wq)}ZaV%u;sOT4fMvc`07HV0#0nJH}3{y#}l<(lrG(Ez-z z4chI^G~CadXRWpiPy|FdS6glh;QVJ!t=o-DJRIOzI3(y239K(J-l28&pSqaceNuJB z$Ah3u2`R}Uag$V7GMCR*%g&?dF3?+DFx4bZYZJ62-MZ~KJ*QTd7gsH(8|cKPLEdfK zHhAaEwT8`#PLSuSL5=`LbM{krs?DS-ivx6LIuUqvUVIC8-B}chp@Hzq{I8-Gv?+C; z>OYe~bmKjY5FZgKC*veJelIg?=gL)V!Q1-ca8MW{c7X$}o>MW$Nd&azouJcsiuR1n!n)@3*w= z!YyU12pLis7PpJH6jAwxCMLh3Z5eomv0V~VLn9*Oq4BQ5BwybAFCckn)&A35YeMYu zD#Owk$>+C9*Gt{9;3E@!9t1z__hW)TYkQ4#$W;>Mj-*`P`AEh}29ToMoZIL@)j{26 zJ8ZIY>b%*9LJGn)FAo5IPM36A3B1)rD6HI8gHp?_JC$sk{(LWq!uUK2i@PmoMVONAZ_3Xlu|Hn53l4P{CPzl?8?SNt{HT+`&_oAa`xk z$72mF5V)$>4JM!`LGntqT?;C_oOK?N>+kg-F?cQKl9jCb`xvdIg5;X=`2wcL<7bHR zahhE*~TJFsRU-ctqvT(Q%|Y8c<OU-$W^#lX z{-KB4)e&MNDlS{7@ZgH?%FNk{boGy#Pp1Edew|K#&UfDa}Il6y^AAB0K{S?N^2xeS?zj zR>b7ZVkkomF(secU4pbMZT=acLl2>YM!m}`5A`wV$ZhG@bwCFn89Fd0K*#rD==e|HDZ-NC-bFSD9*Xj>E`jWZBoCAh*Y(R)xrG7fCb{m3( zu49vh^-X9k^YI`Ew-R#B$xxWEaV?j?Wp1Ny)GoXZA!p{ zvc@(Pef#|4@<-LIrqYB#+t@6z+KW(pzwgifgV(HwwUeVQY$V{36z*F}&VbWCbr3BB%akf$|2I{py z-3(Me&|&1YgM7aQJP&f4)>Vg8H~*l7j}G>V1w3s?yyX7$DXZJSpkq*lfbQsxo5s0MLTB^L!PNqPD@gdLnknnk+6U=4ucd^;4(@4ICN4o?e zN>S6Mp>}F_^;`i;CrN(rHne~WuBA30)l%&*3Z-ck9R64ahlkxZX!Ow^os$STg^xRf zf;konz)&OeOhC3&)n%wEr0zIWp_=2F)Ta8@Ch7L|t9Glo?$suu>qf6Rj#z9A;CnU( zzC({9(WPkqlNrOLWsd5OAkfHI34?(d>bIb-;HcrKd5Xs)zoFeA!!VcPQz)b?QKeZZ zDH@#B0tbc$H?dXdtVp>k$PCO>W=R^bs{lvtV$g>^FmGOkq6)cfHQ)lFTan!_AKU>b zbOs&PG&=B`(K(Ag=5B7qV@vq7TKK_}Y$TSx8=m~dcbxN>yz^Nq;or$a!HSp&FMmCLha z09EsTH^#&BFjBCwWC+NeCV&K#T>>Uct&H@uw4>#DD1Whg>YjVP44*;#5dTRK%zgS_ z^7C)eBmGI!O8i~say1Wtq5%dayF2KR_r?w;q|k4W3lo+*+PQ&##ex)&+!m-U+P3bc z@lDyoUDYvoso|W%1ph5oGTxWjY-yj-Y-DT5Rvp0w;w_KlFgAt$5ia<@7_Y`Cf3>=z z1$!=9V%`K2h>1P*Fv~s8Vo|Ty!eno=x}^=IzpC!6U>@*lKwG!rGqAhCqy+~Gd=GL= zw^a$tV;a_ELbfe&?cdXSGGG~g>|sy23PviAtYV~~GlA1H1Q}OSYhq%SmibVk)lc7J zHHF=DY<3L72V)EP)#pyXeYP5{ZrcBp?qR0^4x*DzEYRI5bOLaI?3;Hg>^y-UXAUFw zC#3Mi6CoM3L;;S>*PQ6BjZ zVBj^(L9ZNH!_X+qZ>~bKgY05INDe-eoCm{lOtaIswCfrHhu|~T?Ieu6pI*b1%pf;g zwB%&*&8@(lVV;61gOxl_Her=9=0RQ%7!hzLjCDq=oh@hs`H~ z(con;yuuKW71eCQ>?n{UEWeez4rN{*W3NET|J{3Rf8+~@1PWXKX%X_Ui1y=a(3-q2 zj_3|4fBt>@XY)Y158Sk4!8wF=OgR)J=k7Zd1{P$W(!_?U}R# z_pAwLnZCNjP!iy5@n$Nuj}i@pPYI22`IJ0FjZ3OF=!h-Xn36u0p&Z_;mmi6$*vBRE zp?^zKRpFtis(25o(nG4^V+<;W|N9R`RdD8qE`kVXVx#gws6w*xp(yJ<&q!032}8?t zZ5Vwbo6wdejXgF-WAO|?XA2onHAZKt>dpp801}(rC=S!okI`8c4js&(g1DzgVtRtr zM4phCp0eEh_v>kdrbthXk?LRK!hb)FR8mJJA1NFmFL)3wq^o`tV&{FBQtVC5A0W3x z0d5I-R+MMW99$h5`AL}$b&q0bIvgN*^iO`$@-V+qfZhv#=|+*XUUFj$^C99HoBtf* z4p{~)?sRxcD{y2K62xP-e`%|59RY0M9iK5};;K67|K0;AldRMo)ChHXG+vnQWT)P7 z9(@qr(J_iaI(8qSOh`}i+l>bx(LbImJtCyLr>yKmO0G!MBTdymmO*Oi5wiDX-el)H zEHa9vj` z;r;vrAUEo>!Db)zi9Z9Mm^JO#bWi|*WhF}lL7Tp>CrXVVk{Zbyk;^rTKQQ!^aU;(wB z^o${b6p?DcjG}K9uNUF*<6{iqQ~s@(!pA&*GO>Z&WtQpo?M7=2cgU9p-5@P@Qvkj* z(99J>AGkS}!g2~aTMl-0q%X3hLMN%gp9xOY@QLZ%C`0tO8pS`UIc@A2z zPYj`{K5U6VziPF-bm)PcWTeW_jMC!=o=+{B(%&u_$UlgsAnwhs{qd9Ckl%U@Mt$UN ztRuvRJcWC=s+O``Z_cMNm0%b9-)!efuP*12=pxVHduYBaI>=!bls#UxI;34KI=32) zS`khczR4%J6BK|#2xexo)1$N)Y+HN15uN2Uu@tc{=`3TOIe6qW`R^|aMS~dYZb9D% zNmNiPcl|7#=3Y-3kw0Wg64af-$E~1X_TX3@o8lkF1UhPpM*w|O7gpCV7R-wEO4 zwm2d7LwC)_IN9UUjaZvOP`^z9QZ@>h5JPLyxr~{OTa$a!s77W4x41-mK=uQ9pZvK8 z09V}XjzEh$U)iH5Bk3(9-f2~Nd69dk`2U&~_-~3~6wUAoH#GM>uP7jtp$w?o#w|o1 zhU!KsjBb#iS2hrR5D1WRbVHA$8#>1^DB^*18pJiA4seO+27;wIig1Xjo8>UZfdDA_ ztw33r#&I%2#dnqNtB83_eO28=O;#2tU`iOc+uOB6Spd#nFE9q;7-Qg!|DR(L1LHGD zjOhUq1tsgn0z5@oJAtp+Lv{(P7;~f?ggG|Cc3Tcy_54L>V~xmTL`fR$0jinEVp)$L zMY!0J&Q%T>q`mXic@y4P3kt{pABmpqbPaV2vsAoJ#T!(dqk<^EmhW5vC^14i4sI~UVkJgq z4`im50U~SjrvE~sjS!IRs7gy5)<>2PyYb1|l6nZK1rlA$EjOBt3~!WH5NwgY0@h&O zq^fUG!MHo7yf6Ywy`@%)ceOS}cnhZj@TgoA`$t26e=e0Iuk82AGIS^Lb?uP#Ce<~2*LcPY->q=&PEQJ z3vn!`QlLaK1yV>; z@>eZ^Sq@A8DIBCy-JU1bkT+9GcRV6O^12wvjKd{!ZP0}`6~1&NiD8$U$dl~#O+xm# z#d2>X&67G<_q@Klbog)+d7^_=N)$d!KFo#Fl+8wgLwR|A6RF7Eg^Q=(JiOZOtsd@J zE?jMgMXzz#>bDM4c5@vkiyb7*+G`6}8Z@zb!*cLIiepAi^QAIq?2 z6MiEdrndi{ql`i^Gx50NeX5n>B_9`!QbP$Dgk)T_$sZDrkjF-;P^wx1{1E^9NHetm zd2dZ5qqh+@Al08f%m(CNA1d1lL>lbYZIwkE0{(dgB(A-zoCPMO*o`!_9--wr*b&h>= zV^YWobIil;?yY+>Szz6jm9tI{hh-#nUcxcZ8@>MK7%4a|zCi`jSpyxRK-cNxv^E=R zRU`UlZBMArrVRzhL;Z1rnBvjeb>75A|DO>VP}CMm5@2x}xr!hM5QA~oe=0*|8g1R6 z8AX5O>Ln{LhmSjte*Hg@DZrlc6?o=vPzK0BI6iRpzUsj{&-dZFxPeXI(!Q*$0S`s) z1A}UF8W{QIQ5zw?*)Vx5Pdd#INcpOA`O~PE-!>43M#32EKIG$nNfY@sU@8*DqHDg( z_YAMJT^<^@)a?lnVF{UzKtkB6x|6dc-hKMQ{eM1wg9RMYQn#Ny}*x8Mh^o*&c?g zN1hc0)E?T~NV?7g9XiR-p{*pooa{uE6mwp%kmos;I{LXdLu<)ACv!)W7?zxi^d_64 z2d$9OIgs>BF?g(RQd7qMK~k65RD4Mx!mXc**qK+Y=8CBIXas56g(UWnn)WI@eL)#HXFSI1el`HzG|3LqU>5PuI5XR^CQ45rhefvSuCeB>hF{3g-~ zMjEGE!bLXY{2iK#MFqLY0H}WM7g2g<+ojcOnNo=5kGgasW>oS|aV2EL@>Q7(c5# zs>HQ(R1=!AHc%WN{=h<0gVIsFr2xq_&)6uzdR8{z2PAD88G|~f4HLd21D>7dkOYkF z>|cab$cqZO+e0V%)aHK&wo@W82%6;qled>T)clnuLjZ*5JoqVlhI9qN@|r-EAo({!R@sNY?k27f>sxTp+zACvxh`W zOhYCjCAPlvcR{`q0i7@7RBHCvQ6w+Jtif9*d6}0}36-!l2VwQvUYPz4MK9`I#1mZU zlXPp-sYTW|Gq<~O_IbXrE@03tvplQ;@STmsMZW&ErRK)u_$YLho z2}(#}4k79%c~MBfPgC8n#*xzzP@Z^~D5 zULZ=q2WF7}T!z`Rv&D!iM;lBZ-r!H+L^^&T++;GBhBLhXDAa-K93#gL9q|SX?%g2Q z4)RQJ#o3BP8cZH}{|{pTy$JUhzkm|0bo{<%*R-zSy(GM1Wh1EM;3ZhN@+fVbqM}IN zAV)nD6y2q_s@5noGmV&8&U{WmB9%AQWth0iSS(*1rT4NB|L@|JP+QCsfKvV=J%rTj z@IjG4S@w6ofmixWdbiFDhribyFa>1m;^d^-o>g% zxsW=kMu84rhcmwSyVszw#f`~D^a^b~rzsT?@r3ga>JZr9e>Q^*b~GNL0%Gx)3P{(x zpZhOmYMXwC%YieA?I`3!2gXVa0u5_Lhr z1fDdlGfjGf)70s0JNmJKn@E8Y^iEpA`cG+*K~WaT-Qf&KQ?OG0t!TjVrFf}Au@f={ z06TG?wG7Jj@!;q>ADQN%eiuGz(L9H|MtTQ+)%LF1P|c1o67PRL~S`|VbP!IhAg;3A@{JvRqvKvMu~Z|q zXfcvHXPI1AkPMz9vgEKQ1$=@cdC#G#)+Ve0G#P%q){b7lsLc`sr|6kiEJ~XC7}|4B zL7t~NPVBp*hRq&&rrE9$k#j1?6!fFJnV`6%(FjiyD3X_h$$J(jMBXuB=R)Ai`>(%^ z>F*$B@^D43L5vb}bXV>_Njsj>V(UTrRnnu)1i0O_5AhPx zB3z?IAY8)xC|OcrM+0`2jQB>0$uTau3KSlTm|Up*_8Kf#M0SyGNKSGR1lAE$kAV)k zrA`0k|0r9KIa(5+SPtpVhlaO*)*E~R{3@y&Vj=;33BUz|s-H@_rJ za{a~l*+kINK{nFUYe$hGl^PZl0S`<@%olZR9=Uep<mDO5Q<~kAIYf^#fSqh`d+h1~eU+;d2ph9gqP2ar!NmQ)p+N?lthrO8EBf z7v~n^C>&yF`1KKl;t-Krpz|s*EJzN&dD2d~T6wL;o{ir%2_F=ZJYsywW8#6~_k8wb zkSZhTogX4)K9v5*FA1MOB3d$3gpt1v<_rq*9xMx~>DI5dgttBpmz-r9)rinZzV+O? zJdBZZW(s~Z|6aMJ_iBIVE(UVexsGxqmyT2g`n-vcn?{l~q>)l=1YR=r8~U*D%j!?j zTO*qFU5q4NqSxU~kq(t;z*0BBKGH^a1=bur3n1OQI9Gk=r5jl*ouWj(uyy~^n#35lmoX! z1`voHuNL)8y212I)Gvt%LGBKI#)x9wctgI8^&A0I`c;eZ!Gw|=VfOG%KkAm32O@rM zfS(pwf&~U-6-Z7=f%8i+wz~KojKuVV%|&frmLdDM#KjQ;mQKoT|)#gW1gx)ws>Tb3s z6uul9O;NILL_FhsD^n3z6LNlq*6mlRSfzr^xx}WMVOAT_c|MG%t7SVxXpkc<8l>e# zf21ek6z#uVlTc$~5VTAMBXf#gnvm+{dNM^rABu^nLWg2fQ--Yk@ZYHN^ zwf!3r|I=yWXYT)7>gOD>`-nhTEB~K~z!Bj1*8XVp^chhyAq^mrvAS9GgFi+;{KH@f7{mn(UkDw5K zaK)o^PT+ILA;$z~NjN`;vs#%#eTy8;bV#1pN_>Of`gKUA*DB-?S-k4NGY026iFk+d iS9mO0(+-SR3` literal 0 HcmV?d00001 diff --git a/Robotics_API/__pycache__/Bestman_Real_Xarm7.cpython-310.pyc b/Robotics_API/__pycache__/Bestman_Real_Xarm7.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..1988416b7391395fd572e7d95ccc9ca0334b850f GIT binary patch literal 2443 zcma)8OK%iM5bo}I?9BLq<2VmMf>D5oJ$Rj5uw)?w!8nQ%OfV$CCTKKF_pS%#v2@RZ zJ=%?u7{n=uoI@o07XCqw`5k@DB~~su=Ri_5vug}EQhL?BJvH6cRbPEoJ^9p>N1%Q8 zOLsk(B;5sqpKzURu3hpk$61_)i7z$^ktq~+a_MhefGz%=PzmmXXsm7x)!hVjXS`i-fvoOQ7@Jhj zzPitn-GHv1aphck&{Ld)i6z2(W;e+mYgaUu4ndGpgMnh>}pLAb3Xp zURqesQ?XFwS$6^PxeNh8@D|okOB|`-!snk+z5-^# zqSJTt7-V7AO-5U;U;Nzfukk(bq*$DHM^Kk+Su4EckTLg2YiSz)Ad9S9%gmggISXmzCmS}D&H!iUQmI1 z`USMj^4M25vG&~dt0WnRrZ$Ez`Q@qc`%6*+Ue${@>%J7pCBHePt;%ve(-lQfA6z@3 zCsDFzqxU7^R4LPRO?w@gr-A4vkpGQP1!$VKiyZycMv`~68E2hbyO%a1QI%0`3A98T zs*p`jx(LU})A~Sp6h$*5=LJ9r=m@17svku{Dea7rkcVNVJW^@ckGrO?1@g@U!ZQyW z)oB=*IZfx_H_IIA(6`yUlo^cK)O>C~pLJ|H%}!G1TlP=+{{M^nfrP9I4BE=Es6dI? zh5|F#OV0rXS{mi-l?MP%%!R*vWd&)!*pjobshp_e3sQ&{lP5tYPr(553POYDombi0 zz{tUlPhp)xwwY{}nzTG}fE%NwFPV;1#ayaPe-9>{cERX5GK88qnz4cu)uph-w$KH$ z^!Hq->q8Q!aiOZ@Uz`VPTHWK1|GfI^G55F1v+>h;V7yt=ri^<{qjt-!X^_U59#6xK zSN+__**keZ3GC*c0 z^_%LlkqdB!{16AchxOFxCW}Op1i?NFr*H`t6NwSPCx!QoL#C;7+&MO5SMsK-8$rPH z2o7Vpl2w&(<9of$RstUc62qY1QBe#6)q@X%FliMs%oKcmq!RCG>EnQB>^mF_Ty?m1 W&eqPH!|JNwYYWKWSd$cgbL?M3K#pwy literal 0 HcmV?d00001 diff --git a/Robotics_API/__pycache__/Bestman_Real_Xarm7.cpython-38.pyc b/Robotics_API/__pycache__/Bestman_Real_Xarm7.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..2974411e21fde18e86e466d49c12ddc6e1f83c93 GIT binary patch literal 2437 zcma)8&2Q936rUM;y|%LnX_|fj5?BQ)_Q0k+lt>XmP(rGzB|w0ZZi_7AncbN9W0~>N z#M-UY2E?g{o=c_VR<1qu*uP`0y^!|Oa}TKR*}EY?D%Go*9lv?=K7Q{vGrl!B=@NLp z`=z_)HwgIyU-myHyj*~$7NO&$O*rK&rftd%ZpJKOZAMX6i;cu=o8{PwYl+paQ}PMn zb#6Z-+!k!hXxp6KAWf(E#y#O(Sc+n?93;Z4o(1n?q4Fe1eJO(2-w0$f-|B6m=Auxc zjCy&LrKr2$U5U~t31Uxd%|e%xEwW7qbifA2jsaj3FhrM| zOxIMuC#1H0AHd0dziDbyiMXR}ncdBDKk8{a&O*QfMoveH`+bpyn@_2Bd_Rl>rF{P> z`DbBnElb2)o~7M6#ODeW1i_nIK`T+H{EOE0X*Lt zhZkZI#t$+O+a+k~Z|I6&2jm{PN0Dnk&>3dlOvLNloqa`;lUGsXG5?+*g_xqqE)6ot|g1q@+r?mDQ-UO;Uk@980qac-7 zJ&EO6-mPOTPA&zoRk@dE7>Wo($1AtVxHwfXe8DSDjpr{*35b;wQQCc}mJ42UQrF7J^>kO{ ze&x7ILN`#d7o+YBhvw4rj@bftrEj54hp%A+Wn9wjdTLO@3-U03}u6iR7(jD$Q4n(|0#VL$GgzE;XN z4=B$WSg1}yM@&Yi=q#OOGt{Owdz-#%Fvh6$+f4E;SAaN32SXUPyw#LmM z#WE!k__hTr1 literal 0 HcmV?d00001 diff --git a/Robotics_API/__pycache__/Bestman_Real_Xarm7.cpython-39.pyc b/Robotics_API/__pycache__/Bestman_Real_Xarm7.cpython-39.pyc new file mode 100644 index 0000000000000000000000000000000000000000..aa34832851689ecdcc0dfa2888fcbc57304e8964 GIT binary patch literal 2435 zcma)8&2Jk;6rY)0+q)Yl4IzBAMeWir)rUF}heoohs-2+@c4zEtyC3e% zx+E(b3EZkD4xA}ca-&yH9Qixu%7q9{aN|(%-maadZID=No@d{@c^|*`-pu;b(++{> zo1ePtUY(HN@MZ5~!pj91Y8eP8ZNe#M5p7d$a5G{tYcqRUN-hf( z$gr1%X@asX_ez+Au^+i&BNK^2b|;nY#zolT-tm>-F0dx*)==FoPuD?&vNY)~AU;piBtYBl11P^5?{-6r|bmRT@hKZE|lvh+?68h%S5@^x^RQOlp@L8BBQQ? z+61A~cheAfe$tJ`TW(ys=Jrx0+%R$P_d!S|;~t6qtns{8n!5&XK(X7Q@}$p0KarR} zi3wWWsbMxwtoYEZ+|5#SM~32oa7Dt;h)zcY8MGc+dm*E!0gLw&7)9ZB$$;Fh7nvl7 z$@lEOiG>^f4wWqrfDlwLYcA@_13&7Ef>V-NSkunAC{$UH#=Y|`c>fy)$y@tWSHH(0 zT(7M75})R|qUzOom`s%HU_WVNV>92W!VGL%5J8BYuBTFnFDcs|7U~7MUg4I?-^hZV zS3rCEIk1iV$QQTp?78h0O)^PL=?rhV`N_%pWhnuzVj)brFXXZ1Hl}r@@LbP!Mdp=; z%OG?eIlDeOpCL+xGELXC(~)WHiGBqAU-OlRuIXx)V!Yaj(ylhcq?2m<@=TZ+Zug3fY0P~dpk zg2QF7+nNIk1!?4SS04a0As6oQ)fFuGrKX&NP5D%rTi`;bm^=wQc?t;33-}D4c3!UA z8fFc4d<1d|D~&Ld&e8nPK4FYSUvM4E6uYQ!{TNI*?Sj+uWC-Wsc*Zi46&J#YjW7f= zcXvZ5PlhOr!%P)zzjOw?X?Bl3`s4a9N9h(_APq?}czK#QSs6+GP{}FJhyxh}ZtE&wQFR%o@3Or9co)@Pasv72X534#V zSBi^9E`kmD9+D*>dU|||1tN+(Z;yx5D1yg`#0=n@!i&nHGe@l>R<+o_y{tm+X5SX(SsCuq^?;;m0E%-+Mp4$4REA zJqyZQ=T8U!nz5|^U}v(}(76Xy{S*kdwk=LLjmS2IuN~Q4XS+nK-&ow?rB@a&9ntMF zr%$a$Is2#g8N2r|jKpTp6)Z0;wv{N+>>c9VS79-Zvn{r|yn6fA@||1DYi#9bt>rtd zwL8eS(N7N&IotvH4qxE#gKiK;#V8I3@&1c`xU#bPi+lSx*=#3W2PAV*|i`D8$z+3=-M@$00e0Zq%D$`O@6 z8PJZ)$qQ(u)e~<(Psuknth7(2PA#*##@(luSuvejxN6Uab?fQOfF1!1VC6dSLmPI! zocGy~*9P8aX|*%UodM+~UVd#~Dyz;bN93=hQ4Ri1EX!z$hI1ehxpe=#s( z$!kTN)Iw7?%`6u@Oh7+5_$kV3G%C<)RF zeo7C-c)Ku0uc=pg z6->2sZq>AEZLY2(O#BC#UcvZ0+S5lfz5Fx7>yq)Cwj&X1&#()$sP=vzM1Ao96>2!z zQ6k!t7(?X;Ks1qWEO{MTZx@#ilCD@vlX!pW*FvSg3u6C|f$T0lNp_PoY%70b>(LV0 z2;!l3eLn<{`#$cXZUFJTI+?dUGDqq*eODthH2>D=9HEpF=Up~!R*zP;;l>mN+gLw` zs>(nHR>y*XM6xf63$icL0@343^hBh68DF?ZXzJFO83Bi-L5%9~`3OW9y>s#-=;|5> z@JH_XIUa-pDOa*9=G>FB+8ZOXja;LGZ3LOoy&hC zZev(}4Os|g%P@OmJbU7tf)&!zFwD5w<8#)S@c;KR$Rxip2oTBOl)7ZVBGK{>Wd%oO zM>lE+ySF>vhkwSy8bI&v0WHKFUTV1dG|-Ab9bWFBgLgvqOgZzmRL>s-FV_)&*yoVpG$ zg`9wSXeG5c9gt&-L$2x&?p%W>#j(T7rdK)FbBDbuFP~78a{=NfHYca#K4h;`+@<#I z)Da;TzXnjCd>SO|{@)U2AgkvRrr#@J79{*5bvh+p=U4dLYf2y!|IZrz0CoV4L6_N6 z!msiV`1~s?B>cnI^w|2=8)=kQ_zz94{3yF#fVd~a*j#E+3>iFT^JB2x?mmk5k~|n^ zwc=8@*p9@A@z!px?PYY4onh|`FJN}%Il`=Xf&lBeU^BX;Pl5NpSApz=Oekqh518O4>oB z?m}b4`j$MGp91H5k0o!RtR@nS&T^?}nJ&gFxdLPd8>>ieAi>b{RfFm(JlNWhR+3n| zs^7!so671Jtpa%h9}fPDFy`k@f|f)@&_K!sGJtpVu>^^Lk??)(`F=Oykc**R^D%Qr!y-1oTl$V!_0SJV$;xab!__A{A=x5~Trs|jE=cRAXVx7dq7`6E?dmR&_=Hwc# zY1}$MrA0CzLU*ztuP~$jWJB&Mpxhenaz^#ZDm?CtYG85m$qhWtj2e@V@HnxlgOr<3 O=HWGGw43b0n*sm=7976- delta 154 zcmZ1^y+WENl$V!_0SE*pCZ=!X@nsb>($C1xP1UbV%+q(zEXgQM+ML2ViHY&n=I`uv zOpIES%ebb=>jE_t$$$twI19{`1#t}~YjaltRW9Q$XVjQ1$Kx)ljzyKlY~=b!T@;E?1O9yN**QZZb)L$}D6`vfS1n1lMkC2W}BXwu82y;%?TQk+jru zcQZ3Yr3G`Td^OM>dJ=+4M_=;;(*BHKt~u%4n=kHrLvl$eDv)%6d3-bT=FR(l!*F)i zx8S?j{qxSh<}B+woJ@ZXOzyytzW~6jw#5juW74+a*NL5;+b$97Ba6AL^xR^l1G`;j z_7kg7&j0CuK<_+=V!j^sI30d1x{*rW%=U5b)2LX--4?yEeBkXp4$EgZ&vx6(sU!nX>SmjomfB``9w!>`ZJh$5JQ0;b!&9Vyfo)b`h zj`?f`unMbz8RoT%8s{56N_v-e;!sK|cqX{yiHeeK8q(E!Gy@{qNfQ}KvzTt-QiM{` zFkv(m5s(`xn}$^fWgCAkASDR^u*L5N zcGqKM7cf;l@(1=Y`O<-t&e6=VWlq{C*vo7zbreBcV72C2^%2Ld&dBu({e1oYSp1 z4b>ShsU1FH7bWO3nS~ub^afsr-;|rExpR@Keirk^^KLHo3JFfd1>QK#?|)v37e7?A z6DIT-r#ym#g5K}P03jn{F6p&Z%46Px2$GFOu;CZ|P;rr9a9z7Oy@is{)xA|JMkzsi zp%T&FM9YSK;y{2247n2o2Gh>oe*WMk(G|wG!GtpZY!l=DDSdl$b*0CJNX6Tm%e@x; zWD>|UO0vG9y*{`G1RrPN=t}U`qluVHrxaRmT+l8$PkUn*YqF=wz9zZ$BN-)9g-M5N zJIQoqK=85(VIY*0X{|{l@=l4;e zhO1r4<1K*|RJ;K|6YxMK?WNyxsXh*q;7_6GEj>=3r7G&k;O@r5 zCA1M#L+u4Y1VJ7I$VL7ffaUw7?p!7a>#uF%zqXxPfz?Ufwu${ZpEVrDr#p}}#fLbK z4`IkMfPvMuAh}5X`yxrUxhkT1l1Pua>Wkz|E}^+w6JiWH{2Iil4?l0g6-M_V_!&%f z4K4`AF8UP;Vgs-$`2{2Q^se?NQQ0w6NC_X}7-&Y14L}CO6s@_TJ}|>lw5Z4t6uGW- zOu7zpurR-XJ_NgE*u6K|J#vr13aT_+Zt_Q`EHV`bKgcN4^u{Ov1O-WT$pno+>pu_` zT$vwd)+qd5ZT^5nQTrJ$@?>FRr-enjU~T{eQg`7D?}hb7*~F!|h*wul7@JV{Am)eh zTm@{H2;vG5HEiQY?c$YZy*ZEmno{;Ra6lq!tKyL|@yKNuqrkjWIC$!SwobJzEF^Up z#tI!^>9eu7WW)H}r#h_(%33IQ&%i_>wWizXsnt7vHm6eZd(|RG!j~qsh$vtRd z$H=Aj)yx4QR`ME8pnjsS5k4X_{9cuiwhA7EzpuC^4DMizO3~ z`Q=Hp-RwO~w$fo$&TBjQx#{NsZW4b zUIMTUwt8Co5*{JUI&}YTf8jo{UJ`*%NfTQyZEZJ~#msmw*Lde z%HJ_TWt3~K#c3yu4%~ z!`4-J$#tOPq>1)qKf`yO%mb!Y!6F@V@gK5o47G5c% z5(F&m1c7)5559}QsED#m$u|*lT$m(wc8|EO_nuevFA7|5R0P&NT}t}BY=3B`H)SWv p_M7k^#CK{_i7=69Xo0RZ#n&7??=J1F4W+(~%uSzms~)L2{{_X`aAW`g literal 0 HcmV?d00001 diff --git a/Robotics_API/__pycache__/__init__.cpython-310.pyc b/Robotics_API/__pycache__/__init__.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..c205a5bf1a461c233cea224041a7227e3eb41a6c GIT binary patch literal 250 zcmd1j<>g`kg8WH288txqF^GcAU0DDV=iM9Bbd#c z!<5Su#RO!t1T$!|zGMU{*JQjUZi$E z1hTV;8APxEiIogRtROas_+_AV!Z literal 0 HcmV?d00001 diff --git a/Robotics_API/__pycache__/__init__.cpython-38.pyc b/Robotics_API/__pycache__/__init__.cpython-38.pyc index fc6b4619d3e868332d58b749e2d7256fd8f00408..32c2ba50de39a9c2171fc9b38c0a1f2219c586af 100644 GIT binary patch delta 114 zcmcc0_=Ax*l$V!_0SGMTrf1Yl!G8D0b*dXGUfqq7QZmNDseqQ>-S*9Eyc^*a{ FMgV%67pMRL delta 94 zcmeytc$JYil$V!_0SLsTlhQpW^2##GOw^WPWe#T0WSJNtXTTDWU!3Zv$yCGyR8zzZ nBvvvMv4Geh;+L6zMt*Lpeq~~wzI$d#MrqQ--KK0Hr96xPl#~=f diff --git a/Robotics_API/__pycache__/__init__.cpython-39.pyc b/Robotics_API/__pycache__/__init__.cpython-39.pyc new file mode 100644 index 0000000000000000000000000000000000000000..09a057aecc51549dff358e81be937ded7f408c58 GIT binary patch literal 248 zcmYe~<>g`kg8WH288txqF^GcAU0DDV=iM9Bbd#c z!<5Su#RO!t1T$!|zGMU{*JQjUZi$E z1hTV;8APxEiIogRtROas_+_A + +""" +Description: read force raw data and external data +""" + +import sys +import time +import math +import socket +import struct +from xarm.wrapper import XArmAPI +####################################################### + +if len(sys.argv) >= 2: + ip = sys.argv[1] +else: + try: + from configparser import ConfigParser + parser = ConfigParser() + parser.read('../robot.conf') + ip = parser.get('xArm', 'ip') + except: + ip = input('Please input the xArm ip address:') + if not ip: + print('input error, exit') + sys.exit(1) +######################################################## + +arm = XArmAPI(ip, enable_report=True) +arm.motion_enable(enable=True) +arm.ft_sensor_enable(0) + +arm.clean_error() +arm.clean_warn() +arm.ft_sensor_enable(1) +time.sleep(0.5) +arm.ft_sensor_set_zero() + +while arm.connected and arm.error_code == 0: + # ft_raw_force and ft_ext_force will update by reporting socket + # print('raw_force: {}'.format(arm.ft_raw_force)) + print('exe_force: {}'.format(arm.ft_ext_force)) + + # # get_ft_sensor_data() will get the last ext_force + # code, ext_force = arm.get_ft_sensor_data() + # if code == 0: + # print('exe_force: {}'.format(ext_force)) + time.sleep(0.2) + +arm.ft_sensor_enable(0) +arm.disconnect() \ No newline at end of file From 84f9ececfdb501d9b2293d42420d0942f187d3c4 Mon Sep 17 00:00:00 2001 From: Yongchong Gu <107824980+Tastooger@users.noreply.github.com> Date: Wed, 20 Aug 2025 00:38:44 +0800 Subject: [PATCH 08/24] Update Bestman_Real_Xarm7.py --- Robotics_API/Bestman_Real_Xarm7.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Robotics_API/Bestman_Real_Xarm7.py b/Robotics_API/Bestman_Real_Xarm7.py index 0855625..e86371b 100644 --- a/Robotics_API/Bestman_Real_Xarm7.py +++ b/Robotics_API/Bestman_Real_Xarm7.py @@ -72,7 +72,7 @@ def update_robot_states(self): except Exception as e: self.log.error(f"[XArm7] Error updating robot states: {str(e)}") - def get_DOF(self): + def get_dof(self): """Override DOF to 7.""" dof = 7 self.log.info(f"Robot DOF: {dof}") From 4ff84f5b99ab8820efc2ed7019207b541d3a2aa6 Mon Sep 17 00:00:00 2001 From: Yongchong Gu <107824980+Tastooger@users.noreply.github.com> Date: Wed, 20 Aug 2025 01:09:52 +0800 Subject: [PATCH 09/24] Update Bestman_Real_Xarm7.py --- Robotics_API/Bestman_Real_Xarm7.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Robotics_API/Bestman_Real_Xarm7.py b/Robotics_API/Bestman_Real_Xarm7.py index e86371b..0855625 100644 --- a/Robotics_API/Bestman_Real_Xarm7.py +++ b/Robotics_API/Bestman_Real_Xarm7.py @@ -72,7 +72,7 @@ def update_robot_states(self): except Exception as e: self.log.error(f"[XArm7] Error updating robot states: {str(e)}") - def get_dof(self): + def get_DOF(self): """Override DOF to 7.""" dof = 7 self.log.info(f"Robot DOF: {dof}") From d9c1b4a97a146a24512fa8cb09fcdbfaf497ec50 Mon Sep 17 00:00:00 2001 From: Yongchong Gu <107824980+Tastooger@users.noreply.github.com> Date: Wed, 20 Aug 2025 01:24:55 +0800 Subject: [PATCH 10/24] Update Bestman_Real_Xarm7.py --- Robotics_API/Bestman_Real_Xarm7.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Robotics_API/Bestman_Real_Xarm7.py b/Robotics_API/Bestman_Real_Xarm7.py index 0855625..e86371b 100644 --- a/Robotics_API/Bestman_Real_Xarm7.py +++ b/Robotics_API/Bestman_Real_Xarm7.py @@ -72,7 +72,7 @@ def update_robot_states(self): except Exception as e: self.log.error(f"[XArm7] Error updating robot states: {str(e)}") - def get_DOF(self): + def get_dof(self): """Override DOF to 7.""" dof = 7 self.log.info(f"Robot DOF: {dof}") From 7cf855e50df9996a685120719b33a18dd35a91c2 Mon Sep 17 00:00:00 2001 From: Yongchong Gu <107824980+Tastooger@users.noreply.github.com> Date: Wed, 20 Aug 2025 01:37:29 +0800 Subject: [PATCH 11/24] Update Bestman_Real_Xarm6.py --- Robotics_API/Bestman_Real_Xarm6.py | 42 +++++++++++++++--------------- 1 file changed, 21 insertions(+), 21 deletions(-) diff --git a/Robotics_API/Bestman_Real_Xarm6.py b/Robotics_API/Bestman_Real_Xarm6.py index a50d268..a324d4a 100644 --- a/Robotics_API/Bestman_Real_Xarm6.py +++ b/Robotics_API/Bestman_Real_Xarm6.py @@ -150,7 +150,7 @@ def update_robot_states(self): except Exception as e: self.log.error(f"Error updating robot states: {str(e)}") - def go_home(self): + def move_to_home(self): # DONE """ Moves the robot arm to its initial (home) pose. @@ -188,7 +188,7 @@ def go_home(self): self.log.error(f"Cannot go home: {str(e)}") - def set_pay_load(self, payload_weight, payload_cog): + def set_payload(self, payload_weight, payload_cog): # TODO self.robot.set_tcp_load(payload_weight, payload_cog) @@ -208,7 +208,7 @@ def get_joint_bounds(self): return list(zip(joint_min, joint_max)) - def print_arm_jointInfo(self): + def print_links_info(self): # TODO """ Prints the joint and link information of the robot's arm. @@ -230,7 +230,7 @@ def get_joint_idx(self): """ return list(range(len(self.active_joints))) - def get_DOF(self): + def get_dof(self): # DONE """ Retrieves the degree of freedom (DOF) of the xArm robot. @@ -242,7 +242,7 @@ def get_DOF(self): self.log.info(f"Robot DOF: {dof}") return dof - def get_tcp_link(self): + def get_links_info(self): # TODO """ Retrieves the name of the TCP (Tool Center Point) link. @@ -258,7 +258,7 @@ def get_tcp_link(self): self.log.error(f"Failed to retrieve TCP link: {str(e)}") return None - def get_current_joint_values(self): + def get_joint_angles(self): # DONE """ Retrieves the current joint angles of the robot arm. @@ -270,13 +270,13 @@ def get_current_joint_values(self): # Get joint states and extract joint angles for the first 6 joints joint_states = self.robot.get_joint_states(is_radian=True) joint_values = joint_states[1][0][:6] - self.log.info(f"Current joint values: {joint_values}") + self.log.info(f"Current joint angles: {joint_values}") return joint_values except Exception as e: - self.log.error(f"Failed to retrieve current joint values: {str(e)}") + self.log.error(f"Failed to retrieve current joint angles: {str(e)}") return [] - def get_current_joint_velocities(self): + def get_joint_velocities(self): """ Retrieves the current joint velocities of the robot arm. @@ -293,7 +293,7 @@ def get_current_joint_velocities(self): self.log.error(f"Failed to retrieve current joint velocities: {str(e)}") return [] - def get_current_eef_pose(self): + def get_eef_pose(self): # DONE """ Retrieves the current pose of the robot arm's end effector. @@ -321,7 +321,7 @@ def get_current_eef_pose(self): self.log.error(f"Failed to retrieve end effector pose: {str(e)}") return None - def move_arm_to_joint_values(self, joint_values, target_vel=None, target_acc=None, max_vel=None, max_acc=None): + def move_to_joint_angles(self, joint_values, target_vel=None, target_acc=None, max_vel=None, max_acc=None): # DONE """ Moves the robotic arm to a specific set of joint values. @@ -350,17 +350,17 @@ def move_arm_to_joint_values(self, joint_values, target_vel=None, target_acc=Non if result != 0: self.log.error( - f"Failed to move arm to joint values: {joint_values}. Error code: {result}" + f"Failed to move arm to joint angles: {joint_values}. Error code: {result}" ) else: - self.log.info(f"Moving arm to joint values: {joint_values}") + self.log.info(f"Moving arm to joint angles: {joint_values}") except Exception as e: self.log.error( - f"An error occurred while moving arm to joint values: {str(e)}" + f"An error occurred while moving arm to joint angles: {str(e)}" ) - def get_current_tcp_speed(self): + def get_eef_velocities(self): # TODO """ Retrieves the current tcp velocities of the robot arm. @@ -375,7 +375,7 @@ def get_current_tcp_speed(self): # End Effector (EEF) Functions # ---------------------------------------------------------------- - def move_eef_to_goal_pose(self, goal_pose, max_linear_vel=100, max_angular_vel=5000): + def move_to_eef_pose(self, goal_pose, max_linear_vel=100, max_angular_vel=5000): """ Move arm's end effector to a target position. @@ -416,7 +416,7 @@ def move_eef_to_goal_pose(self, goal_pose, max_linear_vel=100, max_angular_vel=5 except Exception as e: self.log.error(f"Failed to move end effector to goal pose: {str(e)}") - def move_eef_to_tcp_velocity(self, _velocity_setpoint, _duration): + def set_eef_velocity(self, _velocity_setpoint, _duration): #TODO """ Move arm's end effector to a target tcp velocity. @@ -442,7 +442,7 @@ def move_eef_to_tcp_velocity(self, _velocity_setpoint, _duration): duration=_duration, ) - def rotate_eef_tcp(self, axis, angle): + def rotate_eef_joint(self, axis, angle): # TODO """ Rotate the end effector of the robot arm by a specified angle by joint. @@ -551,7 +551,7 @@ def cartesian_to_joints(self, pose, initial_joint_values=None): raise - def calculate_IK_error(self, goal_position, goal_orientation): + def calculate_ik_error(self, goal_position, goal_orientation): """ Calculate the inverse kinematics (IK) error for performing pick-and-place manipulation of an object using a robot arm. @@ -611,7 +611,7 @@ def get_gripper_position_xarm(self): return gripper_pos[1] - def gripper_goto_xarm(self, value, speed=5000, force=None): + def set_gripper_position_xarm(self, value, speed=5000, force=None): """ Moves the gripper to a specified position with given speed. @@ -655,7 +655,7 @@ def get_gripper_position_robotiq(self, number_of_registers=3): gripper_width = status[1][-2] return gripper_width - def gripper_goto_robotiq(self, width, speed=0xFF, force=0xFF, wait=False, timeout=5, **kwargs): + def set_gripper_position_robotiq(self, width, speed=0xFF, force=0xFF, wait=False, timeout=5, **kwargs): # DONE """ Go to the position with determined speed and force. From 11bb0647314a708b8b80c1f4ac14838fe0df10e9 Mon Sep 17 00:00:00 2001 From: Yongchong Gu <107824980+Tastooger@users.noreply.github.com> Date: Wed, 20 Aug 2025 01:39:41 +0800 Subject: [PATCH 12/24] Update move_arm_to_home.py --- Examples/move_arm_to_home.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Examples/move_arm_to_home.py b/Examples/move_arm_to_home.py index fcaea3d..ae06d18 100644 --- a/Examples/move_arm_to_home.py +++ b/Examples/move_arm_to_home.py @@ -31,7 +31,7 @@ def main(): return # Exit if initialization fails # Go back to home pose - bestman.go_home() + bestman.move_to_home() time.sleep(1) except Exception as e: From bf199667fedf0bf746d5bd5ddb9d5c1c1b9e75ff Mon Sep 17 00:00:00 2001 From: Yongchong Gu <107824980+Tastooger@users.noreply.github.com> Date: Wed, 20 Aug 2025 01:40:28 +0800 Subject: [PATCH 13/24] Update move_arm_to_joint_values.py --- Examples/move_arm_to_joint_values.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Examples/move_arm_to_joint_values.py b/Examples/move_arm_to_joint_values.py index 181d1b6..0fe841b 100644 --- a/Examples/move_arm_to_joint_values.py +++ b/Examples/move_arm_to_joint_values.py @@ -33,7 +33,7 @@ def main(): import math target_joint = [16.8, -24.1, -24.7, 11.2, -77.7, -5.1] target_joint = [math.radians(target_joint[n]) for n in range(6)] - bestman.move_arm_to_joint_values(target_joint) + bestman.move_to_joint_angles(target_joint) except Exception as e: # Log any exceptions that occur From fa26b37f0eeb430c0e7715eb1c96bd64bb50db76 Mon Sep 17 00:00:00 2001 From: Yongchong Gu <107824980+Tastooger@users.noreply.github.com> Date: Wed, 20 Aug 2025 01:41:30 +0800 Subject: [PATCH 14/24] Update move_eef_to_pose.py --- Examples/move_eef_to_pose.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Examples/move_eef_to_pose.py b/Examples/move_eef_to_pose.py index ef1d14e..36f80fc 100644 --- a/Examples/move_eef_to_pose.py +++ b/Examples/move_eef_to_pose.py @@ -37,7 +37,7 @@ def main(): # target_pose = Pose([0.420434235, -0.00581615, 0.459876038], [0.7071068218077394, -0.00032550013355177364, 0.7071065907288765, 0.0003255002399235735]) # 四元数 # Move the arm to follow the target trajectory - bestman.move_eef_to_goal_pose(mvpose) + bestman.move_to_eef_pose(mvpose) except Exception as e: # Log any exceptions that occur From dc20afbcd49a65e7399ea900637f7b207a4ff7ce Mon Sep 17 00:00:00 2001 From: Yongchong Gu <107824980+Tastooger@users.noreply.github.com> Date: Wed, 20 Aug 2025 01:45:31 +0800 Subject: [PATCH 15/24] Update test.py --- Examples/test.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/Examples/test.py b/Examples/test.py index 7f2c7c5..f235c8e 100644 --- a/Examples/test.py +++ b/Examples/test.py @@ -40,14 +40,14 @@ def main(): # bestman.get_current_eef_pose() # Test - gt_pose = bestman.get_current_eef_pose() - gt_joint = bestman.get_current_joint_values() + gt_pose = bestman.get_eef_pose() + gt_joint = bestman.get_joint_angles() # gt_joint[1] += 0.3 - # bestman.move_arm_to_joint_values(gt_joint) + # bestman.move_to_joint_angles(gt_joint) # gt_pose.position[2] += 0.3 - # bestman.move_eef_to_goal_pose(gt_pose) + # bestman.move_to_eef_pose(gt_pose) # print(f'gt position:{gt_pose.position}, gt orientation:{gt_pose.orientation}') # print(f'gt joint:{gt_joint}') @@ -65,15 +65,15 @@ def main(): trans_joint = bestman.cartesian_to_joints(gt_pose) print(f'trans_joint:{trans_joint}') - bestman.move_arm_to_joint_values(trans_joint) + bestman.move_to_joint_angles(trans_joint) trans_pose = bestman.joints_to_cartesian(gt_joint) print(f'trans_pose:{trans_pose}') - bestman.move_eef_to_goal_pose(trans_pose) + bestman.move_to_eef_pose(trans_pose) bestman.close_gripper_robotiq() bestman.open_gripper_robotiq() - bestman.gripper_goto_robotiq(100) + bestman.set_gripper_position_robotiq(100) # print(bestman.get_gripper_position_robotiq()) except Exception as e: From ac7730b28daa9134eeee7bdd3d7f63f3547b99c2 Mon Sep 17 00:00:00 2001 From: Yongchong Gu <107824980+Tastooger@users.noreply.github.com> Date: Wed, 20 Aug 2025 02:18:18 +0800 Subject: [PATCH 16/24] Update Bestman_Real_Xarm6.py --- Robotics_API/Bestman_Real_Xarm6.py | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/Robotics_API/Bestman_Real_Xarm6.py b/Robotics_API/Bestman_Real_Xarm6.py index a324d4a..de3c1a7 100644 --- a/Robotics_API/Bestman_Real_Xarm6.py +++ b/Robotics_API/Bestman_Real_Xarm6.py @@ -258,7 +258,7 @@ def get_links_info(self): self.log.error(f"Failed to retrieve TCP link: {str(e)}") return None - def get_joint_angles(self): + def get_joint_ang(self): # DONE """ Retrieves the current joint angles of the robot arm. @@ -276,7 +276,7 @@ def get_joint_angles(self): self.log.error(f"Failed to retrieve current joint angles: {str(e)}") return [] - def get_joint_velocities(self): + def get_joint_vel(self): """ Retrieves the current joint velocities of the robot arm. @@ -293,7 +293,7 @@ def get_joint_velocities(self): self.log.error(f"Failed to retrieve current joint velocities: {str(e)}") return [] - def get_eef_pose(self): + def get_eef_pos(self): # DONE """ Retrieves the current pose of the robot arm's end effector. @@ -321,7 +321,7 @@ def get_eef_pose(self): self.log.error(f"Failed to retrieve end effector pose: {str(e)}") return None - def move_to_joint_angles(self, joint_values, target_vel=None, target_acc=None, max_vel=None, max_acc=None): + def move_to_joint_ang(self, joint_values, target_vel=None, target_acc=None, max_vel=None, max_acc=None): # DONE """ Moves the robotic arm to a specific set of joint values. @@ -360,7 +360,7 @@ def move_to_joint_angles(self, joint_values, target_vel=None, target_acc=None, m f"An error occurred while moving arm to joint angles: {str(e)}" ) - def get_eef_velocities(self): + def get_eef_vel(self): # TODO """ Retrieves the current tcp velocities of the robot arm. @@ -375,7 +375,7 @@ def get_eef_velocities(self): # End Effector (EEF) Functions # ---------------------------------------------------------------- - def move_to_eef_pose(self, goal_pose, max_linear_vel=100, max_angular_vel=5000): + def move_to_eef_pos(self, goal_pose, max_linear_vel=100, max_angular_vel=5000): """ Move arm's end effector to a target position. @@ -416,7 +416,7 @@ def move_to_eef_pose(self, goal_pose, max_linear_vel=100, max_angular_vel=5000): except Exception as e: self.log.error(f"Failed to move end effector to goal pose: {str(e)}") - def set_eef_velocity(self, _velocity_setpoint, _duration): + def set_eef_vel(self, _velocity_setpoint, _duration): #TODO """ Move arm's end effector to a target tcp velocity. @@ -442,7 +442,7 @@ def set_eef_velocity(self, _velocity_setpoint, _duration): duration=_duration, ) - def rotate_eef_joint(self, axis, angle): + def rot_eef_joint(self, axis, angle): # TODO """ Rotate the end effector of the robot arm by a specified angle by joint. @@ -551,7 +551,7 @@ def cartesian_to_joints(self, pose, initial_joint_values=None): raise - def calculate_ik_error(self, goal_position, goal_orientation): + def calc_ik_error(self, goal_position, goal_orientation): """ Calculate the inverse kinematics (IK) error for performing pick-and-place manipulation of an object using a robot arm. @@ -603,7 +603,7 @@ def find_gripper_robotiq(self): self.robot.robotiq_reset() self.robot.robotiq_set_activate() # enable the robotiq gripper - def get_gripper_position_xarm(self): + def get_gripper_pos_xarm(self): """ Get the position of the XArm gripper. """ @@ -611,7 +611,7 @@ def get_gripper_position_xarm(self): return gripper_pos[1] - def set_gripper_position_xarm(self, value, speed=5000, force=None): + def set_gripper_pos_xarm(self, value, speed=5000, force=None): """ Moves the gripper to a specified position with given speed. @@ -633,7 +633,7 @@ def open_gripper_xarm(self): """Opens the gripper to its maximum position with maximum speed and force.""" self.gripper_goto(value=850, speed=5000, force=None) - def get_gripper_position_robotiq(self, number_of_registers=3): + def get_gripper_pos_robotiq(self, number_of_registers=3): # TODO """ Reading the status of robotiq gripper @@ -655,7 +655,7 @@ def get_gripper_position_robotiq(self, number_of_registers=3): gripper_width = status[1][-2] return gripper_width - def set_gripper_position_robotiq(self, width, speed=0xFF, force=0xFF, wait=False, timeout=5, **kwargs): + def set_gripper_pos_robotiq(self, width, speed=0xFF, force=0xFF, wait=False, timeout=5, **kwargs): # DONE """ Go to the position with determined speed and force. From e2a88ac4f9226fbe7789896440038a1a01615d08 Mon Sep 17 00:00:00 2001 From: Yongchong Gu <107824980+Tastooger@users.noreply.github.com> Date: Wed, 20 Aug 2025 02:19:07 +0800 Subject: [PATCH 17/24] Update move_arm_to_joint_values.py --- Examples/move_arm_to_joint_values.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Examples/move_arm_to_joint_values.py b/Examples/move_arm_to_joint_values.py index 0fe841b..29516e4 100644 --- a/Examples/move_arm_to_joint_values.py +++ b/Examples/move_arm_to_joint_values.py @@ -33,7 +33,7 @@ def main(): import math target_joint = [16.8, -24.1, -24.7, 11.2, -77.7, -5.1] target_joint = [math.radians(target_joint[n]) for n in range(6)] - bestman.move_to_joint_angles(target_joint) + bestman.move_to_joint_ang(target_joint) except Exception as e: # Log any exceptions that occur From a177b73d1843b4b67bd0c977f98359c7943f5233 Mon Sep 17 00:00:00 2001 From: Yongchong Gu <107824980+Tastooger@users.noreply.github.com> Date: Wed, 20 Aug 2025 02:19:42 +0800 Subject: [PATCH 18/24] Update move_eef_to_pose.py --- Examples/move_eef_to_pose.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Examples/move_eef_to_pose.py b/Examples/move_eef_to_pose.py index 36f80fc..ba3f4f7 100644 --- a/Examples/move_eef_to_pose.py +++ b/Examples/move_eef_to_pose.py @@ -37,7 +37,7 @@ def main(): # target_pose = Pose([0.420434235, -0.00581615, 0.459876038], [0.7071068218077394, -0.00032550013355177364, 0.7071065907288765, 0.0003255002399235735]) # 四元数 # Move the arm to follow the target trajectory - bestman.move_to_eef_pose(mvpose) + bestman.move_to_eef_pos(mvpose) except Exception as e: # Log any exceptions that occur From dd21505d8350219b5a310bfeecd27a48cb453a6c Mon Sep 17 00:00:00 2001 From: Yongchong Gu <107824980+Tastooger@users.noreply.github.com> Date: Wed, 20 Aug 2025 02:20:33 +0800 Subject: [PATCH 19/24] Update test.py --- Examples/test.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/Examples/test.py b/Examples/test.py index f235c8e..bfc5b8a 100644 --- a/Examples/test.py +++ b/Examples/test.py @@ -40,8 +40,8 @@ def main(): # bestman.get_current_eef_pose() # Test - gt_pose = bestman.get_eef_pose() - gt_joint = bestman.get_joint_angles() + gt_pose = bestman.get_eef_pos() + gt_joint = bestman.get_joint_ang() # gt_joint[1] += 0.3 # bestman.move_to_joint_angles(gt_joint) @@ -65,15 +65,15 @@ def main(): trans_joint = bestman.cartesian_to_joints(gt_pose) print(f'trans_joint:{trans_joint}') - bestman.move_to_joint_angles(trans_joint) + bestman.move_to_joint_ang(trans_joint) trans_pose = bestman.joints_to_cartesian(gt_joint) print(f'trans_pose:{trans_pose}') - bestman.move_to_eef_pose(trans_pose) + bestman.move_to_eef_pos(trans_pose) bestman.close_gripper_robotiq() bestman.open_gripper_robotiq() - bestman.set_gripper_position_robotiq(100) + bestman.set_gripper_pos_robotiq(100) # print(bestman.get_gripper_position_robotiq()) except Exception as e: From e9a431894e46622d1c2f148bf3122636cadecd5a Mon Sep 17 00:00:00 2001 From: Yongchong Gu <107824980+Tastooger@users.noreply.github.com> Date: Wed, 20 Aug 2025 03:22:28 +0800 Subject: [PATCH 20/24] Update Bestman_Real_Xarm6.py --- Robotics_API/Bestman_Real_Xarm6.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/Robotics_API/Bestman_Real_Xarm6.py b/Robotics_API/Bestman_Real_Xarm6.py index de3c1a7..ea7d1bd 100644 --- a/Robotics_API/Bestman_Real_Xarm6.py +++ b/Robotics_API/Bestman_Real_Xarm6.py @@ -150,7 +150,7 @@ def update_robot_states(self): except Exception as e: self.log.error(f"Error updating robot states: {str(e)}") - def move_to_home(self): + def reset_to_home(self): # DONE """ Moves the robot arm to its initial (home) pose. @@ -258,7 +258,7 @@ def get_links_info(self): self.log.error(f"Failed to retrieve TCP link: {str(e)}") return None - def get_joint_ang(self): + def get_joint_state(self): # DONE """ Retrieves the current joint angles of the robot arm. @@ -293,7 +293,7 @@ def get_joint_vel(self): self.log.error(f"Failed to retrieve current joint velocities: {str(e)}") return [] - def get_eef_pos(self): + def get_eef_state(self): # DONE """ Retrieves the current pose of the robot arm's end effector. @@ -321,7 +321,7 @@ def get_eef_pos(self): self.log.error(f"Failed to retrieve end effector pose: {str(e)}") return None - def move_to_joint_ang(self, joint_values, target_vel=None, target_acc=None, max_vel=None, max_acc=None): + def set_joint_cmd(self, joint_values, target_vel=None, target_acc=None, max_vel=None, max_acc=None): # DONE """ Moves the robotic arm to a specific set of joint values. @@ -375,7 +375,7 @@ def get_eef_vel(self): # End Effector (EEF) Functions # ---------------------------------------------------------------- - def move_to_eef_pos(self, goal_pose, max_linear_vel=100, max_angular_vel=5000): + def set_eef_cmd(self, goal_pose, max_linear_vel=100, max_angular_vel=5000): """ Move arm's end effector to a target position. @@ -465,7 +465,7 @@ def rot_eef_joint(self, axis, angle): # ---------------------------------------------------------------- # Functions for IK # ---------------------------------------------------------------- - def joints_to_cartesian(self, joint_values): + def forward_kinematics(self, joint_values): # DONE """ Converts the robot's joint angles to its Cartesian coordinates. @@ -509,7 +509,7 @@ def joints_to_cartesian(self, joint_values): self.log.error(f"Error converting joint values to Cartesian: {str(e)}") raise - def cartesian_to_joints(self, pose, initial_joint_values=None): + def inverse_kinematics(self, pose, initial_joint_values=None): # DONE """ Converts the robot's Cartesian coordinates to its joint angles. From 1ba1dea2d5cb6c59d7078cb9c6cff2de063c4a3f Mon Sep 17 00:00:00 2001 From: Yongchong Gu <107824980+Tastooger@users.noreply.github.com> Date: Wed, 20 Aug 2025 03:23:10 +0800 Subject: [PATCH 21/24] Update move_arm_to_home.py --- Examples/move_arm_to_home.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Examples/move_arm_to_home.py b/Examples/move_arm_to_home.py index ae06d18..c4491da 100644 --- a/Examples/move_arm_to_home.py +++ b/Examples/move_arm_to_home.py @@ -31,7 +31,7 @@ def main(): return # Exit if initialization fails # Go back to home pose - bestman.move_to_home() + bestman.reset_to_home() time.sleep(1) except Exception as e: From d558966a696aa5fd2cf20b685ee65d045cfd6ab4 Mon Sep 17 00:00:00 2001 From: Yongchong Gu <107824980+Tastooger@users.noreply.github.com> Date: Wed, 20 Aug 2025 03:23:49 +0800 Subject: [PATCH 22/24] Update move_arm_to_joint_values.py --- Examples/move_arm_to_joint_values.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Examples/move_arm_to_joint_values.py b/Examples/move_arm_to_joint_values.py index 29516e4..4f142e8 100644 --- a/Examples/move_arm_to_joint_values.py +++ b/Examples/move_arm_to_joint_values.py @@ -33,7 +33,7 @@ def main(): import math target_joint = [16.8, -24.1, -24.7, 11.2, -77.7, -5.1] target_joint = [math.radians(target_joint[n]) for n in range(6)] - bestman.move_to_joint_ang(target_joint) + bestman.set_joint_cmd(target_joint) except Exception as e: # Log any exceptions that occur From a392858a69da1cac99faef2a05e4e3afab127fcb Mon Sep 17 00:00:00 2001 From: Yongchong Gu <107824980+Tastooger@users.noreply.github.com> Date: Wed, 20 Aug 2025 03:24:19 +0800 Subject: [PATCH 23/24] Update move_eef_to_pose.py --- Examples/move_eef_to_pose.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Examples/move_eef_to_pose.py b/Examples/move_eef_to_pose.py index ba3f4f7..8cd58d4 100644 --- a/Examples/move_eef_to_pose.py +++ b/Examples/move_eef_to_pose.py @@ -37,7 +37,7 @@ def main(): # target_pose = Pose([0.420434235, -0.00581615, 0.459876038], [0.7071068218077394, -0.00032550013355177364, 0.7071065907288765, 0.0003255002399235735]) # 四元数 # Move the arm to follow the target trajectory - bestman.move_to_eef_pos(mvpose) + bestman.set_eef_cmd(mvpose) except Exception as e: # Log any exceptions that occur From 8e220b152d1d86f3f8de538722fc26530236ac83 Mon Sep 17 00:00:00 2001 From: Yongchong Gu <107824980+Tastooger@users.noreply.github.com> Date: Wed, 20 Aug 2025 03:26:12 +0800 Subject: [PATCH 24/24] Update test.py --- Examples/test.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/Examples/test.py b/Examples/test.py index bfc5b8a..c07643e 100644 --- a/Examples/test.py +++ b/Examples/test.py @@ -40,8 +40,8 @@ def main(): # bestman.get_current_eef_pose() # Test - gt_pose = bestman.get_eef_pos() - gt_joint = bestman.get_joint_ang() + gt_pose = bestman.get_eef_state() + gt_joint = bestman.get_joint_state() # gt_joint[1] += 0.3 # bestman.move_to_joint_angles(gt_joint) @@ -63,13 +63,13 @@ def main(): # # print(f'test position:{pose.position}, test orientation:{pose.orientation}') # # bestman.move_eef_to_goal_pose(pose) - trans_joint = bestman.cartesian_to_joints(gt_pose) + trans_joint = bestman.inverse_kinematics(gt_pose) print(f'trans_joint:{trans_joint}') - bestman.move_to_joint_ang(trans_joint) + bestman.set_joint_cmd(trans_joint) - trans_pose = bestman.joints_to_cartesian(gt_joint) + trans_pose = bestman.forward_kinematics(gt_joint) print(f'trans_pose:{trans_pose}') - bestman.move_to_eef_pos(trans_pose) + bestman.set_eef_cmd(trans_pose) bestman.close_gripper_robotiq() bestman.open_gripper_robotiq()