Author: <Baekseok Kim> Email: kimb52@unlv.nevada.edu
Date: Last modified on <02/14/23>
Keywords: RB5 manipulator, RB5 Programming, collaborative robot, Admittance Control

The video above depicts the goal of this tutorial The RB5 already have collision detection and gravity compensation mode built in that allows it to work collaboratively with human. However, it doesn't have any Force/Torque sensor. I added the F/T sensor and implement compliance control algorithm and tested it. This tutorial introduce how to connect External sensor on RB5 controller and how to get the external sensor data from RB5 controller to PC and how to use that data for control RB5.

Before starting this tutorial, the author assumes you either:

• Understand basic robotics knowledge
• Understand Control System Theory

In previous tutorials, we learned to connect an external sensor to RB5 and get sensor data with external script. In this tutorial, we will learn how to control the RB5 based on the sensor data.

The rest of this tutorial is presented as follows:

• Python Robotics Tool Box
• Configure RB5 Kinematic Parameter in Python Robotics Tool Box
• Admittance Control Algorithm
• Final Words

This toolbox brings robotics-specific functionality to Python, and leverages Python's advantages of portability, ubiquity and support, and the capability of the open-source ecosystem for linear algebra (numpy, scipy), graphics (matplotlib, three.js, WebGL), interactive development (jupyter, jupyterlab, mybinder.org), and documentation (sphinx).

The Toolbox provides tools for representing the kinematics and dynamics of serial-link manipulators - you can easily create your own in Denavit-Hartenberg form, import a URDF file, or use over 30 supplied models for well-known contemporary robots from Franka-Emika, Kinova, Universal Robotics, Rethink as well as classical robots such as the Puma 560 and the Stanford arm.

The Toolbox contains fast implementations of kinematic operations. The forward kinematics and the manipulator Jacobian can be computed in less than 1 microsecond while numerical inverse kinematics can be solved in as little as 4 microseconds.

Tutorial for Robotics Tool Box

https://github.com/petercorke/robotics-toolbox-python#3

Tutorial for Manipulator differential kinematics

https://github.com/jhavl/dkt

You will need Python >= 3.6

Using pip Install a snapshot from PyPI

pip3 install roboticstoolbox-python

Available options are:

collision install collision checking with pybullet Put the options in a comma separated list like

pip3 install roboticstoolbox-python[optionlist]

From GitHub To install the bleeding-edge version from GitHub

git clone https://github.com/petercorke/robotics-toolbox-python.git
cd robotics-toolbox-python
pip3 install -e .

Download the RB5 setup file and transfer it to the robotics toolbox folder.

route : (python dir)\Lib\site-packages\roboticstoolbox\models\DH

rb5.zip

