├── README.md ├── dy.py └── iiwa7_description.urdf /README.md: -------------------------------------------------------------------------------- 1 | # A Example for Calculating Robot Dynamics Using **Pinocchio** Library 2 | Developed by: Xinyang Tian. 3 | 4 | Platform: Linux(ubuntu18.04) + [Pinocchio](https://github.com/stack-of-tasks/pinocchio). 5 | 6 | In this work, i use **Pinocchio** to verify the dynamics of kuka iiwa7. This library has a lot of functions, such as forward/inverse dynamics and their analytical derivatives, jocobian and its analytical derivatives, etc. The main functions of this script are as follows: 7 | 1. Calculating inverse dynamics of the 7-DOF manipulator (kuka iiwa7 as example), include: 8 | - Total torque of each joint; 9 | - Gravity contribution ***G(q)***; 10 | - Inertia Matrix ***M*** of each joint; 11 | - Coriolis Matrix ***C*** of each joint. 12 | 2. Verify the anti-symmetric property of ***dM***/*dt* - 2* ***C*** 13 | 14 | ## Implementation 15 | 16 | First, you should install **Pinocchio** on your system，please follow the procedure described [here](https://stack-of-tasks.github.io/pinocchio/download.html). Then, put the **dy.py** file and **iiwa7_description.urdf** file in the same folder, run the script using *Python*, finally you can get the ***M***, ***C***, and ***G*** with your model. 17 | - - - 18 | **Note**: if you change the **.urdf** file with your model, you should also change the dimension of ***q***, ***qdot***, and ***qddot*** equal to your model' DOF at the same time. 19 | 20 | ## References 21 | [1] Carpentier, J., Saurel, G., Buondonno, G., Mirabel, J., Lamiraux, F., Stasse, O., and Mansard, N, "The Pinocchio C++ library: A fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives," in 2019 *IEEE/SICE International Symposium on System Integration (SII)*, pp. 614-619. 22 | [2] R. Featherstone, *Rigid Body Dynamics Algorithms*. Springer, 2008. 23 | -------------------------------------------------------------------------------- /dy.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | import pinocchio as pin, math 3 | import numpy as np 4 | from matplotlib import pyplot as plt 5 | # load the urdf file of your robot model 6 | model = pin.buildModelFromUrdf('iiwa7_description.urdf') 7 | print('model name: ' + model.name) 8 | # Create data required by the algorithms 9 | data = model.createData() 10 | NQ = model.nq 11 | NV = model.nv 12 | print('Dimension of the configuration vector representation: ' + str(NQ)) 13 | print('Dimension of the velocity: ' + str(NV)) 14 | # calculate the total mass of the model, put it in data.mass[0] and return it 15 | total_Mass = pin.computeTotalMass(model,data) 16 | print('Total mass of the model: ', data.mass[0]) 17 | # Generating joint position, angular velocity and angular acceleration using quintic polynomials 18 | # 1. Generating position q, initial: 0 deg, end: 60 deg, ouput:rad 19 | def mypoly_p(t): 20 | p = np.poly1d([12*60*math.pi/180/(2*3**5),-30*60*math.pi/180/(2*3**4),20*60*math.pi/180/(2*3**3),0,0,0]) 21 | return p(t) 22 | 23 | q = np.zeros((61, 7)) 24 | for i in range(0, 61): 25 | for j in range(7): 26 | q[i, j] = mypoly_p(0 + 3 / 60 * i) # 61*7 matrix, each column represents a sequence of joint 27 | 28 | 29 | # 2. Generating angular velocity qdot, initial: 0 rad/s, end: rad/s, ouput:rad/s 30 | def mypoly_v(t): 31 | p = np.poly1d([5*12*60*math.pi/180/(2*3**5),-4*30*60*math.pi/180/(2*3**4),3*20*60*math.pi/180/(2*3**3),0,0]) 32 | return p(t) 33 | 34 | qdot = np.zeros((61, 7)) 35 | for i in range(0, 61): 36 | for j in range(7): 37 | qdot[i, j] = mypoly_v(0 + 3 / 60 * i) # 61*7 matrix, each column represents a sequence of joint 38 | 39 | # 3. Generating angular acceleration qddot, initial: 0 rad/s^2, end: rad/s^2, ouput:rad/s^2 40 | def mypoly_a(t): 41 | p = np.poly1d([4*5*12*60*math.pi/180/(2*3**5),-3*4*30*60*math.pi/180/(2*3**4),2*3*20*60*math.pi/180/(2*3**3),0]) 42 | return p(t) 43 | 44 | qddot = np.zeros((61, 7)) 45 | for i in range(0, 61): 46 | for j in range(7): 47 | qddot[i, j] = mypoly_a(0 + 3 / 60 * i) # 61*7 matrix, each column represents a sequence of joint 48 | 49 | # Calculates the torque of each joint, return 1*7 vector 50 | Torque = np.zeros((61, 7)) 51 | for i in range(0,61): 52 | tau = pin.rnea(model,data,q[i],qdot[i],qddot[i]) # 1*7 vector 53 | Torque[i][:] = tau.T 54 | print('The ' + str(i) + 'th Torque is: ',i,tau.T) 55 | 56 | # Computes the generalized gravity contribution G(q), stored in data.g 57 | G_Torque = np.zeros((61, 7)) 58 | for i in range(0,61): 59 | G_Tau = pin.computeGeneralizedGravity(model,data,q[i]) 60 | G_Torque[i][:] = G_Tau 61 | print('The ' + str(i) + 'th G_Tau is: ', G_Tau) 62 | 63 | 64 | # Computes the upper triangular part of the joint space inertia matrix M, stored in data.M 65 | M_Matrix = np.zeros((61,7,7)) 66 | for i in range(0,61): 67 | M_Temp = pin.crba(model,data,q[i]) 68 | M_Matrix[i,:,:] = M_Temp 69 | print('The ' + str(i) + 'th M_Matrix is: ', M_Temp) 70 | 71 | #Computes the Coriolis Matrix C 72 | C_Matrix = np.zeros((61,7,7)) 73 | for i in range(0,61): 74 | C_Temp = pin.computeCoriolisMatrix(model,data,q[i], qdot[i]) 75 | C_Matrix[i,:,:] = C_Temp 76 | print('The ' + str(i) + 'th C_Matrix is: ', C_Temp) 77 | 78 | # Verify the anti-symmetric property of dM/dt - 2* C, take the fifth sequence as example 79 | M = pin.crba(model,data,q[5]) 80 | dt = 1e-8 81 | q_plus = pin.integrate(model,q[5],qdot[5]*dt) 82 | M_plus = pin.crba(model,data,q_plus) 83 | dM = (M_plus - M)/dt 84 | C = pin.computeCoriolisMatrix(model,data,q[5],qdot[5]) 85 | print('The ' + str(5) + 'th C_Matrix is: ', C) 86 | A = dM - 2*C 87 | print('A is: ', A) 88 | res = A + A.T 89 | print('res is: ', res) 90 | -------------------------------------------------------------------------------- /iiwa7_description.urdf: -------------------------------------------------------------------------------- 1 | 2 | 5 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 20 | 21 | 22 | 23 | 24 | 26 | 27 | 30 | 32 | 39 | 40 | 41 | 44 | 45 | 47 | 48 | 50 | 52 | 53 | 54 | 55 | 58 | 59 | 61 | 62 | 63 | 64 | 66 | 67 | 70 | 72 | 79 | 80 | 81 | 84 | 85 | 87 | 88 | 90 | 92 | 93 | 94 | 95 | 98 | 99 | 101 | 102 | 103 | 104 | 107 | 110 | 112 | 114 | 116 | 121 | 122 | 124 | 125 | 128 | 130 | 137 | 138 | 139 | 142 | 143 | 145 | 146 | 148 | 150 | 151 | 152 | 153 | 156 | 157 | 159 | 160 | 161 | 162 | 165 | 168 | 170 | 172 | 174 | 179 | 180 | 182 | 183 | 186 | 188 | 195 | 196 | 197 | 200 | 201 | 203 | 204 | 206 | 208 | 209 | 210 | 211 | 214 | 215 | 217 | 218 | 219 | 220 | 223 | 226 | 228 | 230 | 232 | 237 | 238 | 240 | 241 | 244 | 246 | 253 | 254 | 255 | 258 | 259 | 261 | 262 | 264 | 266 | 267 | 268 | 269 | 272 | 273 | 275 | 276 | 277 | 278 | 281 | 284 | 286 | 288 | 290 | 295 | 296 | 298 | 299 | 302 | 304 | 311 | 312 | 313 | 316 | 317 | 319 | 320 | 322 | 324 | 325 | 326 | 327 | 330 | 331 | 333 | 334 | 335 | 336 | 339 | 342 | 344 | 346 | 348 | 353 | 354 | 356 | 357 | 360 | 362 | 369 | 370 | 371 | 374 | 375 | 377 | 378 | 380 | 382 | 383 | 384 | 385 | 388 | 389 | 391 | 392 | 393 | 394 | 397 | 400 | 402 | 404 | 406 | 411 | 412 | 414 | 415 | 418 | 420 | 427 | 428 | 429 | 432 | 433 | 435 | 436 | 438 | 440 | 441 | 442 | 443 | 446 | 447 | 449 | 450 | 451 | 452 | 455 | 458 | 460 | 462 | 464 | 469 | 470 | 471 | --------------------------------------------------------------------------------