dvrk_dynamics_identification
Dynamic model identification of the DVRK
view repo
The da Vinci Research Kit (dVRK) is a teleoperated surgical robotic system. For dynamic simulations and modelbased control, the dynamic model of the dVRK with standard dynamic parameters is required. We developed a dynamic model identification package for the dVRK, capable of modeling the parallelograms, springs, counterweight, and tendon couplings, which are inherent to the dVRK. A convex optimizationbased method is used to identify the standard dynamic parameters of the dVRK subject to physically feasible constraints. The relative errors between the predicted and measured motor torque are calculated on independent test trajectories, which are less than 16.3 first three joints and 34.0 manipulator and patient side manipulator, respectively. We open source the identification software package. Although this software package is originally developed for the dVRK, it is easy to apply it on other robots with similar characteristics to the dVRK through simple configuration.
READ FULL TEXT VIEW PDFDynamic model identification of the DVRK
The da Vinci Research Kit (dVRK) is an opensource surgical robotic system whose mechanical components are obtained from the first generation of the da Vinci Surgical Robot®[1]. It has made the research on surgical robotics more accessible. To date, researchers from over 30 institutes around the world are using the physical dVRK [2], and some other researchers are using dVRK simulations [1].
Simulations and modelbased control require the dynamic model of the dVRK. Fontanelli et al. [3] has obtained the dynamic model of the dVRK using dynamic model identification techniques. However, the dynamic parameters obtained in [3] are base parameters (also called lumped parameters) [4], a minimum set of dynamic parameters that can sufficiently describe the dynamic model of a robot. Although base parameters are adequate to represent the dynamics of a robot in dynamic equations, standard parameters are required for the efficient recursive NewtonEulerbased dynamic algorithms. Towards this end, several dynamics libraries utilize standard parameters, such as Rigid Body Dynamics Library (RBDL) [5] and Kinematics and Dynamics Library (KDL) [6].
The dynamic parameters vary between different robots of the same make and model due to manufacturing and assembly variances. Furthermore, the assembly components of the robots are subject to deformation and wear & tear along their life cycle which can potentially alter the dynamic model. As such, the dynamic model identification is required before implementation of any robust modelbased control algorithm. This requirement drives the need for a robust opensource dynamic model identification package. There are existing software packages for the dynamic model identification of generic openchain manipulators such as SymPybotics
[7] and FloBaRoID [8]. However, these packages lack the capability of modeling closedloop kinematic chains, springs, counterweights, and tendon couplings, which are inherent to dVRK’s mechanical design. To address this, we utilize and extend the convex optimizationbased method proposed by Sousa and Cortesão [9] to obtain the physicallyfeasible standard dynamic parameters of the dVRK arms.This paper is structured into seven sections as the workflow of dynamic model identification in Fig. 1. Sections II and III explain the mathematical formulation of the kinematic and dynamic modeling of the Master Tool Manipulator (MTM) and Patient Side Manipulator (PSM). Section IV describes the trajectory optimization method to improve parameter identification quality. Section V presents the identification method to obtain the standard dynamic parameters with physical feasibility considered.The experimental results which validate the proposed approaches are presented in section V. Finally, the concluding arguments are entailed in section VII.
link  joint type  prev  succ  link inertia  motor inertia  friction  spring  
1  R  0  0  0  ✓  ✕  ✓  ✕  
2  R  1  3  0  0  ✓  ✕  ✓  ✕  
3  R  2  4  0  0  ✓  ✕  ✓  ✕  
R  1  0  0  ✓  ✕  ✓  ✕  
R  0  0  ✓  ✕  ✓  ✕  
4  R  3  5  ✓  ✕  ✓  ✕  
5  R  4  6  0  0  ✓  ✕  ✓  ✓  
6  R  5  7  0  0  ✓  ✕  ✓  ✕  
7  R  6    0  0  ✓  ✕  ✓  ✕  
R      0  0  0  ✕  ✓  ✓  ✕  

The dVRKROS package [1] employs the (DenavitHartenberg) DH convention based on kinematic frames located on the joint axes, whereas almost all the joints are actuated off axes using a combination of cams, links, or cables and pulleys. Consequently, we need to define additional coordinate axes to relate the joint motions  as defined in the dVRKROS package  with the motor torques.
To build the relationship between the robot joint motion in the dVRKROS package [1] and the torque of each motor, several types of joint coordinates are defined. is the joint coordinate used in the dVRKROS package. is the joint coordinate used in the kinematic modeling in this work, where is the basis joint coordinate which can adequately represent the kinematics of the robot, and is the additional joint coordinate which represents the joint coordinate of the passive joints in the parallel mechanism and can be represented by the linear combination of
. Since both the MTM and PSM have seven actuated degrees of freedom (DOF), the basis joint coordinate can be represented by
. is the equivalent motor coordinate which is considered to happen at joints, with the reduction ratio caused by gearboxes and tendons included for most motors unless explicitly specified. Finally, defines the complete joint coordinate. The relation between these joint coordinates is illustrated for both the MTM and PSM in this section.The left and right MTMs are identical to each other, except the last four joints being mirrored to each other. Consequently, the two MTMs can be modeled in a similar fashion. The frame definition is shown in Fig. 2, and the kinematic parameters of the MTM are described in Table I. The kinematics of the MTM can be described as
Joint 1 rotates around the Zaxis of the base frame, .
Joints 2, 3, , , and construct a parallelogram, which is actuated by joints 2 and .
Joints 4, 5, 6, and 7 form a 4axis nonlocking gimbal.
The kinematics of the MTM is fully described by the basis joint coordinates , which are equal to the dVRK joint coordinate , . The additional joints can be described as the linear combination of by
(1) 
Joints 1, 5, 6, and 7 are independently driven, and thus the motion of these joints is equivalent to their corresponding driven motors, = . The motion of depends on both and and can be described by
(2) 
where and are the radii of the pulleys shown in Fig. 3. Based on the user guide of the dVRK, mm and mm, and thus .
link 

prev  succ 


friction  spring  

1  R  0  2  0  0  ✓  ✕  ✓  ✕  
2  R  1  ,  0  0  ✓  ✕  ✓  ✕  
  2  ,  0  0  ✕  ✕  ✕  ✕  
