使用外骨骼灵活远程控制协作机器人案例

外骨骼控制器采用可调节结构，简化了机器人编程，使协作机器人 FR3 的远程控制变得容易。

一、引言

在开发机器人手臂或双臂系统的应用程序时，经常会遇到以下挑战：

1. 使用拖动和示教进行定位的困难：拖动和示教功能通常效率低下且劳动密集。

2. 示教器效率低下： 使用示教器进行路径编程复杂、容易出错且效率低下。

3. 与人体运动不匹配：将双臂机器人运动与自然人体运动保持一致具有挑战性，使运动数据收集复杂化。

4. 远程控制限制：由于对碰撞或开发要求的担忧，对远程控制的需求一直很高。

myController S570 是应对这些挑战的理想工具，因为它专为机器人系统的远程控制和运动数据收集而设计。主要功能包括：

● 可调接头：J3 和 J4 之间的角度可以通过扭动螺丝自由调整（90° 或 0°），允许修改 DH 模型参数以兼容各种 6 DOF 机械臂。

● 高性能：凭借快速的数据采集和近乎零的延迟，控制器与各种机器人无缝集成，用于数据接口和控制。

我们将使用 FR3 协作机器人来演示如何无缝调整外骨骼控制器以远程作协作机器人。

二、硬件介绍

Elephant Robotics myController S570

myController S570 是大象机器人首款便携外骨骼数据采集设备，具备便携式穿戴、精准动作捕捉和高度开源等特性，涵盖数据采集、遥操作、端到端应用开发等场景，助力研究人员与开发者，在教育培训中结合人形、轮式等机器人，拓宽教学科研实践，促进知识技能传播与掌握。

三、项目

在构建项目时，需要注意以下问题：

● 如何将外骨骼的运动与受控机械臂对齐？

● 应使用什么通讯方式进行控制？

● 如何构建项目？

1. 将外骨骼运动控制与机械臂对齐

DH 模型的概念是机器人运动学的基础，美国机械工程师协会的文章提供了一个通用标准，阐明了机器人的运动计算方法。

由于外骨骼和机械臂具有不同的机械结构，因此它们也具有不同的 DH 模型，需要运动映射过程。

可以采用两种方法来建立运动映射。

方法 1：使用模拟工具（例如 ROS-RViz）

通过 ROS 仿真，您可以在不为机器人供电的情况下比较外骨骼控制器和机械臂的关节方向，从而快速识别 0° 位置偏移并简化关节角度映射。

方法 2：使用 DH 模型获取控制器的正向运动学

在得到控制器的 3D 坐标和欧拉角（X、Y、Z、A、B、C）后，可以通过机器人的 API 发送 MoveL（X、Y、Z、A、B、C、vel、acc、dec、P）命令，将数据直接应用于机器人。或者，您可以在 DH 模型中调整链接长度，以重新计算运动学并获取机器人控制的笛卡尔坐标。

此外，外骨骼的 DH 信息可以从其开源 URDF 模型派生。

我们将使用方法 1 来控制协作机器人 FR3。

2. 控制方法

整个控制系统由三个主要部分组成：外骨骼控制器、PC 和受控机械臂。计算机充当中介，通过数据线连接到外骨骼控制器，并通过 API 获取双臂 14 DOF 的实时信息。然后，数据被处理并通过制造商提供的方法用于控制机械臂。

我们可以使用 ROS 进行模拟和数据处理。ROS 基于主题的通信机制支持在同一项目中同时使用 Python 和 C++，与硬件 API 具有出色的兼容性。这种方法不仅允许在联合接口后无缝添加发布者和订阅者，而且还有助于高效传输联合角度信息。

3. 项目设置

该项目是在 Ubuntu 20.04 + ROS Noetic 环境中构建的。为避免错误，请确保使用相同的设置。

3.1 创建企业空间，克隆项目，并执行初始构建。

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

您可以通过运行外骨骼的 RViz 来测试它是否正常运行。

bash

roslaunch mycontroller_s570 test.launch

3.2 手动对齐动作。

您应该在 'xxx_description' 包下添加新的启动文件。该文件用于通过在 ROS 中接收节点信息来加载机器人关节信息并控制关节角度，帮助建立机械臂与外骨骼的关节运动关系。

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>

 关闭文件并继续。

bash

cd ~/myController_ws/
catkin_make
roslaunch frcobot_description fr3robot_control_rviz.launch

之后，我们可以在 RViz 中加载机械臂模型，并使用滑块对照控制器检查关节角度和旋转方向，根据需要进行初步调整。 

3.3 修改通信的外骨骼代码。

在终端中运行以下命令：

bash

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

重写处理左右臂数据的代码。在此示例中，仅使用右臂的日期来控制单个机器人手臂。

（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()

关闭文件并运行脚本以验证运动。

bash

python3  test.py

3.4 驾驶真人机器人

调整成功后，我们需要接收关节信息，并根据机器人的 API 驱动真实机器人。

此步骤需要根据不同机器人的特定 API 编写代码。为了简化演示，我们修复了 J1、J5 和 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. 测试

我们总共需要运行 3 个终端：

● 一个用于启动 RViz 以显示机器人的模拟状态

● 一个用于接收 myController S570 的数据 并将其 作为节点消息发布到 ROS 中

● 一个用于调用机器人 API、接收节点消息和控制机器人

航站楼 A：

cd ~/myController_ws
roslaunch frcobot_description fr3robot_control_rviz.launch

航站楼 B：

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

C 航站楼：

cd /home/u204/myController_ws/src/frcobot_ros/frcobot_python_sdk-main/linux/example
python3 control_test.py

然后你可以尝试使用外骨骼来控制真正的协作机器人。

总结

我们使用 FAIRINO 协作机器人 FR3 演示了 myController S570 外骨骼控制器如何远程控制协作机器人。

myController S570 的强大兼容性使开发人员能够轻松进行二次开发，包括远程控制各种机器人并简化数据收集过程。

通过使用外骨骼的摇杆和按钮来控制机器人的末端执行器，用户可以解锁更多潜在的应用场景。我们期待看到创客们更多创意发展，并热忱邀请您参与我们的案例收集活动：Call for User Cases - Elephant Robotics。