DASL Wiki

This page is read only. You can view the source, but not change it. Ask your administrator if you think this is wrong.
====== RB5 Admittance Control ======
**Author:** <Baekseok Kim> Email: <kimb52@unlv.nevada.edu> <!-- replace with your email address -->
\\
**Date:** Last modified on <02/14/23>
\\
**Keywords:** RB5 manipulator, RB5 Programming, collaborative robot, Admittance Control
\\
{{ youtube>UM-zWlZEces?medium  }}

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.

===== Prerequisites =====
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

===== Python Robotics Tool Box =====

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
====Getting going====

You will need Python >= 3.6

Using pip
Install a snapshot from PyPI

<code python>
pip3 install roboticstoolbox-python
</code>

Available options are:

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

<code python>
pip3 install roboticstoolbox-python[optionlist]
</code>

From GitHub
To install the bleeding-edge version from GitHub

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


=====Configure RB5 Kinematic Parameter in Python Robotics Tool Box=====
Download the RB5 setup file and transfer it to the robotics toolbox folder.

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

{{ :bskim:rb5_tutorial:rb5.zip |}}

<code python>
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())
</code>




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

<code python>
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° │  
└─────┴───────┴─────┴─────┴─────┴──────┴─────┘  
</code>


==== Exercise ====
* 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.



=====Admittance Control Algorithm=====
{{ :bskim:rb5_tutorial:compliance_control_basic.pdf | Compliance Control}}

====Sample Code====
<code python>

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

</code>
====== Final Words ======

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