import numpy as np
from roboticstoolbox import DHRobot, RevoluteDH
from spatialmath import SE3
 
 
class RB5(DHRobot):
    """
    Class that models a Rainbow Robotics RB5 manipulator
 
    :param symbolic: use symbolic constants
    :type symbolic: bool
 
    ``RB5()`` is an object which models a Unimation Puma560 robot and
    describes its kinematic and dynamic characteristics using standard DH
    conventions.
 
    .. runblock:: pycon
 
        >>> import roboticstoolbox as rtb
        >>> robot = rtb.models.DH.RB5()
        >>> print(robot)
 
    Defined joint configurations are:
 
    - qz, zero joint angle configuration
    - qr, arm horizontal along x-axis
 
    .. note::
        - SI units are used.
 
    :References:
 
    """  # noqa
 
    def __init__(self, symbolic=False):
 
        if symbolic:
            import spatialmath.base.symbolic as sym
 
            zero = sym.zero()
            pi = sym.pi()
        else:
            from math import pi
 
            zero = 0.0
 
        deg = pi / 180
        inch = 0.0254
 
        a =     [0,   0.42500,    0.39225,      0,      0,      0]
        d =     [0.1692,    0,          0,-0.1107, 0.1107,-0.0967-0.003]
        alpha = [-pi/2,  zero,       zero,   pi/2,  -pi/2,   pi/2]
        offset= [0,     -pi/2,          0,   pi/2,      0,      0]
        # mass data, no inertia available
        mass = [5.588336, 9.503399, 3.202978, 1.356780, 1.356780, 0.171541]
        center_of_mass = [
            [0.000012, -0.002613, 0.060352],
            [0, -0.114981, 0.211114],
            [0.000004, -0.018194, 0.219836],
            [-0.000024, -0.106297, 0.029356],
            [0.000024, -0.029356, -0.044022],
            [0.001715, -0.080700, -0.000180],
        ]
        inertia = [
            [0.041227473290685, 0.040817666882974, 0.012209015209776,   -0.000007297191574, -0.001034629955434, -0.000008403038448],
            [0.949319006774135, 0.819596599298864, 0.146822420639023,   -0.000000000004452, -0.234095204410459,  0.000009076795146],
            [0.245139908783287, 0.243950697411086, 0.006921912119249,   -0.000000596636031, -0.015628137286249,  0.000013885046111],
            [0.017845581259381, 0.002477039039656, 0.016422745831595,    0.000002891849882, -0.004375863929516, -0.000000525951420],
            [0.002541474364616, 0.001118638936830, 0.002477039039657,   -0.000000525951420,  0.000033215608299, -0.000000669457895],
            [0.001442738077985, 0.000213622367571, 0.001446789772376,   -0.000002617454254,  0.000002748095325, -0.000000228220422]        
        ]
        links = []
 
        for j in range(6):
            link = RevoluteDH(
                d=d[j], a=a[j], alpha=alpha[j], offset=offset[j], m=mass[j], r=center_of_mass[j], G=1, I=inertia[j]
            )
            links.append(link)
 
        super().__init__(
            links,
            name="RB5",
            manufacturer="Rainbow Robotics",
            keywords=("dynamics", "symbolic"),
            symbolic=symbolic,
        )
 
        self.qr = np.array([180, 0, 0, 0, 90, 0]) * deg
        self.qz = np.zeros(6)
 
        self.addconfiguration("qr", self.qr)
        self.addconfiguration("qz", self.qz)
 
 
if __name__ == "__main__":  # pragma nocover
 
    rb5 = RB5(symbolic=False)
    print(rb5)
    # print(rb5.dyntable())

After that, you can check the DH parameter of RB5 as shown below by executing the following code.

import roboticstoolbox as rtb
robot = rtb.models.DH.RB5()
print(robot)
 
DHRobot: RB5 (by Rainbow Robotics), 6 joints (RRRRRR), dynamics, standard DH parameters
┌──────────┬─────────┬────────┬────────┐        
│   θⱼ     │   dⱼ    │   aⱼ   │   ⍺ⱼ   │        
├──────────┼─────────┼────────┼────────┤        
│ q1       │  0.1692 │      0 │ -90.0° │        
│ q2 - 90° │       0 │  0.425 │   0.0° │        
│ q3       │       0 │ 0.3922 │   0.0° │        
│ q5       │  0.1107 │      0 │ -90.0° │        
│ q6       │ -0.0997 │      0 │  90.0° │        
└──────────┴─────────┴────────┴────────┘        
 
┌─┬──┐
└─┴──┘
 
┌─────┬───────┬─────┬─────┬─────┬──────┬─────┐  
│name │ q0    │ q1  │ q2  │ q3  │ q4   │ q5  │  
├─────┼───────┼─────┼─────┼─────┼──────┼─────┤  
│  qr │  180° │  0° │  0° │  0° │  90° │  0° │  
│  qz │  0°   │  0° │  0° │  0° │  0°  │  0° │  
└─────┴───────┴─────┴─────┴─────┴──────┴─────┘  

* Demonstrate to get FT sensor data using script command on your Laptop, using python, C++ or something.

* Change and printout the FT sensor's coordinate system based on the global coordinate system using a rotation matrix.