R  , , 3  0  0  ✓  ✕  ✕  ✕  
R    0  0  ✓  ✕  ✕  ✕  
R  3  0  0  ✓  ✕  ✕  ✕  
R    0  0  ✓  ✕  ✕  ✕  
3  P  4  0  ✓  ✕  ✓  ✕  
P  2    0  ✓  ✕  ✕  ✕  
4  R  3  5  0  0  ✕  ✓  ✓  ✓  
5  R  4  6, 7  0  0  ✕  ✓  ✓  ✕  
6  R  5    0  ✕  ✕  ✓  ✕  
7  R  5    0  ✕  ✕  ✓  ✕  
R      0  0  0  ✕  ✓  ✓  ✕  
R      0  0  0  ✕  ✓  ✓  ✕  
R      0  0  0  ✕  ✕  ✓  ✕ 
The coupling between the dVRKROS joint coordinates and motor joint coordinates due to the parallelogram and tendon is resolved by the coupling matrix as
(3) 
where , based on the user guide of the dVRK.
The frame definition and kinematic dimensions of the PSM are shown in Fig. 4, and the corresponding parameters are shown in Table II. The kinematics of the PSM can be concluded as
The first two revolute joints form a remotecenterofmotion (RCM) point which remains fixed in Cartesian space. This RCM is achieved via a double fourbar linkage with six links actuated by a single motor.
The third joint is prismatic and provides insertion of the instrument through the RCM. The first three joints allow the 3DOF Cartesian space motion.
Revolute joints 4 and 5 construct the roll and pitch motion of the wrist to reorient the endeffector.
The last two joints construct the yaw motion of the endeffector, as well as the opening and closing of the gripper.
We model the first five joints of the PSM identical to the dVRKROS package, that is . The dVRKROS package models the last two joints as , the angle from the insertion direction to the bisector of the two jaw tips, and , the angle between the two jaw tips. However, the gripper jaws are designed and actuated as two seperate links which we consider in our model. As shown in Fig. (a)a, the relation between the dVRKROS joint coordinates , and the joint coordinates in our model is described by
(4) 
Since the first four joints are independently driven, the equivalent motor motion is considered to occur at joints and thus is the same as joint motion, . Based on the user guide of the dVRK, the coupling of the wrist joint actuation can be resolved by the coupling matrix mapping the motor joint coordinates to the dVRKROS joint coordinates by
(5) 
where .
In this section, the dynamic parameters are described first. The dynamic equation is then formulated based on EulerLagrange equation. Finally, the dynamic modeling of the MTM and PSM is introduced based on the formulation.
The formulation of the dynamic parameters is modified from [9] and tailored towards the mechanical design of the dVRK arms.
Each link is characterized by the mass , the center of mass (COM) relative to the link frame ,
, and the inertia tensor about the COM,
. To express the equations of motion as a linear form of dynamic parameters, we use the socalled barycentric parameters [10], in which the mass of linkis first used, followed by the first moment of inertia
.(6) 
Finally, the inertia tensor about frame is used [11]. is calculated via the parallel axis theorem
(7) 
where
is the skewsymmetric operator.
Besides the inertial parameters of link , the corresponding joint friction coefficients, motor inertia , and spring stiffness are grouped as additional parameters
(9) 
where and are the viscous and Coulomb friction constants, and is the Coulomb friction offset of joint .
Eventually, all the parameters of joints are grouped together as the dynamic parameters of the robot.
(10) 
The EulerLagrange equation for closedchain robots [12] is used to model the dynamics of the dVRK. The Lagrangian is calculated by the difference of the kinetic energy and potential energy of the robot, . Motor inertias and springs are not included in and are modeled separately.
The relation from motor motion to the torque of each motor caused by link inertia is then computed as
(11) 
The friction torques of all the joints are considered as
(12) 
where and are diagonal matrices encapsulating the viscous and Coulomb friction constants, and is the vector of the Coulomb friction offset constants corresponding to the joint coordinate .
The torques caused by motor inertia are defined as
(13) 
For spring , we only model the stiffness constant as its parameter, which results into the spring torques
(14) 
where is a diagonal matrix of the stiffness constants of the springs, and is their corresponding equivalent prolongation vector.
The joint torques caused by springs and frictions can be projected onto the motor joints, using the Jacobian matrix of their corresponding joint coordinate with respect to the motor joint angle [12]. Thus, the motor torques with link inertia, springs, frictions, motor inertia, and motion couplings considered are given by
(15) 
QR decomposition with pivoting [4] is used to calculate the base parameters. With this method, we get a permutation matrix , where is the number of standard dynamic parameters and is the number of base parameters. The base parameters and the corresponding regressor can be calculated by
(17) 
The dynamic modeling description for each link of the MTM is shown in Table I. All the nine links are modeled with link inertia. The frictions of all the modeling joints are considered, except joint since joint and joint share the same joint coordinate. For an independently driven joint, the friction from the joint and its driven motor is coupled together and thus impossible to distinguish from each other. All the motors except the one have their corresponding independently driven joints which have already been modeled with link inertia and joint friction. Therefore, only motor 4 is modeled with motor inertia and motor friction.
The electrical cable along joint 4 (Fig. (a)a) affects its joint torque significantly. The joint torque data of joint 4, and , is collected, with joint 4 rotating at rad/s and other joints being stationary, as shown in Fig. (b)b. We exclude the joint acceleration to record data at constant joint velocities, which explicitly removes any torque due to inertia. Moreover, due to the friction model in (12), the frictions with the joint velocity at rad/s should be opposite to each other if the Coulomb friction offset is not considered. Thus finally, if we computed the mean of and , the viscous friction and Coulomb friction terms will be canceled, and the joint friction offset and torque applied to the joint from the cable physically acting on it will be left.
To get , we first fit the joint torque data at rad/s using order polynomial functions of , respectively, as shown in Fig. (b)b. Next, the mean of the obtained coefficients of the two polynomials and is calculated as the coefficients of the polynomial that represents .
In addition, on joint 5 of the MTM, there is a spring to balance the gravitational force (Fig. (a)a). Due to the model of the spring shown in Fig. (b)b, the joint torque from the spring is given by
(18) 
where is the length between the two axes connecting the spring, which can be calculated using the law of sines as
(19) 
and mm by measurement is the value of when the spring is relaxed. Based on basic trigonometry, the moment arm can be calculated by
(20) 
where , and are constants shown in Fig. (b)b.
Thus, .
The dynamic modeling description of the PSM is shown in Table II. Inertia is considered for all the links contributing to the Cartesian motion, including the counterweight, link . The motor inertia of these joints is ignored since it is not significant compared to their link inertia. The inertia of the wrist and gripper links is minimal, and thus infeasible to identify. Therefore, we only model the inertia of motors for the wrist and gripper, corresponding to the motion of .
Since joints , , , , and are all driven by a single motor, their friction can be represented by the friction of one joint for simplicity. Thus, among these joints, only joint 2 is modeled with friction. Similarly, only joint 3 is modeled with friction out of joints 3 and . Because of the contact between links 5 and 6, and between links 5 and 7 as shown in Fig. (b)b, the frictions on joints 6 and 7 are modeled, corresponding to the motion of and . Moreover, the friction between link 6 and link 7 due to the contact between the two jaw tips is considered, corresponding to the motion of . Additionally, the frictions on the motor sides of the last four joints are also modeled, corresponding to the motor motion of .
The torsional spring on joint 4 which rotates the joint back to its home position is modeled as
(21) 
A periodic excitation trajectory based on Fourier series [13] is used to generate data for dynamic model identification. This trajectory minimizes the condition number of the regression matrix for the base parameters that decide the dynamic behavior of a robot.
(22) 
where is the motor joint coordinate at sampling points and is the sampling point number. The joint position of joint can be calculated by
(23) 
where is the angular component of the fundamental frequency , is the harmonic number of Fourier series, and are the amplitudes of the order sine and cosine functions of joint , is the position offset of motor joint , and is the time.
The motor joint velocity and acceleration can be calculated easily by the differentiation of . And the trajectory must satisfy the following constraints:
The joint position is between the lower bound and the upper bound , .
The robot is confined in its work space. The Cartesian position of frame is within its lower bound and upper bound , .
Finally, pyOpt [14] is used to solve this constrained nonlinear optimization problem.
To identify the dynamic parameters, we use the excitation trajectory described in Section IV to move the robot. Data is collected at each sampling time to obtain the regression matrix and the dependent variable vector :
(24) 
where is the sampling point number, and and are the motor joint position and torque at the th sampling point.
The identification problem can then be formulated into an optimization problem which minimizes the squared residual error with respect to the decision vector .
(25) 
To get more realistic dynamic parameters and avoid overfitting to the identification data, we utilized the physical feasibility constraints for the dynamic parameters:
The mass for each link is positive, .
The inertia matrix of each link is positive definite, [15]
, and its eigenvalues
, and should follow the socalled triangle inequality conditions [16], , , and .The COM of link , , is inside its convex hull, and , where and are the lower and upper bound of , respectively [9].
The viscous and Coulomb friction coefficients for each joint are positive, and .
The inertia of motor is positive, .
The stiffness of spring is positive, .
The first two constraints regarding the inertia properties of each link can be derived into an equivalent [17] as
(26) 
We can also add the lower and upper bounds to , , and when we have more knowledge about them.
Finally, we use the CVXPY package [18] with the SCS solver [19] to solve this convex optimization problem. With the identified barycentric parameters, the standard inertia parameters are computed by solving (6) and (7).
57  10  30  9  40  87  40  460  
29  60  30  39  195  180  38  450  
2.8  3.1  3.1  6.2  6.2  3.1  3.1  12.6  
2.8  3.1  3.1  6.2  6.2  3.1  3.1  12.6 
85  43  0.07  86  86  86  80  8  
85  46  0.235  86  86  86  80  172  
1.7  1.7  0.35  2  2  2  2   3  
1.7  1.7  0.35  2  2  2  2  3 
This section presents the experimental results of the dynamic model identification conducted on the dVRK arms.
Two independent excitation trajectories are generated for each of the MTM and PSM. One is for identification, and the other is for test. The harmonic number is set to 6. The fundamental frequency of the MTM and PSM are 0.1 Hz and 0.18 Hz, respectively. The joint position and velocity are constrained in the optimization, as in Table III and IV. Since links and of the PSM have similar motion and are very close to each other, it is hard to get a trajectory with a low condition number of the regression matrix when both links and are considered. Links 2 and have the similar problem. Therefore, the trajectory optimization of the PSM is based on the model without links and . The obtained optimal excitation trajectories for identification of the MTM and PSM are shown in Fig. 8, with the condition number of 211 and 362, respectively. The trajectories for test can be found in the opensource package and not shown here.
When the robot moves along these trajectories, the trajectory amplitudes and are increased gradually from zero to their nominal values in the first five seconds, which ensures the continuity of velocity and acceleration. The joint position, velocity, and torque are then collected at 200 Hz with the robots running in position control mode.
The joint position and velocity are collected directly, and the joint acceleration is obtained by the secondorder numerical differentiation of the velocity. A order lowpass Butterworth filter is used to filter all the data with the cutoff frequencies of 1.8 Hz for the MTM and 5.4 Hz for the PSM. To achieve zero phase delay, we apply this filter in both the forward and backward directions.
To get uniformly precise identification results for all joints, the residual error of each motor joint in (25) is weighted by . The identified dynamic parameters from identification trajectories are used to predict the motor joint torque on test trajectories, . The relative root mean squared error is used as the relative prediction error to assess the identification quality, .
MTM (%)  7.3  15.1  16.2  22.3  27.0  23.3  34.0 
PSM (%)  9.1  17.9  18.9  13.4  23.9  21.2  26.5 
Fig. 9 and 10 show the comparison of the measured motor torque and predicted motor torque on the test trajectories using the identified dynamic parameters for the MTM and PSM, respectively. The relative prediction error of each motor joint is shown in Table V.
The maximum relative prediction error of the MTM occurs on motor joint 7, which is . The relative prediction error of the first three motor joints is less than , which correspond to the Cartesian motion and most of the link inertia of the MTM. The large backlash caused by the gearboxes and the small link inertia of the last four joints make it hard to identify their dynamic parameters accurately. Hence, the relative prediction error of the last four motor joints is relatively higher. The overall identification performance of the MTM is better than [3], in which the largest relative prediction error is for all the joints and for the first three joints.
The maximum relative prediction error of the PSM occurs on motor joint 7, which is . The relative prediction error of the first three motor joints is less than , which correspond to the Cartesian motion and most of the link inertia of the arm. The relative prediction error of the last four motor joints is relatively larger since they are only modeled with motor inertia and frictions and the magnitudes of the joint torques are very small. The overall identification performance of the PSM is also better than [3], in which the largest relative prediction error is for all the joints and for the first three joints.
In this work, an opensource software package for the dynamic model identification of the full standard dynamic parameters of the dVRK is presented (https://github.com/WPIAIM/dvrk_dynamics_identification). Link inertia, joint friction, springs, tendon couplings, cable force, and closedchains are incorporated in the modeling. Fourier seriesbased trajectories are used to excite the dynamics of the dVRK, with the condition number of the regression matrix minimized. A convex optimizationbased method is used to get the full standard dynamic parameters subject to physical feasibility constraints, which are more suitable for the fast recursive NewtonEuler computation. The identification performance is improved significantly compared to previous work [3]. The package is written in Python under Jupyter Notebooks with free dependent software modules, which makes it easy to read and replicate. Although this software package is initially developed for the dVRK, it is easy to use it for the dynamic model identification of other robots.
Future work will be devoted to further reducing the error by precisely modeling the cables and gears, and applying the identified dynamic model in simulations and modelbased control.
This work has been supported by the National Science Initiative NSF grant: IIS1637759.
Journal of Machine Learning Research
, vol. 17, no. 83, pp. 1–5, 2016.
Comments
There are no comments yet.