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/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..5c8f9c3
--- /dev/null
+++ b/Examples/close_gripper_robotiq.py
@@ -0,0 +1,42 @@
+# !/usr/bin/env python
+# -*- encoding: utf-8 -*-
+"""
+# @FileName : close_gripper_robotiq.py
+# @Time : 2024-12-01 15:21:45
+# @Author : Zhaxi & Yan
+# @Email : zhaxizhuoma.ayang@gmail.com & yding25@binghamton.edu
+# @Description : Close 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..e27d532 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 : display_states.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_Xarm7, 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_Xarm7(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 715ca7a..c4491da 100644
--- a/Examples/move_arm_to_home.py
+++ b/Examples/move_arm_to_home.py
@@ -1,49 +1,38 @@
-'''
-Run this script using:
-
-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
+# !/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 move_arm_to_home.py 192.168.1.208
+"""
+
+import argparse
+import time
+from Robotics_API import Bestman_Real_Xarm7
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")
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 = Bestman_Real_Xarm7(args.robot_ip)
+
+ # Initialize robot
+ if not bestman.initialize_robot():
+ return # Exit if initialization fails
# Go back to home pose
- bestman.go_home()
- sleep(1)
+ bestman.reset_to_home()
+ time.sleep(1)
except Exception as e:
# Log any exceptions that occur
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..4f142e8
--- /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_joint_values.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.set_joint_cmd(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..8cd58d4
--- /dev/null
+++ b/Examples/move_eef_to_pose.py
@@ -0,0 +1,48 @@
+# !/usr/bin/env python
+# -*- encoding: utf-8 -*-
+"""
+# @FileName : move_eef_to_pose.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_Xarm7, Pose
+import numpy as np
+
+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_Xarm7(args.robot_ip)
+
+ # Initialize robot
+ if not bestman.initialize_robot():
+ return # Exit if initialization fails
+
+ # Define the target trajectory
+ 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.set_eef_cmd(mvpose)
+
+ 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..c07643e
--- /dev/null
+++ b/Examples/test.py
@@ -0,0 +1,85 @@
+# !/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()
+
+ # bestman.go_home()
+ # bestman.get_tcp_link()
+ # bestman.get_current_eef_pose()
+
+ # Test
+ gt_pose = bestman.get_eef_state()
+ gt_joint = bestman.get_joint_state()
+
+ # gt_joint[1] += 0.3
+ # bestman.move_to_joint_angles(gt_joint)
+
+ # gt_pose.position[2] += 0.3
+ # 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}')
+ # # 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.inverse_kinematics(gt_pose)
+ print(f'trans_joint:{trans_joint}')
+ bestman.set_joint_cmd(trans_joint)
+
+ trans_pose = bestman.forward_kinematics(gt_joint)
+ print(f'trans_pose:{trans_pose}')
+ bestman.set_eef_cmd(trans_pose)
+
+ bestman.close_gripper_robotiq()
+ bestman.open_gripper_robotiq()
+ bestman.set_gripper_pos_robotiq(100)
+ # print(bestman.get_gripper_position_robotiq())
+
+ except Exception as e:
+ # Log any exceptions that occur
+ print(str(e))
+
+
+if __name__ == "__main__":
+ 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/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/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/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/README.md b/README.md
index c4bada1..48eb6ce 100644
--- a/README.md
+++ b/README.md
@@ -12,29 +12,20 @@
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".
-## 💻 Installation
-
-- (Optional) Create conda environment
-
- ```
- conda env create -f basic_environment.yaml
- ```
-
-- Install xArm-Python-SDK
+### 🏠 Prerequisites
- ```bash
- cd ~
- git clone https://github.com/xArm-Developer/xArm-Python-SDK.git
- python3 setup.py install
- ```
+> ***Note**: We recommand Ubuntu 22.04 and python version deault to 3.8.*
-- Clone the BestMan_Xarm Repository
+- Ubuntu 22.04
+- Conda
+ - Python 3.8
- ```bash
- git clone https://github.com/yding25/BestMan_Xarm.git
- ```
+## 💻 Installation
+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 +43,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/RoboticsToolBox/Bestman_real_xarm6.py b/RoboticsToolBox/Bestman_real_xarm6.py
deleted file mode 100644
index 12c0872..0000000
--- a/RoboticsToolBox/Bestman_real_xarm6.py
+++ /dev/null
@@ -1,562 +0,0 @@
-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 390b55a..0000000
Binary files a/RoboticsToolBox/__pycache__/Bestman_real_xarm6.cpython-38.pyc and /dev/null differ
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 03e5e68..0000000
Binary files a/RoboticsToolBox/__pycache__/Bestman_sim_flexiv.cpython-38.pyc and /dev/null differ
diff --git a/RoboticsToolBox/__pycache__/utility.cpython-38.pyc b/RoboticsToolBox/__pycache__/utility.cpython-38.pyc
deleted file mode 100644
index fbcfa12..0000000
Binary files a/RoboticsToolBox/__pycache__/utility.cpython-38.pyc and /dev/null differ
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_Xarm6.py b/Robotics_API/Bestman_Real_Xarm6.py
new file mode 100644
index 0000000..ea7d1bd
--- /dev/null
+++ b/Robotics_API/Bestman_Real_Xarm6.py
@@ -0,0 +1,717 @@
+# !/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
+
+
+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):
+ # DONE
+ """
+ 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
+ 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."
+ # )
+
+ # 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):
+ # DONE
+ """
+ Initializes the robot by clearing faults and enabling it.
+
+ Returns:
+ bool: True if the robot is successfully initialized, False otherwise.
+ """
+ try:
+ self.log.info("Checking for faults on the robot...")
+ self.robot.clean_error()
+ self.robot.clean_warn()
+ time.sleep(2)
+ 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):
+ # DONE
+ """
+ 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_values = joint_states[1][0][:6]
+ joint_velocities = joint_states[1][1][:6] # TODO
+
+ # Retrieve end effector pose (Euler angles)
+ 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]] # 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": orientation.tolist(), # Quaternion (x, y, z, w)
+ },
+ }
+
+ # Log and display TCP pose in quaternion format
+ 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 reset_to_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.
+ """
+ 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)}")
+
+
+ def set_payload(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.
+
+ 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_links_info(self):
+ # TODO
+ """
+ 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):
+ print(f"Link {i} attributes: {vars(link)}")
+
+ def get_joint_idx(self):
+ # TODO
+ """
+ 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):
+ # DONE
+ """
+ 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_links_info(self):
+ # TODO
+ """
+ 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_joint_state(self):
+ # DONE
+ """
+ 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 angles: {joint_values}")
+ return joint_values
+ except Exception as e:
+ self.log.error(f"Failed to retrieve current joint angles: {str(e)}")
+ return []
+
+ def get_joint_vel(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_eef_state(self):
+ # DONE
+ """
+ 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,
+ raw_pose[1] / 1000,
+ raw_pose[2] / 1000,
+ ]
+ orientation = raw_pose[3:]
+ pose = Pose(position, orientation)
+ 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 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.
+
+ 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 angles: {joint_values}. Error code: {result}"
+ )
+ else:
+ 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 angles: {str(e)}"
+ )
+
+ def get_eef_vel(self):
+ # TODO
+ """
+ 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 set_eef_cmd(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 set_eef_vel(self, _velocity_setpoint, _duration):
+ #TODO
+ """
+ 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 rot_eef_joint(self, axis, angle):
+ # TODO
+ """
+ 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 forward_kinematics(self, joint_values):
+ # DONE
+ """
+ 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 inverse_kinematics(self, pose, initial_joint_values=None):
+ # DONE
+ """
+ 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 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.
+
+ 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-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_pos_xarm(self):
+ """
+ Get the position of the XArm gripper.
+ """
+ gripper_pos = self.robot.get_gripper_position()
+
+ return gripper_pos[1]
+
+ def set_gripper_pos_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_pos_robotiq(self, number_of_registers=3):
+ # TODO
+ """
+ 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 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.
+
+ :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
+ :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(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
+
+ :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_robotiq(
+ self, speed=0xFF, force=0xFF, wait=False, timeout=5, **kwargs
+ ):
+ # DONE
+ """
+ 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_Xarm7.py b/Robotics_API/Bestman_Real_Xarm7.py
new file mode 100644
index 0000000..e86371b
--- /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/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/__init__.py b/Robotics_API/__init__.py
new file mode 100644
index 0000000..a2e3d1e
--- /dev/null
+++ b/Robotics_API/__init__.py
@@ -0,0 +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_Xarm.cpython-38.pyc b/Robotics_API/__pycache__/Bestman_Real_Xarm.cpython-38.pyc
new file mode 100644
index 0000000..c28273a
Binary files /dev/null and b/Robotics_API/__pycache__/Bestman_Real_Xarm.cpython-38.pyc differ
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 0000000..0934c11
Binary files /dev/null and b/Robotics_API/__pycache__/Bestman_Real_Xarm6.cpython-310.pyc differ
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 0000000..4dc7b57
Binary files /dev/null and b/Robotics_API/__pycache__/Bestman_Real_Xarm6.cpython-38.pyc differ
diff --git a/Robotics_API/__pycache__/Bestman_Real_Xarm6.cpython-39.pyc b/Robotics_API/__pycache__/Bestman_Real_Xarm6.cpython-39.pyc
new file mode 100644
index 0000000..0f78ab7
Binary files /dev/null and b/Robotics_API/__pycache__/Bestman_Real_Xarm6.cpython-39.pyc differ
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 0000000..1988416
Binary files /dev/null and b/Robotics_API/__pycache__/Bestman_Real_Xarm7.cpython-310.pyc differ
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 0000000..2974411
Binary files /dev/null and b/Robotics_API/__pycache__/Bestman_Real_Xarm7.cpython-38.pyc differ
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 0000000..aa34832
Binary files /dev/null and b/Robotics_API/__pycache__/Bestman_Real_Xarm7.cpython-39.pyc differ
diff --git a/Robotics_API/__pycache__/Pose.cpython-310.pyc b/Robotics_API/__pycache__/Pose.cpython-310.pyc
new file mode 100644
index 0000000..68bf5d5
Binary files /dev/null and b/Robotics_API/__pycache__/Pose.cpython-310.pyc differ
diff --git a/Robotics_API/__pycache__/Pose.cpython-38.pyc b/Robotics_API/__pycache__/Pose.cpython-38.pyc
new file mode 100644
index 0000000..245b6f0
Binary files /dev/null and b/Robotics_API/__pycache__/Pose.cpython-38.pyc differ
diff --git a/Robotics_API/__pycache__/Pose.cpython-39.pyc b/Robotics_API/__pycache__/Pose.cpython-39.pyc
new file mode 100644
index 0000000..a6df98f
Binary files /dev/null and b/Robotics_API/__pycache__/Pose.cpython-39.pyc differ
diff --git a/Robotics_API/__pycache__/__init__.cpython-310.pyc b/Robotics_API/__pycache__/__init__.cpython-310.pyc
new file mode 100644
index 0000000..c205a5b
Binary files /dev/null and b/Robotics_API/__pycache__/__init__.cpython-310.pyc differ
diff --git a/Robotics_API/__pycache__/__init__.cpython-38.pyc b/Robotics_API/__pycache__/__init__.cpython-38.pyc
new file mode 100644
index 0000000..32c2ba5
Binary files /dev/null and b/Robotics_API/__pycache__/__init__.cpython-38.pyc differ
diff --git a/Robotics_API/__pycache__/__init__.cpython-39.pyc b/Robotics_API/__pycache__/__init__.cpython-39.pyc
new file mode 100644
index 0000000..09a057a
Binary files /dev/null and b/Robotics_API/__pycache__/__init__.cpython-39.pyc differ
diff --git a/Sensor/Camera.py b/Sensor/Camera.py
new file mode 100755
index 0000000..e1aef5e
--- /dev/null
+++ b/Sensor/Camera.py
@@ -0,0 +1,240 @@
+# !/usr/bin/env python
+# -*- encoding: utf-8 -*-
+"""
+# @FileName : Camera.py
+# @Time : 2024-12-02 01:45:01
+# @Author : Yan
+# @Email : yding25@binghamton.edu
+# @Description : Camera functions
+# @Usage : None
+"""
+
+
+import cv2
+import numpy as np
+import pyrealsense2 as rs
+from typing import Dict
+import time
+import threading
+import signal
+
+class Camera:
+ def __init__(self, device_id: str, width: int = 640, height: int = 480, fps: int = 30):
+ """
+ Initializes the Camera class with RealSense device parameters.
+
+ Args:
+ device_id (str): The ID of the RealSense device.
+ width (int): The width of the image stream (default is 640).
+ height (int): The height of the image stream (default is 480).
+ fps (int): Frames per second for the image stream (default is 30).
+ """
+ self.device_id = device_id
+ self.width = width
+ self.height = height
+ self.fps = fps
+
+ # Check if a RealSense device is connected
+ if not self.check_realsense_connection():
+ raise RuntimeError("No RealSense device connected.")
+
+ # Configure RealSense pipeline
+ self.pipeline = rs.pipeline()
+ self.config = rs.config()
+ self.config.enable_device(self.device_id)
+ self.config.enable_stream(rs.stream.color, self.width, self.height, rs.format.bgr8, self.fps)
+ self.config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
+ self.config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
+
+ # Start streaming
+ self.profile = self.pipeline.start(self.config)
+ time.sleep(2) # Wait for the camera to stabilize
+
+ # Get the stream profile and camera intrinsics
+ stream = self.profile.get_stream(rs.stream.color)
+
+ # Debug: Extract camera intrinsic parameters
+ intrinsics = stream.as_video_stream_profile().get_intrinsics()
+ self.camera_matrix_test = np.array([[intrinsics.fx, 0, intrinsics.ppx],
+ [0, intrinsics.fy, intrinsics.ppy],
+ [0, 0, 1]], dtype=float)
+ print(f'self.camera_matrix_test:{self.camera_matrix_test}')
+
+ #TODO: camera matrix is wrong
+ self.camera_matrix = np.array([[607.168, 0, 325.575], [0, 606.94, 325.575], [0, 0, 1]], dtype=float)
+ self.dist_coeffs = np.zeros((4, 1)) # Assuming no lens distortion
+
+ # Load the predefined ArUco marker dictionary
+ self.aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_6X6_250)
+ self.parameters = cv2.aruco.DetectorParameters()
+
+ # check realsense status
+ def check_realsense_connection(self) -> 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 0000000..735cccb
Binary files /dev/null and b/Sensor/__pycache__/Camera.cpython-38.pyc differ
diff --git a/Sensor/__pycache__/__init__.cpython-38.pyc b/Sensor/__pycache__/__init__.cpython-38.pyc
new file mode 100644
index 0000000..195bd75
Binary files /dev/null and b/Sensor/__pycache__/__init__.cpython-38.pyc differ
diff --git a/Sensor/__pycache__/camera.cpython-38.pyc b/Sensor/__pycache__/camera.cpython-38.pyc
new file mode 100644
index 0000000..7624df0
Binary files /dev/null and b/Sensor/__pycache__/camera.cpython-38.pyc differ
diff --git a/Sensor/force.py b/Sensor/force.py
new file mode 100644
index 0000000..9408046
--- /dev/null
+++ b/Sensor/force.py
@@ -0,0 +1,58 @@
+#!/usr/bin/env python3
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2020, UFACTORY, Inc.
+# All rights reserved.
+#
+# Author: Vinman
+
+"""
+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
diff --git a/TODO/Utils.py b/TODO/Utils.py
new file mode 100644
index 0000000..8bd4bac
--- /dev/null
+++ b/TODO/Utils.py
@@ -0,0 +1,10 @@
+# !/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
+"""
\ No newline at end of file