Compliance Control

import os
from multiprocessing import Process, Queue, current_process
from xmlrpc.client import boolean
 
from matplotlib.pyplot import get
import matplotlib as mpl
import roboticstoolbox as rtb
import numpy as np
 
import time
usleep = lambda x: time.sleep(x/1000000.0)
 
from math import pi
from queue import Queue 
import threading
 
 
import socket
import sys
import struct
 
from numpy import uint16, uint8, int16, int8
RB5_ip = "10.0.2.7"
 
 
global data_socket
global cmd_socket
global bPause
bPause = False
 
global robot
global robot_state
robot_state = -1
global iRequestSkipUpdate
iRequestSkipUpdate = 0
 
global MAX_SKIP_UPDATE 
MAX_SKIP_UPDATE = 10
 
global robot_pose
global robot_jnt
global robot_twist_pos
global robot_twist_jnt
global robot_end_rotmat
global end2ext_ft_rotmat
global ext_ft_rotmat
 
robot_end_rotmat = np.array([[1,0,0],
                        [0,1,0],
                        [0,0,1]])
end2ext_ft_rotmat = np.array([[1,0,0],
                        [0,0,-1],
                        [0,1,0]])
 
ext_ft_rotmat = np.array([[1,0,0],
                        [0,1,0],
                        [0,0,1]])
robot_pose = np.zeros(6)
robot_jnt = np.zeros(6)
robot_twist_pose = np.zeros(6)
robot_twist_jnt = np.zeros(6)
 
desired_robot_twist_pose = np.zeros(6)
 
global step_time
 
 
 
####### Set Parameter for Admittance control #######
 
mass_arm = [
    [1, 0, 0, 0, 0, 0],
    [0, 1, 0, 0, 0, 0],
    [0, 0, 1, 0, 0, 0],
    [0, 0, 0,.2, 0, 0],
    [0, 0, 0, 0,.1, 0],
    [0, 0, 0, 0, 0,.1]
            ]
 
# mass_arm = [
#     [1, 0, 0, 0, 0, 0],
#     [0, 1, 0, 0, 0, 0],
#     [0, 0, 1, 0, 0, 0],
#     [0, 0, 0, 10000, 0, 0],
#     [0, 0, 0, 0, 10000, 0],
#     [0, 0, 0, 0, 0, 10000]
#             ]
 
 
# mass_arm = [
#     [10000, 0, 0, 0, 0, 0],
#     [0, 10000, 0, 0, 0, 0],
#     [0, 0, 10000, 0, 0, 0],
#     [0, 0, 0, .1, 0, 0],
#     [0, 0, 0, 0, .1, 0],
#     [0, 0, 0, 0, 0, .1]
#             ]
 
 
damping_coupling = [
    [20,0, 0, 0, 0, 0],
    [0,20, 0, 0, 0, 0],
    [0, 0, 0, 0, 0, 0],
    [0, 0, 0, 0, 0, 0],
    [0, 0, 0, 0, 0, 0],
    [0, 0, 0, 0, 0,10]
                   ]
 
damping_arm = [
    [60, 0, 0, 0, 0, 0],
    [ 0,60, 0, 0, 0, 0],
    [ 0, 0,60, 0, 0, 0],
    [ 0, 0, 0,15, 0, 0],
    [ 0, 0, 0, 0,15, 0],
    [ 0, 0, 0, 0, 0,15]
    ]
 
stiffness_coupling = [
    [0,0,0, 0, 0, 0],
    [0,0,0, 0, 0, 0],
    [0,0,0, 0, 0, 0],
    [0,0,0,10, 0, 0],
    [0,0,0, 0,10, 0],
    [0,0,0, 0, 0,10]
    ]
 
M_a_=np.array(mass_arm)*.05
D_a_=np.array(damping_arm)*0
arm_max_vel_ = 1.5
arm_max_acc_ = 1.0
platform_max_vel_ = 0.1
platform_max_acc_ = 0.2
step_time = 0.005
 
 
def pause():
 
    bPause = True
 
