====== RB5 Admittance Control ====== **Author:** Email: \\ **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 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 . =====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 |}} 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° │ └─────┴───────┴─────┴─────┴─────┴──────┴─────┘ ==== 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==== 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) # # change to 'real robot' mode -- robot will move def setRealMode(): sendCommand("pgmode real") time.sleep(1) # return *this; # # change to 'simulation' mode -- robot will not move except teaching def setSimulationMode(): sendCommand("pgmode simulation") time.sleep(1) # return *this; # // # // : 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() # // # // : 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 ====== Final Words ====== For questions, clarifications, etc, Email: