-
Notifications
You must be signed in to change notification settings - Fork 1
Dynamics Parameters
This Wiki page documents the dynamics parameters for dVRK and RAVEN-II surgical robot systems.
The dynamics parameters for the dVRK arms presented below are based on the work titled: A Convex Optimization-based Dynamic Model Identification Package for the da Vinci Research Kit. The code repository can be accessed using the following link: https://github.com/WPI-AIM/dvrk_dynamics_identification
Friction:
Coulomb friction(Fc) = 0.050003
Viscous friction(Fv) = 0.0905326
Offset friction(Fo) = −0.3000027
mass(kg): 6.0667823
Center of Mass(m): [0.0006075, 0.0, 0.0121153]
Moments of inertia (kg.m2) taken at the center of mass and aligned with the output coordinate system:
Ixx: 1.3183907 | Ixy: 0.0 | Ixz: 0.0 | Iyy: 3.73⋅10−5 | Iyz: 0.0 | Izz: 1.3183903
Friction:
Coulomb friction(Fc) = 0.0668202
Viscous friction(Fv) = 0.2180617
Offset friction(Fo) = 0.1419199
mass(kg): 10.0001371
Center of Mass(m): [0.0110856, 0.0195141, −0.0721861]
Moments of inertia (kg.m2) taken at the center of mass and aligned with the output coordinate system:
Ixx: 0.0044209 | Ixy: 0.0090817 | Ixz: −0.0040484 | Iyy: 0.0235898 | Iyz: 0.0015899 | Izz: 0.0265699
Friction:
Coulomb friction(Fc) = 0.0
Viscous friction(Fv) = 0.0
Offset friction(Fo) = 0.0
mass(kg): 2.956747
Center of Mass(m): [−0.0173905, 0.1752131, −0.0199989]
Moments of inertia (kg.m2) taken at the center of mass and aligned with the output coordinate system:
Ixx: 90.0948873 | Ixy: 2.4⋅10−5 | Ixz: 4⋅10−7 | Iyy: 3.61⋅10−5 | Iyz: 2.87⋅10−5 | Izz: 90.0948874
Friction:
Coulomb friction(Fc) = 0.0
Viscous friction(Fv) = 0.0
Offset friction(Fo) = 0.0
mass(kg): 0.4796773
Center of Mass(m): [−0.0299994, 0.2978392, 0.030008]
Moments of inertia (kg.m2) taken at the center of mass and aligned with the output coordinate system:
Ixx: 37.04596 | Ixy: 1.0⋅10−7 | Ixz: 0.0 | Iyy: 3.69⋅10−5 | Iyz: -2.0⋅10−7 | Izz: 37.04596
Friction:
Coulomb friction(Fc) = 0.0
Viscous friction(Fv) = 0.0
Offset friction(Fo) = 0.0
mass(kg): 0.1000014
Center of Mass(m): [0.0293552, −7.0⋅10−7, −0.0500018]
Moments of inertia (kg.m2) taken at the center of mass and aligned with the output coordinate system:
Ixx: 6.94⋅10−5 | Ixy: −3.59⋅10−5 | Ixz: −1.2⋅10−5 | Iyy: 2.56⋅10−5 | Iyz: -2.07⋅10−7 | Izz: 8.04⋅10−5
Friction:
Coulomb friction(Fc) = 0.0
Viscous friction(Fv) = 0.0
Offset friction(Fo) = 0.0
mass(kg): 0.5000069
Center of Mass(m): [-0.02, 0.0455733, −0.1400001]
Moments of inertia (kg.m2) taken at the center of mass and aligned with the output coordinate system:
Ixx: 6.96⋅10−5 | Ixy: −3.58⋅10−5 | Ixz: −1.19⋅10−5 | Iyy: 2.61⋅10−5 | Iyz: -2.06⋅10−7 | Izz: 8.04⋅10−5
Friction:
Coulomb friction(Fc) = 0.4475953
Viscous friction(Fv) = 1.2035645
Offset friction(Fo) = 0.0038872
mass(kg): 0.3630268
Center of Mass(m): [−0.0199936, −0.0199883, 0.0995037]
Moments of inertia (kg.m2) taken at the center of mass and aligned with the output coordinate system:
Ixx: 6.9⋅10−5 | Ixy: −3.58⋅10−5 | Ixz: −1.2⋅10−5 | Iyy: 2.54⋅10−5 | Iyz: -2.07⋅10−7 | Izz: 8.02⋅10−5
Friction:
Coulomb friction(Fc) = 0.4475953
Viscous friction(Fv) = 1.2035645
Offset friction(Fo) = 0.0038872
mass(kg): 0.3499395
Center of Mass(m): [−0.018457, −0.0260167, −0.0994826]
Moments of inertia (kg.m2) taken at the center of mass and aligned with the output coordinate system:
Ixx: 6.97⋅10−5 | Ixy: −3.6⋅10−5 | Ixz: −1.23⋅10−5 | Iyy: 2.58⋅10−5 | Iyz: -2.11⋅10−7 | Izz: 8.07⋅10−5
Friction:
Coulomb friction(Fc) = 0.048851
Viscous friction(Fv) = 0.0490775
Offset friction(Fo) = −0.059371
mass(kg): 7.9205524
Center of Mass(m): [0.0, 0.0, 0.1475298]
Moments of inertia (kg.m2) taken at the center of mass and aligned with the output coordinate system:
Ixx: 20.0328015 | Ixy: 0.0 | Ixz: 0.0 | Iyy: 20.0328015 | Iyz: 0.0 | Izz: -8.0⋅10-7
Friction:
Coulomb friction(Fc) = 0.0317147
Viscous friction(Fv) = 0.0925067
Offset friction(Fo) = −0.3000007
mass(kg): 10.9839011
Center of Mass(m): [−0.0037064, −0.0011631, 0.0282467]
Moments of inertia (kg.m2) taken at the center of mass and aligned with the output coordinate system:
Ixx: 0.0191505 | Ixy: −0.0029393 | Ixz: −0.0003536 | Iyy: 0.0006272 | Iyz: −0.0022588 | Izz: 0.0192754
Friction:
Coulomb friction(Fc) = 0.0018543
Viscous friction(Fv) = 0.0091921
Offset friction(Fo) = 0.0481495
mass(kg): 0.8122158
Center of Mass(m): [−0.1397595, −0.0300011, −0.030025]
Moments of inertia (kg.m2) taken at the center of mass and aligned with the output coordinate system:
Ixx: 0.0071813 | Ixy: −0.0006232 | Ixz: 0.0117288 | Iyy: 0.0261615 | Iyz: 0.0003851 | Izz: 0.019021
Friction:
Coulomb friction(Fc) = 0.0432045
Viscous friction(Fv) = 0.0619387
Offset friction(Fo) = −0.3000007
mass(kg): 10.0000001
Center of Mass(m): [0.0301125, −0.0033478, 0.0011687]
Moments of inertia (kg.m2) taken at the center of mass and aligned with the output coordinate system:
Ixx: 0.0026841 | Ixy: −0.0002349 | Ixz: 0.004443 | Iyy: 0.0098409 | Iyz: 0.0001459 | Izz: 0.0071718
Friction:
Coulomb friction(Fc) = 0.0018543
Viscous friction(Fv) = 0.0091921
Offset friction(Fo) = −0.0481495
mass(kg): 0.9964826
Center of Mass(m): [−0.0200044, 0.0199996, −0.0499997]
Moments of inertia (kg.m2) taken at the center of mass and aligned with the output coordinate system:
Ixx: 0.0191021 | Ixy: −0.0029318 | Ixz: −0.0003526 | Iyy: 0.0006262 | Iyz: −0.002253 | Izz: 0.0192267
Friction:
Coulomb friction(Fc) = 0.0287844
Viscous friction(Fv) = 0.0048418
Offset friction(Fo) = −0.1492096
mass(kg): 0.2077769
Center of Mass(m): [0.0006989, −0.0673588, −0.1500229]
Moments of inertia (kg.m2) taken at the center of mass and aligned with the output coordinate system:
Ixx: 0.003407 | Ixy: −3.49⋅10−5 | Ixz: 0.000484 | Iyy: 0.0032076 | Iyz: 0.0011393 | Izz: −0.0002048
Friction:
Coulomb friction(Fc) = 0.0069851
Viscous friction(Fv) = 0.0006978
Offset friction(Fo) = −0.0027382
mass(kg): 0.0659998
Center of Mass(m): [−0.0076644, −0.099978, −0.0924839]
Moments of inertia (kg.m2) taken at the center of mass and aligned with the output coordinate system:
Ixx: −0.0005099 | Ixy: 0.0001881 | Ixz: 0.0001365 | Iyy: −0.0003694 | Iyz: 6.68⋅10−5 | Izz: −0.000479
Friction:
Coulomb friction(Fc) = 0.0018971
Viscous friction(Fv) = 0.0022789
Offset friction(Fo) = 0.0045929
mass(kg): 0.0499978
Center of Mass(m): [0.0636551, 0.0028931, −0.0732851]
Moments of inertia (kg.m2) taken at the center of mass and aligned with the output coordinate system:
Ixx: −0.0001555 | Ixy: −5.47⋅10−5 | Ixz: −0.000104 | Iyy: −0.0002185 | Iyz: −9.8⋅10−6 | Izz: 1.17⋅10−5
Friction:
Coulomb friction(Fc) = 0.0018971
Viscous friction(Fv) = 0.0022789
Offset friction(Fo) = 0.0045929
mass(kg): 0.0499978
Center of Mass(m): [0.0636551, 0.0028931, −0.0732851]
Moments of inertia (kg.m2) taken at the center of mass and aligned with the output coordinate system:
Ixx: −1.3⋅10-6 | Ixy: 8.4⋅10−6 | Ixz: −6.4⋅10-6 | Iyy: −5.5⋅10-5 | Iyz: −8.5⋅10−6 | Izz: -5.89⋅10−5
This is the RAVEN-II surgical robot system. Click here to access the RAVEN source code GitHub page. For more information, visit the University of Washington Biotobotics Laboratory Webpage here and the Applied Dexterity company page here.
These are three links of the RAVEN-II robot system. The dynamics parameters for each of them are described below.
- Coordinate system: Frame1
- Length = 0.8 (meters)
- Actual mass = 503.07 (grams)
- Calculated mass = 252.75 (grams)
- Volume = 211338.69 (cubic millimeters)
- Surface area = 179575.08 (square millimeters)
Principal axes of inertia (I) and principal moments of inertia (P) taken at the center of mass (C):
dimension | principal axes of inertia (I) | principal moments of inertia (P) | center of mass (C) |
---|---|---|---|
x | (-0.01, -0.78, -0.62) | 209633.75 | 6.91 |
y | (-0.04, -0.62, 0.78) | 1236881.98 | 98.69 |
z | (-1.00, 0.03, -0.02) | 1397685.57 | -137.55 |
(units) | (millimeters) | (grams * square millimeters) | (millimeters) |
Moments of inertia (L) taken at the center of mass and aligned with the output coordinate system:
dimension | x | y | z |
---|---|---|---|
x | Lxx = 1397316.09 | Lxy = 14485.94 | Lxz = 4164.24 |
y | Lyx = 14485.94 | Lyy = 607489.68 | Lyz = 500386.40 |
z | Lzx = 4164.24 | Lzy = 500386.40 | Lzz = 839395.53 |
(units) | (grams * square millimeters) |
Inertia matrix (I) taken at the output coordinate system. Note that Ixx, Iyy, and Izz are the moments of inertia with respect to the x, y and z axis, which are always positive. The quantities Ixy, Ixz, Iyx, Iyz, Izx and Izy are products of inertia. They can be positive, negative, or zero. (Refer to page 2 of [5] for details)
dimension | x | y | z |
---|---|---|---|
x | Ixx = 8641352.61 | Ixy = 186854.01 | Ixz = -236086.43 |
y | Iyx = 186854.01 | Iyy = 5401935.31 | Iyz = -2930733.77 |
z | Izx = -236086.43 | Izy = -2930733.77 | Izz = 3313125.21 |
(units) | (grams * square millimeters) |
Below are additional parameters with variable names consistent with the dynamics equations in [4].
# Jm = 1e-3
# tr = 135.6566
# j_S_j = [5.51, -95.98, -137.81]'*0.001
# Tens0 = 10
# The Jacobian Matrix J that projects external force to the reference coordinates:
# |dimension |x |y |z |
# |----------|------------------|------------------|-----------------|
# |x |Jxx = 17376080 |Jxy = 291149 |Jxz = -374467 |
# |y |Jyx = 291149 |Jyy = 11000340 |Jyz = -548255 |
# |z |Jzx = -374467 |Jzy = -5482855 |Jzz = 6498903 |
# |(units) |(grams * square millimeters *(1e-7)) | |
# Homing angle: q = pi/6
# Rotor inertia: 0.00001 (N.m)
# Cable stiffness: ke = 2.2e4 (N/m)
# Damping parameters: be = 9.5 (N.s/m)
# Capstan radius of motor: rm = 6e-3 (m)
# Capstan radius of link: rl = (62.5+(1.19/2))*1e-3 (m)
# Coulomb friction coefficient for link: Fc = 0.01
# Viscous friction coefficient for link: Fv = 2.2e-3
# Coulomb friction coefficient for motor: Fmc = 9e-4
# Viscous friction coefficient for motor: Fmv = 5e-4
# Stribeck coefficient: stribeck_coeff = 100
# Joint limit max: qmax = pi/2 (rad)
# Joint limit min: qmin = 0 (rad)
# Link start Cartesian position: start = [0, 0, -1] (m)
# Link end Cartesian position: end = [0, -cos(15*pi/180), -sin(15*pi/180)] (m)
# Is prismatic? No.
One of the components requires updating. This may cause the mass properties calculation to be out of date
- Coordinate system: Frame2
- Actual mass = 750.34 (grams)
- Calculated mass = 650.18 (grams)
- Volume = 248900.36 (cubic millimeters)
- Surface area = 198525.62 (square millimeters)
Principal axes of inertia (I) and principal moments of inertia (P) taken at the center of mass (C):
dimension | principal axes of inertia (I) | principal moments of inertia (P) | center of mass (C) |
---|---|---|---|
x | (-0.04, -0.87, -0.50) | 1026255.53 | -1.20 |
y | (-0.07, 0.50, -0.86) | 18427216.38 | 179.09 |
z | (1.00, 0.00, -0.08) | 19114564.30 | -233.88 |
(units) | (millimeters) | (grams * square millimeters) | (millimeters) |
Moments of inertia (L) taken at the center of mass and aligned with the output coordinate system:
dimension | x | y | z |
---|---|---|---|
x | Lxx = 19084791.25 | Lxy = 578379.57 | Lxz = 387081.98 |
y | Lyx = 578379.57 | Lyy = 5401187.68 | Lyz = 7526849.90 |
z | Lzx = 387081.98 | Lzy = 7526849.90 | Lzz = 14082057.28 |
(units) | (grams * square millimeters) |
Moments of inertia (I) taken at the output coordinate system:
dimension | x | y | z |
---|---|---|---|
x | Ixx = 75504153.00 | Ixy = 717927.88 | Ixz = 569320.40 |
y | Iyx = 717927.88 | Iyy = 40967267.11 | Iyz = 34760711.79 |
z | Izx = 569320.40 | Izy = 34760711.79 | Izz = 34937207.20 |
(units) | (grams * square millimeters) |
Below are additional parameters with variable names consistent with the dynamics equations in [4].
# Jm = 1e-3
# tr = 121.5221
# j_S_j = [-2.34, -174, -226]'*0.001
# Tens0 = 10
# The Jacobian Matrix J that projects external force to the reference coordinates:
# |dimension |x |y |z |
# |----------|------------------|------------------|-----------------|
# |x |Jxx = 81578955 |Jxy = 772920 |Jxz = -762100 |
# |y |Jyx = 772920 |Jyy = 44329717 |Jyz = 37973327 |
# |z |Jzx = 7620100 |Jzy = 37973327 |Jzz = 37686468 |
# |(units) |(grams * square millimeters *(1e-7)) | |
# Homing angle: q = pi/2
# Rotor inertia: 0.00001 (N.m)
# Cable stiffness: ke = 2.2e4 (N/m)
# Damping parameters: be = 9.5 (N.s/m)
# Capstan radius of motor: rm = 6e-3 (m)
# Capstan radius of link: rl = (40.85+(1.19/2))*1e-3 (m)
# Coulomb friction coefficient for link: Fc = 0.01
# Viscous friction coefficient for link: Fv = 2.2e-3
# Coulomb friction coefficient for motor: Fmc = 9e-4
# Viscous friction coefficient for motor: Fmv = 5e-4
# Stribeck coefficient: stribeck_coeff = 100
# Joint limit max: qmax = 2.35619 (rad)
# Joint limit min: qmin = 0.78539 (rad)
# Link start Cartesian position: start = [0, 0, -1] (m)
# Link end Cartesian position: end = [0.7, 0, -1] (m)
# Is prismatic? No.
- Coordinate system: Frame3
- Actual mass = 406.58 (grams)
- Center of mass = (20.01, 0.19, 12.97) mm
Moments of inertia (I) taken at the output coordinate system. Again, refer to page 2 in [5] for explanations about the negative values for the off diagonal terms.
dimension | x | y | z |
---|---|---|---|
x | Ixx = 2908131 | Ixy = -1085 | Ixz = -66027 |
y | Iyx = -1085 | Iyy = 3009048 | Iyz = -8871 |
z | Izx = -66027 | Izy = -8871 | Izz = 355197 |
(units) | (grams * square millimeters) |
Below are additional parameters with variable names consistent with the dynamics equations in [4].
# Jm = 1e-3
# tr = 2158.6
# j_S_j = [-20.01,0.19,-12.97]'*0.001
# Tens0 = 10
# The Jacobian Matrix J that projects external force to the reference coordinates:
# |dimension |x |y |z |
# |----------|------------------|------------------|-----------------|
# |x |Jxx = 2908131 |Jxy = -1085 |Jxz = -66027 |
# |y |Jyx = -1085 |Jyy = 3009048 |Jyz = -8871 |
# |z |Jzx = -66027 |Jzy = -8871 |Jzz = 355197 |
# |(units) |(grams * square millimeters *(1e-7)) | |
# Homing angle: q = -0.15
# Rotor inertia: 0.00001 (N.m)
# Cable stiffness: ke = 2.2e4 (N/m)
# Damping parameters: be = 9.5 (N.s/m)
# Capstan radius of motor: rm = 4.7e-3 (m)
# Capstan radius of link: rl = 4.7e-3 (m)
# Coulomb friction coefficient for link: Fc = 0.01
# Viscous friction coefficient for link: Fv = 2.2e-3
# Coulomb friction coefficient for motor: Fmc = 9e-4
# Viscous friction coefficient for motor: Fmv = 5e-4
# Stribeck coefficient: stribeck_coeff = 0
# Joint limit max: qmax = 0.01 (rad)
# Joint limit min: qmin = -0.23 (rad)
# Link start Cartesian position: start = [0, 0, 0] (m)
# Link end Cartesian position: end = [0, 0, -0.6] (m)
# Is prismatic? Yes.
The information above is the original RAVEN-II dynamics parameter dataset. Below are research applications that extend from the dataset and provide further insights into the RAVEN-II dynamics.
[1] Increasing Precision of the Raven-II Surgical Robot by Applying Cascade Control: Kim Schwaner et al. (link here)
[2] AMBF simulator - raven blender model: Adnan Munawar et al. (link here)
[3] raven2_sim - a RAVEN-II simulator with no robotic software dependencies: Homa Alemzadeh et al.
[4] Dynamic modeling of cable driven elongated surgical instruments for sensorless grip force estimation: Yangming Li et al. (link here)
[5] 3D Rigid Body Dynamics: The Inertia Tensor (link here)
- Melody Su (yhsu83@uw.edu, msu@mtholyoke.edu)
- Andrew Lewis (alewi@uw.edu)
- Yangming Li (Yangming.Li@rit.edu)