Adjustable Controller Makes Robot Remote Control Flexible

When developing applications for robot arms or dual-arm systems, the following challenges are often encountered:

1. Difficulty in Positioning with Drag&Teach：The drag & teach function is often inefficient and labor-intensive.

2. Inefficiency of Teach Pendant: Path programming with a teach pendant is complex, error-prone, and inefficient.

3. Mismatch with Human Motion: Aligning dual-arm robot movements with natural human motion is challenging, complicating motion data collection.

4. Remote Control Limitations: Due to concerns of collisions or development requirements, the demand for remote control has always been high.

The myController S570 stands out as an ideal tool to these challenges as it designed specifically for remote control and motion data collection for robot systems. Key features include:

● AdjustableJoint: The angle between J3 and J4 can be freely adjusted (90° or 0°) by twisting a screw, allowing modification of DH model parameters for compatibility with a variety of 6 DOF robotic arms.

● High Performance: With rapid data acquisition and near-zero latency, the controller seamlessly integrates with various robots for data interfacing and control.

We will use the FR3 cobot to demonstrate how the exoskeleton controller can be seamlessly adapted for remote operation of a collaborative robot.

Project

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.

bash

cd ~
mkdir myController_ws && mkdir myController_ws/src && cd myController_ws/src
git clone https://github.com/FAIR-INNOVATION/frcobot_ros.git
git clone  -b mycontroller_s570 https://github.com/elephantrobotics/mycobot_ros.git
cd ..
rosdep install --from-paths src --ignore-src -r -y
catkin_make

You can test whether it runs normally by running RViz of the exoskeleton. Follow Gitbook of myController S570

bash

roslaunch 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.

bash

cd ~/myController_ws/src/frcobot_ros/frcobot_description/launch
touch fr3robot_control_rviz.launch
gedit 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.

bash

cd ~/myController_ws/
catkin_make
roslaunch 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:

bash

cd /home/u204/myController_ws/src/mycobot_ros/mycontroller_s570/scripts
gedit 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 python

from exoskeleton_api import Exoskeleton, ExoskeletonSocket
import rospy
from math import pi
import time
from sensor_msgs.msg import JointState
from std_msgs.msg import Header
import rosnode
import os
import subprocess

os.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) # 100Hz

joint_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.

bash

python3  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.

bash

cd /home/u204/myController_ws/src/frcobot_ros/frcobot_python_sdk-main/linux/example
touch control_test.py && gedit control_test.py

（control_test.py)

#!/usr/bin/env python3

import rospy
from sensor_msgs.msg import JointState
import math
import time
import frrpc

# Define your parameters
acc = 0.01
vel = 0.01
t = 0.01
lookahead_time = 0.0
P = 0.0
eP1=[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 variable
joint_angles_deg = [0.0] * 6

robot = 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 node
rospy.init_node('joint_state_listener', anonymous=True)

# Subscribe to the /joint_states topic
rospy.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_ws
roslaunch frcobot_description fr3robot_control_rviz.launch

Terminal B:

cd /home/u204/myController_ws/src/mycobot_ros/mycontroller_s570/scripts
python3 test.py

Terminal C:

cd /home/u204/myController_ws/src/frcobot_ros/frcobot_python_sdk-main/linux/example
python3 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.

By using the exoskeleton's joysticks and buttons to control end-effectors of robots, users may unlock more potential application scenarios. We look forward to seeing more creative developments from makers and warmly invite you to participate in our activity of case collection: https://www.elephantrobotics.com/en/call-for-user-cases-en/.