def resume():
 
    bPause = False
 
 
####### RB5 API #######
#RB5 API to control RB5 Robot, 
#This RB5 API is based on RB5 manual
 
 
def Rb5_decode(data):
    decode_data=[]
    for i in range(26):
        data_4byte=struct.unpack('f',data[i*4:i*4+4])[0]   
        # print(data_4byte)
        decode_data.append(data_4byte)
    for i in range(6):
        data_4byte=struct.unpack('f',data[i*4+488:i*4+4+488])[0]   
        # print(data_4byte)
        decode_data.append(data_4byte)
    return decode_data
 
def Rb5_getdata(sock):
    bufsize = 4096
    msg='reqdata'
    sdata=msg.encode()
    sock.send(sdata)
    data = sock.recv(bufsize)
    res=Rb5_decode(data)
 
    # print(res)
    return res
 
 
def get_data_thread(sock, q_in, q_out):
    print('start')
    # ft_calib=calib_ft_data()
    while True:
        cmd=q_in.get()
        if cmd == 1:
            rb5_data_ = Rb5_getdata(sock)
            q_out.put(rb5_data_)
 
def sendCommand(cmd):
    global cmd_socket
    send_cmd=cmd.encode()
 
    # while(bPause == true):
 
    #     usleep(1000*100)
 
    cmd_socket.send(send_cmd)
 
 
def isMotionIdle():
    global robot_state
    global iRequestSkipUpdate
    print(iRequestSkipUpdate)
    if(iRequestSkipUpdate >0):
 
    #    //     printf("IsMotionIdle : false\r\n");
        return False
 
        # //else if robot is idle, finish waiting
    #   //  printf("IsMotionIdle : %d\r\n",(systemStat.sdata.robot_state == 1) || (systemStat.sdata.robot_state == 0) );
    return ((robot_state == 1) or (robot_state == 0))
 
 
def waitUntilMotionDone():
    usleep(10)
    # while(isMotionIdle() != True):
    #     print(isMotionIdle())
    #     usleep(10)
 
 
def initRobot():
 
    sendCommand("mc jall init")
    time.sleep(1)
 
 
#  <ProgramMode_Real>
#  change to 'real robot' mode -- robot will move
def setRealMode():
 
    sendCommand("pgmode real")
    time.sleep(1)
    # return *this;
 
# <ProgramMode_Simulation>
# change to 'simulation' mode -- robot will not move except teaching
def setSimulationMode():
 
    sendCommand("pgmode simulation")
    time.sleep(1)
    # return *this;
 
 
# // <MoveJoint>
# // : move to target posture in joint coordinate
# // joint1~joint6 : target joint angle in deg unit
# // spd : speed parameter (0~1: user define   or   -1: default setting)
# // acc : acceleration parameter (0~1: user define   or   -1: default setting)
def moveJoint(joint, spd = -1.0, acc = -1.0):
    return moveJoint_raw(joint[0],joint[1],joint[2],joint[3],joint[4],joint[5],spd,acc)
 
def moveJoint_raw(joint1, joint2, joint3, joint4, joint5, joint6, spd=-1.0, acc=-1.0):
    global robot_state 
    global iRequestSkipUpdate
    sendCommand("jointall {:.3f}, {:.3f}, {:.3f}, {:.3f}, {:.3f}, {:.3f}, {:.3f}, {:.3f}".format(spd,
                acc,
                joint1,
                joint2,
                joint3,
                joint4,
                joint5,
                joint6))
    robot_state = 3 #run
    iRequestSkipUpdate = MAX_SKIP_UPDATE
    waitUntilMotionDone()
 
 
 
