When constructing a project, the following issues need to be paid attention to:
● How to Align the Motion of the Exoskeleton with the Controlled Robotic Arm?
● What Communication Method Should Be Used for Control?
● How to Build the Project?
1. Aligning Exoskeleton Motion Control with Robotic Arm
The concept of DH model is fundamental to robotic kinematics, the article by The American Society of Mechanical Engineers provides a universal standard that clarifies motion calculation methods of robots.
Since the exoskeleton and the robotic arm have different mechanical structures, they also possess distinct DH models, requiring a motion mapping process.
Two methods can be employed to establish the motion mapping.
Method 1: Using Simulation Tools (e.g., ROS-RViz)
With ROS simulation, you can compare the joint directions of the exoskeleton controller and robotic arm without powering the robot, allowing for quick identification of 0° position offsets and simplifying joint angle mapping.
Method 2: Using DH Model to Get Forward Kinematics of Controller
After getting the 3D coordinates and Euler angles (X, Y, Z, A, B, C) of Controller, the data can be applied directly to the robot by sending MoveL(X, Y, Z, A, B, C, vel, acc, dec, P) commands via the robot’s API. Optionally, you can adjust the link lengths in the DH model to recalculate the kinematics and obtain Cartesian coordinates for robot control.
Additionally, the DH information of the exoskeleton can be derived from its open-source URDF model.
We will use method 1 to control the cobot FR3.
2. Control Method
The entire control system consists of three main components:the exoskeleton controller , PC , and the controlled robotic arm . The computer acts as an intermediary, connecting to the exoskeleton controller via a data cable and obtaining real-time information for 14 DOF across both arms through an API. The data is then processed and used to control the robotic arm via methods provided by the manufacturer.
We can use ROS for simulation and data handling . The ROS topic-based communication mechanism supports using both Python and C++ within the same project, offering excellent compatibility with hardware API . This approach not only allows for the seamless addition of publishers and subscribers after joint interfacing but also facilitates the efficient transfer of joint angle information.
3. Project Setup
The project was built in an Ubuntu 20.04 + ROS Noetic environment. To avoid errors, ensure the same setup is used.
3.1 Create a workspace, clone the project, and perform the initial build.
bashcd ~mkdir myController_ws && mkdir myController_ws/src && cd myController_ws/srcgit clone https://github.com/FAIR-INNOVATION/frcobot_ros.gitgit clone -b mycontroller_s570 https://github.com/elephantrobotics/mycobot_ros.gitcd ..rosdep install --from-paths src --ignore-src -r -ycatkin_make
You can test whether it runs normally by running RViz of the exoskeleton. Follow Gitbook of myController S570
bashroslaunch mycontroller_s570 test.launch
3.2 Align the movements manually.
You should add a new launch file under the ‘xxx_description’ package. This file is used to load the robotic joint information and control joint angles by receiving node information in ROS, helping to establish the joint motion relationship between the robotic arm and the exoskeleton.
bashcd ~/myController_ws/src/frcobot_ros/frcobot_description/launchtouch fr3robot_control_rviz.launchgedit fr3robot_control_rviz.launch
（fr3robot_control_rviz.launch)
<launch> <param name="robot_description" textfile="$(find frcobot_description)/urdf/fr3_robot.urdf" /> <node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" /> <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/> <node name="rviz" pkg="rviz" type="rviz" args="-d $(find frcobot_description)/launch/fr3robot.rviz"/> <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world dual_base"/></launch>
Close the file and continue.
bashcd ~/myController_ws/catkin_makeroslaunch frcobot_description fr3robot_control_rviz.launch
After that, We can load the robotic arm model in RViz and use the sliders to check the joint angles and rotation directions against the controller, making initial adjustments as needed.
3.3 Modify the exoskeleton code for communication.
Run the following commands in a terminal:
bashcd /home/u204/myController_ws/src/mycobot_ros/mycontroller_s570/scriptsgedit test.py
Rewrite the code of handling the left and right arm data. In this example, only the date of right arm is used to control single robot arm.
(test.py)
#!/usr/bin/env pythonfrom exoskeleton_api import Exoskeleton, ExoskeletonSocketimport rospyfrom math import piimport timefrom sensor_msgs.msg import JointStatefrom std_msgs.msg import Headerimport rosnodeimport osimport subprocessos.system("sudo chmod 777 /dev/ttyACM*")obj = Exoskeleton(port="/dev/ttyACM0")def shutdown_ros_node(node_name): try: subprocess.run(['rosnode', 'kill', node_name]) print(f"Node {node_name} has been shutdown.") except subprocess.CalledProcessError as e: print(f"Error: {e}")# Init ROS node#_________ change _________rospy.init_node("fr3_joint_state_publisher")#_________ change _________shutdown_ros_node('joint_state_publisher_gui')pub = rospy.Publisher('/joint_states', JointState, queue_size=10)rate = rospy.Rate(100) # 100Hzjoint_state = JointState()while not rospy.is_shutdown(): joint_state.header = Header() #_________ change _________ joint_state.header.stamp = rospy.Time.now() joint_state.name = [ 'j1', 'j2', 'j3', 'j4', 'j5', 'j6'] l_angle = obj.get_arm_data(1) l_angle = l_angle[:7] l_angle = [int(x) for x in l_angle] r_angle = obj.get_arm_data(2) r_angle = r_angle[:7] r_angle = [int(y) for y in r_angle] rad_l_angle = [a/180*pi for a in l_angle] joint_state.position = [0.0] * 6 # Initialize with 6 elements #only use r_angle and converse joints angle #FR 1 = EX 2 joint_state.position[0] = -r_angle[1] - 70 #FR 2 = EX 1 joint_state.position[1] = r_angle[0] -170 #FR 3 = EX 4 joint_state.position[2] = -r_angle[3] - 45 #FR 4 = EX 6 joint_state.position[3] = r_angle[5] - 50 #FR 5 = EX 5 joint_state.position[4] = r_angle[4] + 90 #FR 6 = EX 7 joint_state.position[5] = -r_angle[6] # convert to position joint_state.position = [a/180*pi for a in joint_state.position]#_________ change _________ joint_state.effort = [] joint_state.velocity = [] # publish pub.publish(joint_state) rospy.loginfo('success') for i in range(6): print(f"j{i+1}: {joint_state.position[i]/pi*180}") # wait rate.sleep()
Close the file and run the script to verify the motion.
bashpython3 test.py
3.4 Drive Real Robot
After successfully adjusting, we need to receive the joint information and drive real robot according to the robot’s API.
This step requires writing code based on the specific API of different robots. To simplify the demonstration, We fixed the angles of J1, J5, and J6.
bashcd /home/u204/myController_ws/src/frcobot_ros/frcobot_python_sdk-main/linux/exampletouch control_test.py && gedit control_test.py
（control_test.py)
#!/usr/bin/env python3import rospyfrom sensor_msgs.msg import JointStateimport mathimport timeimport frrpc# Define your parametersacc = 0.01vel = 0.01t = 0.01lookahead_time = 0.0P = 0.0eP1=[0.000,0.000,0.000,0.000]dP1=[1.000,1.000,1.000,1.000,1.000,1.000]# Initialize joint_angles_deg as a global variablejoint_angles_deg = [0.0] * 6robot = frrpc.RPC('192.168.58.2')def joint_state_callback(msg): global joint_angles_deg joint_angles_rad = msg.position[:6] # Convert angles from radians to degrees joint_angles_deg = [angle * (180.0 / math.pi) for angle in joint_angles_rad] # Modify specific joint angles as needed # fixed J1 J5 J6 to avoid robot collision, just for test joint_angles_deg[0] = -16.11 joint_angles_deg[4] = 90.0 joint_angles_deg[5] = 90.0# Initialize the ROS noderospy.init_node('joint_state_listener', anonymous=True)# Subscribe to the /joint_states topicrospy.Subscriber('/joint_states', JointState, joint_state_callback)while not rospy.is_shutdown(): # Update the joint angles whenever new data is received if joint_angles_deg: # Check if joint_angles_deg has been updated if(joint_angles_deg[0]!=0.0): joint_angles_deg = [float(angle) for angle in joint_angles_deg] joint_c = robot.GetForwardKin(joint_angles_deg) joint_c = [joint_c[1],joint_c[2],joint_c[3],joint_c[4],joint_c[5],joint_c[6]] ret = robot.RobotEnable(1) ret = robot.MoveJ(joint_angles_deg,joint_c,1,0,250.0,200.0,100.0,eP1,-1.0,0,dP1) print("Updated Joint Angles (Degrees):") for i, angle in enumerate(joint_angles_deg): print(f"Joint {i+1}: {angle:.2f} degrees") for i, angle in enumerate(joint_c): print(f"L{i+1}: {angle:.2f} degrees") time.sleep(0.02)
4. Test
We need to run 3 terminals in total:
● One for launching RViz to display the robot’s simulation status
● One for receiving data of myController S570 and publishing it as node messages in ROS
● One for calling the robot API , receiving node messages, and controlling the robot
Terminal A:
cd ~/myController_wsroslaunch frcobot_description fr3robot_control_rviz.launch
Terminal B:
cd /home/u204/myController_ws/src/mycobot_ros/mycontroller_s570/scriptspython3 test.py
Terminal C:
cd /home/u204/myController_ws/src/frcobot_ros/frcobot_python_sdk-main/linux/examplepython3 control_test.py
Then you can try using exoskeleton to control real cobot.
Summary
We used the FAIRINO Cobot FR3 to demonstrate how the myController S570 exoskeleton controller can remotely control a collaborative robot.
The strong compatibility of the myController S570 enables developers to easily perform secondary development, including remote control of various robots and simplify the data collection process.
