-
Notifications
You must be signed in to change notification settings - Fork 0
Home
Written by Farid Tavakol
This example demonstrates the derivation of the dynamics model of a serial robot using the RBDL library and the implementation of a gravity compensation controller in the AMBF simulator. For this example, we will use the KUKA lbr model as our test case but the same procedure can be used for other serial manipulators. Also, the RBDL library can be used for closed chain manipulators such as the dVRK MTM and PSM but it requires additional steps when creating the robot’s description in the RBDL. It is worth mentioning that the RBDL library is originally written as a C++ library but is also available as a python library using a python wrapper. In this example, we will use the python wrapper for the RBDL. The RBDL library can be accessed using this link.
We will first start off by describing the robot's model in the RBDL and then proceed to the design of the gravity compensation controller in the AMBF. So, we would need two files, where in the first file we use RBDL to derive the dynamics of the robot, and in the second file, we implement the gravity compensation controller and visualize the robot in the AMBF.
For describing the robot's model in the RBDL, we use rbdl, numpy, and yaml libraries.
import numpy as np
import rbdl
import yaml RBDL needs the information regarding the center of mass, mass, parent distance, the inertia, and the rotation axis for each link of the robot in order to derive the dynamics model. All those information are accessible through the robot's description file (YAML). For deriving the inertia of each link simply use theget_inertia command to access the inertia values for each link.
# Create a body for a given mass, center of mass, and inertia
# mass of each link of the robot. Here we are considering value one for all of the links (these values should match the mass values in the YAML file of the robot.
mass_arr = [[1],[1],[1],[1],[1],[1],[1],[1]]
mass_arr = np.array(mass_arr)
# Get the distance vector between two adjacent bodies
# The location of the joints with respect to the previous link's frame. These values are directly taken from the YAML file in Joints>parent pivot.
parent_dist = [[0.0,0.0,0.0],[0.0,0.0,0.103],[0.0,0.013,0.209],[0.0,-0.194,-0.009],[0.0,-0.013,0.202],[-0.002,0.202,-0.008],[0.002,-0.052,0.204],[-0.003,-0.05,0.053]]
parent_dist = np.array(parent_dist)
# create the COM for the bodies
# These values can be found in the Body>inertia offset position. But be careful! In the YAML file, the center of mass of each link is described in the local coordinate frame. However, the orientation and possibly the position of that frame may be different than the frame in which your link's joint frame is described in the RBDL. So, you have to find and define the COM w.r.t the frame in which you described your model in the RBDL. The best way to do this is to open your model in the Blender software and compare your local frame with the RBDL frame and find the corresponding values for the COM.
com_pos = [[0.001,0,0.06],[0.0,-0.017,0.134],[0.0,-0.074,0.009],[0.0, 0.017, 0.134],[-0.001,0.081,0.008],[0.0,-0.017,0.129],[0.0,0.007,0.068],[0.006,0.0,0.015]]#
com_pos = np.array(com_pos)
# create the inertia for the bodies
inertia = [[0.01,0.01,0.01],[0.00815814, 0.007363868, 0.003293455],[0.00812252, 0.00329668, 0.00733904],[0.008159,0.007421,0.00330],[0.0081471,0.003297,0.0073715],[0.0077265,0.006950,0.00329],[0.002983,0.003299,0.003146],[0.000651,0.0006512,0.001120]] #
inertia = np.array(inertia) # 8x3
# Create a joint from joint type The revolute joint can be selected(X,Y,Z)
# can be selected to define the axis of rotation
joint_rot_z = rbdl.Joint.fromJointType ("JointTypeRevoluteZ")# for revoulute joints
joint_fixed= rbdl.Joint.fromJointType ("JointTypeFixed") #for the base
# Some double checking for the assigned values
# print 'mass', np.shape(mass_arr)
# print 'inertia', np.shape(inertia)
# print 'com_pos', np.shape(com_pos)
# print 'parent_dist', np.shape(parent_dist)Now we need to create the kinematic chain of the robot using the physical values that we just defined in the previous section.
model = rbdl.Model()
model.gravity=[0,0,-9.81] # defining the direction of the gravity in the z direction (the default direction is set in the negative Y direction)
# first we create the spacial transformation for each links of the robot.Note that the rotation matrix should be transposed.
trans = rbdl.SpatialTransform()
trans.E = np.eye(3)#np.array([[0.0, -1.0, 0.0],[1.0,0.0,0.0],[0.0,0.0,1.0]])
trans.r = parent_dist[0]
trans1 = rbdl.SpatialTransform()
trans1.E =np.eye(3)
trans1.r = parent_dist[1]
trans2 = rbdl.SpatialTransform()
trans2.E = np.array([[1.0, 0.0, 0.0],[0.0,0.0,-1.0],[0.0,+1.0,0.0]])
trans2.r = parent_dist[2]
trans3 = rbdl.SpatialTransform()
trans3.E =np.array([[1.0, 0.0, 0.0],[0.0,0.0,+1.0],[0.0,-1.0,0.0]])
trans3.r = parent_dist[3]
trans4 = rbdl.SpatialTransform()
trans4.E =np.array([[1.0, 0.0, 0.0],[0.0,0.0,+1.0],[0.0,-1.0,0.0]])
trans4.r = parent_dist[4]
trans5 = rbdl.SpatialTransform()
trans5.E =np.array([[1.0, 0.0, 0.0],[0.0,0.0,-1.0],[0.0,+1.0,0.0]])
trans5.r = parent_dist[5]
trans6 = rbdl.SpatialTransform()
trans6.E =np.array([[1.0, 0.0, 0.0],[0.0,0.0,-1.0],[0.0,+1.0,0.0]])
trans6.r = parent_dist[6]
trans7 = rbdl.SpatialTransform()
trans7.E =np.array([[1.0, 0.0, 0.0],[0.0,0.0,+1.0],[0.0,-1.0,0.0]])
trans7.r = parent_dist[7]
# Using principal inertia values from yaml file
I_x = inertia[0][0]
I_y = inertia[0][1]
I_z = inertia[0][2]
I1_x = inertia[1][0]
I1_y = inertia[1][1]
I1_z = inertia[1][2]
I2_x = inertia[2][0]
I2_y = inertia[2][1]
I2_z = inertia[2][2]
I3_x = inertia[3][0]
I3_y = inertia[3][1]
I3_z = inertia[3][2]
I4_x = inertia[4][0]
I4_y = inertia[4][1]
I4_z = inertia[4][2]
I5_x = inertia[5][0]
I5_y = inertia[5][1]
I5_z = inertia[5][2]
I6_x = inertia[6][0]
I6_y = inertia[6][1]
I6_z = inertia[6][2]
I7_x = inertia[7][0]
I7_y = inertia[7][1]
I7_z = inertia[7][2]
# Creating the inertia Matrix
inertia_matrix=[[I_x, 0, 0], [0, I_y, 0], [0, 0, I_z]],[[I1_x, 0, 0], [0, I1_y, 0], [0, 0, I1_z]],[[I2_x, 0, 0], [0, I2_y, 0], [0, 0, I2_z]],[[I3_x, 0, 0], [0, I3_y, 0], [0, 0, I3_z]],[[I4_x, 0, 0], [0, I4_y, 0], [0, 0, I4_z]],[[I5_x, 0, 0], [0, I5_y, 0], [0, 0, I5_z]],[[I6_x, 0, 0], [0, I6_y, 0], [0, 0, I6_z]],[[I7_x, 0, 0], [0, I7_y, 0], [0, 0, I7_z]]
# print 'inertia', inertia_matrix
# print 'inertia', inertia_matrix[0][0]
inertia_matrix = np.array(inertia_matrix)
# Creating each body of the robot
body = rbdl.Body.fromMassComInertia(mass_arr[0], com_pos[0], inertia_matrix[0])
body1 = rbdl.Body.fromMassComInertia(mass_arr[1], com_pos[1], inertia_matrix[1])
body2 = rbdl.Body.fromMassComInertia(mass_arr[2], com_pos[2], inertia_matrix[2])
body3 = rbdl.Body.fromMassComInertia(mass_arr[3], com_pos[3], inertia_matrix[3])
body4 = rbdl.Body.fromMassComInertia(mass_arr[4], com_pos[4], inertia_matrix[4])
body5 = rbdl.Body.fromMassComInertia(mass_arr[5], com_pos[5], inertia_matrix[5])
body6 = rbdl.Body.fromMassComInertia(mass_arr[6], com_pos[6], inertia_matrix[6])
body7 = rbdl.Body.fromMassComInertia(mass_arr[7], com_pos[7], inertia_matrix[7])
# Adding body to the model to create the complete robot
body_1 = model.AppendBody(trans, joint_fixed, body)
body_2 = model.AppendBody(trans1, joint_rot_z, body1)
body_3 = model.AppendBody(trans2, joint_rot_z, body2)
body_4 = model.AppendBody(trans3, joint_rot_z, body3)
body_5 = model.AppendBody(trans4, joint_rot_z, body4)
body_6 = model.AppendBody(trans5, joint_rot_z, body5)
body_7 = model.AppendBody(trans6, joint_rot_z, body6)
body_8 = model.AppendBody(trans7, joint_rot_z, body7)So far we have successfully built our robot's model in the RBDL. Now in order to be able to implement the gravity compensation controller, we need to use the inverse dynamics equation, which takes the joints position, velocity, and acceleration as input and passes the torque values for each joint of the robot. We then will feed the resulting torque back to the robot's joints to compensate for the gravity forces. Note that in the gravity compensation controller the values for the velocity and acceleration are put equal to zero since the robot is stationary and only the values for the position change. Thus, we put the qdot and qddot equal to zero.
now we define a function that calculates the torque values for each joint of the robot.
def get_G(q_):
q_ = np.asarray(q_)
q = np.zeros(7)
q[0]=q_[0]
q[1]=q_[1]
q[2]=q_[2]
q[3]=q_[3]
q[4]=q_[4]
q[5]=q_[5]
q[6]=q_[6]
qdot = np.zeros(7)
qddot = np.zeros(7)
tau = np.zeros(7)
# RBDL inverse dynamics function
rbdl.InverseDynamics(model, q, qdot, qddot, tau)
return tauThis marks the end of our first file. To summerize what we have done so far:
- Create the robot's model in the RBDL library using the values in the YAML file
- Create the inverse dynamics function to return torque values Now we will proceed to the second part of the tutorial that is the design of a gravity compensation controller for the KUKA lbr arm in the AMBF environment.
The libraries that are used in this part are as follows:
import rospy
import std_msgs.msg
from ambf_msgs.msg import ObjectState, ObjectCmd
from <Name_of_your_RBDL_model_file> import *
from geometry_msgs.msg import Vector3We proceed with the main code.
state_msg = ObjectState()
active = True
pos = []
# ROS Subscriber callback function
def get_joint_values(data):
global state_msg, active, pos
pos = data.joint_positions
pos=np.asarray(pos)
return pos
# main function
def main():
sub = rospy.Subscriber('/ambf/env/base/State', ObjectState, get_joint_values, queue_size=1)
pub = rospy.Publisher('/ambf/env/base/Command', ObjectCmd, queue_size=1)
rospy.init_node('kuka_gravity_compensation')
rate = rospy.Rate(1000) #1000hz
cmd_msg = ObjectCmd()
cmd_msg.enable_position_controller = False
cmd_msg.position_controller_mask = [False]
while not rospy.is_shutdown():
G = get_G(pos)
tau= G
#define header
Header = std_msgs.msg.Header()
Header.stamp = rospy.Time.now()
cmd_msg.header = Header
cmd_msg.joint_cmds = [ tau[0], tau[1],tau[2],tau[3],tau[4],tau[5],tau[6]]
pub.publish(cmd_msg)
rate.sleep()
# rospy.spin()
if __name__ == '__main__':
try:
main()
except rospy.ROSInterruptException:
passSo, we have successfully created the gravity compensation controller for the KUKA lbr arm. To test your controller, you could interact with the robot in using the mouse or any other haptic deivces and the robot should remain stationary in any arbitrary position that you move it to. Below are some screenshots of the robot put in some arbitrary positions while being controlled by the gravity compensation controller.