Dynamic model identification of the DVRK
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 optimization-based 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 PDF
Dynamic model identification of the DVRK
The da Vinci Research Kit (dVRK) is an open-source surgical robotic system whose mechanical components are obtained from the first generation of the da Vinci Surgical Robot®. 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 , and some other researchers are using dVRK simulations .
Simulations and model-based control require the dynamic model of the dVRK. Fontanelli et al.  has obtained the dynamic model of the dVRK using dynamic model identification techniques. However, the dynamic parameters obtained in  are base parameters (also called lumped parameters) , 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 Newton-Euler-based dynamic algorithms. Towards this end, several dynamics libraries utilize standard parameters, such as Rigid Body Dynamics Library (RBDL)  and Kinematics and Dynamics Library (KDL) .
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 model-based control algorithm. This requirement drives the need for a robust open-source dynamic model identification package. There are existing software packages for the dynamic model identification of generic open-chain manipulators such as SymPybotics and FloBaRoID . However, these packages lack the capability of modeling closed-loop kinematic chains, springs, counterweights, and tendon couplings, which are inherent to dVRK’s mechanical design. To address this, we utilize and extend the convex optimization-based method proposed by Sousa and Cortesão  to obtain the physically-feasible 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|
The dVRK-ROS package  employs the (Denavit-Hartenberg) 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 dVRK-ROS package - with the motor torques.
To build the relationship between the robot joint motion in the dVRK-ROS package  and the torque of each motor, several types of joint coordinates are defined. is the joint coordinate used in the dVRK-ROS 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 Z-axis 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 4-axis non-locking 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
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
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 .
|R||, , 3||0||0||✓||✕||✕||✕|
The coupling between the dVRK-ROS joint coordinates and motor joint coordinates due to the parallelogram and tendon is resolved by the coupling matrix as
where , based on the user guide of the dVRK.
The first two revolute joints form a remote-center-of-motion (RCM) point which remains fixed in Cartesian space. This RCM is achieved via a double four-bar 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 3-DOF Cartesian space motion.
Revolute joints 4 and 5 construct the roll and pitch motion of the wrist to reorient the end-effector.
The last two joints construct the yaw motion of the end-effector, as well as the opening and closing of the gripper.
We model the first five joints of the PSM identical to the dVRK-ROS package, that is . The dVRK-ROS 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 dVRK-ROS joint coordinates , and the joint coordinates in our model is described by
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 dVRK-ROS joint coordinates by
In this section, the dynamic parameters are described first. The dynamic equation is then formulated based on Euler-Lagrange 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  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 so-called barycentric parameters , in which the mass of link
is first used, followed by the first moment of inertia.
Finally, the inertia tensor about frame is used . is calculated via the parallel axis theorem
is the skew-symmetric operator.
The aforementioned inertial parameters of link
are grouped into a vectoras
Besides the inertial parameters of link , the corresponding joint friction coefficients, motor inertia , and spring stiffness are grouped as additional parameters
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.
The Euler-Lagrange equation for closed-chain robots  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
The friction torques of all the joints are considered as
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
For spring , we only model the stiffness constant as its parameter, which results into the spring torques
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 . Thus, the motor torques with link inertia, springs, frictions, motor inertia, and motion couplings considered are given by
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 .
where is the length between the two axes connecting the spring, which can be calculated using the law of sines as
and mm by measurement is the value of when the spring is relaxed. Based on basic trigonometry, the moment arm can be calculated by
where , and are constants shown in Fig. (b)b.
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
A periodic excitation trajectory based on Fourier series  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.
where is the motor joint coordinate at sampling points and is the sampling point number. The joint position of joint can be calculated by
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  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 :
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 .
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 COM of link , , is inside its convex hull, and , where and are the lower and upper bound of , respectively .
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  as
We can also add the lower and upper bounds to , , and when we have more knowledge about them.
Finally, we use the CVXPY package  with the SCS solver  to solve this convex optimization problem. With the identified barycentric parameters, the standard inertia parameters are computed by solving (6) and (7).
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 open-source 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 second-order numerical differentiation of the velocity. A order low-pass 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, .
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 , 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 , in which the largest relative prediction error is for all the joints and for the first three joints.
In this work, an open-source software package for the dynamic model identification of the full standard dynamic parameters of the dVRK is presented (https://github.com/WPI-AIM/dvrk_dynamics_identification). Link inertia, joint friction, springs, tendon couplings, cable force, and closed-chains are incorporated in the modeling. Fourier series-based trajectories are used to excite the dynamics of the dVRK, with the condition number of the regression matrix minimized. A convex optimization-based method is used to get the full standard dynamic parameters subject to physical feasibility constraints, which are more suitable for the fast recursive Newton-Euler computation. The identification performance is improved significantly compared to previous work . 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 model-based control.
This work has been supported by the National Science Initiative NSF grant: IIS-1637759.
Journal of Machine Learning Research, vol. 17, no. 83, pp. 1–5, 2016.