# // If you send "move_servo_j(jnt[ANGLE0, ANGLE1, ANGLE2, ANGLE3, ANGLE4, ANGLE5], time1, time2, gain, lpf_gain)"
# // to our system through port number 5000, you can move joints in soft-real-time.
# // time1 : user's command sending period/gap (unit: second) (>0.002) : default = 0.002
# // time2 : lookahead time (unit: second) (0.021 ~ 0.2) : default = 0.1
# // gain : control gain (0~1) : default = 0.02
# // lpf_gain : low pass filter gain (0~1) : default = 0.2
# // This function is the same as UR's servo_j function.
 
 
def servoJoint(joint, time1 = 0.002, time2 = 0.1, gain = 0.02, lpf_gain=0.2):
 
    return servoJoint_raw(joint[0],joint[1],joint[2],joint[3],joint[4],joint[5],time1, time2, gain, lpf_gain)
 
def servoJoint_raw(joint1, joint2, joint3, joint4, joint5, joint6, time1 = 0.002, time2 = 0.1, gain = 0.02, lpf_gain=0.2):
    global robot_state 
    global iRequestSkipUpdate
 
    sendCommand("move_servo_j(jnt[{:.3f}, {:.3f}, {:.3f}, {:.3f}, {:.3f}, {:.3f}],{:.3f}, {:.3f}, {:.3f}, {:.3f})".format(
                joint1,
                joint2,
                joint3,
                joint4,
                joint5,
                joint6,
                time1,  
                time2, 
                gain, 
                lpf_gain))
    robot_state = 3 #run
    iRequestSkipUpdate = MAX_SKIP_UPDATE
    waitUntilMotionDone()
 
 
 
 
# // <MoveTCP>
# // : move to target posture in cartesian coordinate
# // x, y, z : target TCP(tool center point) position in mm unit
# // rx, ry, rz : target TCP orientation (Yaw-Pitch-Roll Euler angle) in degree unit
# // spd : speed parameter (0~1: user define   or   -1: default setting)
# // acc : acceleration parameter (0~1: user define   or   -1: default setting)
def  moveTCP(pose, spd = -1, acc=-1):
 
    return moveTCP_raw(pose[0],pose[1],pose[2],pose[3],pose[4],pose[5],spd,acc)
 
def  moveTCP_raw(x, y, z, rx, ry, rz, spd = -1, acc = -1):
 
    sendCommand("movetcp {:.3f}, {:.3f}, {:.3f}, {:.3f}, {:.3f}, {:.3f}, {:.3f}, {:.3f}".format(
        spd,
        acc,
        x,
        y,
        z,
        rx,
        ry,
        rz))
    robot_state = 3; #run
    iRequestSkipUpdate = MAX_SKIP_UPDATE
    waitUntilMotionDone()
 
# get robot position/joint angle and velocity(twist) parameter 
def get_robot_param():
    global rb5_data
    global robot_pose
    global robot_jnt
    global robot_twist_pose 
    global robot_twist_jnt
    global step_time
    global ft_data
    ft_data = np.array(rb5_data[26:32])
    robot_jnt_now = np.array(rb5_data[2:8])*pi/180.0
    robot_pose_now = np.array(rb5_data[20:26])/1000.0
    robot_twist_pose = (robot_pose_now-robot_pose)/step_time
    robot_twist_jnt = (robot_jnt_now-robot_jnt)/step_time
    # print(robot_twist_pose)
    # print(robot_twist_jnt)
 
    robot_pose = robot_pose_now 
    robot_jnt = robot_jnt_now
    # print(robot_pose)
 
 
#main part of compute admittance control algorithm
def compute_admittance(): 
    global rb5_data
    global robot_pose
    global robot_jnt
    global robot_twist_pose 
    global desired_robot_twist_pose 
    global robot_twist_jnt
    global step_time
    global robot
    global ext_ft_rotmat
 
    # Vector6d platform_desired_acceleration
    wrench_external_=wrench_external_callback()
    arm_desired_accelaration = np.zeros(6)
 
    # ddot(p) = M^-1*(-C*J*dot(q)-f_ext), ignore stiffness here.
    arm_desired_accelaration = np.dot(np.linalg.inv(M_a_) , ( - np.dot(D_a_,robot_twist_pose)+ wrench_external_))
                            #    + wrench_external_ + wrench_control_)
 
    # dot(p) = dot(p)_0+ddot(p)*step_time
    arm_desired_err_ = robot_twist_pose + arm_desired_accelaration * step_time
 
    # dot(q)=J^-1*dot(p)
    arm_desired_jnt_err_ = np.dot(np.linalg.inv(robot.jacob0(robot_jnt)) ,arm_desired_err_)
    #                         #    + wrench_external_ + wrench_control_)
 
    # limiting the accelaration for better stability and safety
    a_acc_norm = np.linalg.norm(arm_desired_accelaration[0:3])
 
    # check robot speed over the safety speed 
    if (a_acc_norm > arm_max_acc_):
        arm_desired_accelaration[0:3] *= (arm_max_acc_ / a_acc_norm)
 
 
    # Integrate for velocity based interface
    servoJoint((robot_jnt+arm_desired_jnt_err_* step_time)*180/pi)    
 
    # print calculate result to debug
    # print(wrench_external_)   
    # print(-np.dot(D_a_,robot_twist_pose))
    # print(arm_desired_accelaration) 
    # print(robot_twist_pose)   
    # print(robot_pose)
    # print(np.linalg.inv(robot.jacob0(robot_jnt)))
    # print(arm_desired_accelaration)
    print('arm')
    print(arm_desired_err_)
    # print(arm_desired_jnt_err_)
 
 
 
def get_rotation_matrix():
    global robot
 
    global robot_end_rotmat
    global end2ext_ft_rotmat
    global ext_ft_rotmat
 
    # To get end-effector coordinate matrix
    # Using robotics toolbox package
    rot_mat = np.array(robot.fkine(robot_jnt))
    # print(rot_mat)
    robot_end_rotmat = rot_mat[:3,:3]
    ext_ft_rotmat = np.dot(robot_end_rotmat,end2ext_ft_rotmat)
 
def wrench_external_callback():
 
    global ext_ft_rotmat
    global ft_data
    get_rotation_matrix()
    ft_data_base = np.concatenate([np.dot(ext_ft_rotmat,ft_data[0:3]),np.dot(ext_ft_rotmat,ft_data[3:6])])   
    # print(ft_data) 
    # print(ft_data_base)
    return ft_data_base
 
 
 
if __name__ == "__main__":
    q_in =Queue(1)
    q_out =Queue(2)
    global ft_data
    global rb5_data
    global cmd_jnt
    global cmd_pose
    robot = rtb.models.DH.RB5()
 
 
    host = RB5_ip
    data_port = 5001
    cmd_port = 5000
    ###### Initializing #######
    if len(sys.argv)>1:
        host = sys.argv[1]
 
    data_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    cmd_socket  = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
 
    try:
        data_socket.connect((host, data_port))
        cmd_socket.connect((host, cmd_port))
 
 
        print("start")
 
        ###### Start daemon thread to get External F/T sensor data & RB5 data  #######
        t1 = threading.Thread(target=get_data_thread, args=(data_socket, q_in, q_out))
        t1.daemon = True 
        t1.start()
        print("start_tread")
        initRobot()
        print("init")
        setRealMode()
        print("set_real_mode")
        q_in.put(1)
        rb5_data=q_out.get()
        get_robot_param()
        cmd_jnt = robot_jnt
        cmd_pose = robot_pose
        while True:
            time.sleep(step_time)
            q_in.put(1)
            rb5_data=q_out.get()
            get_robot_param()
            compute_admittance()
            # print(ft_data)
            # print(wrench_external_callback())
        print("### End ###")
 
 
 
    except KeyboardInterrupt:
        pass
    finally:
        os._exit(0) # not call close() ** workaround for native dead
 
    print("\nOutput:%s" % OUTFILE_NAME)
#eof

For questions, clarifications, etc, Email: kimb52@unlv.nevada.edu