# Problem with dynamic model and inverse dynamic solver with KDL

 Submitted by focke_85 on Wed, 2014-01-22 11:24

Hi everyone

I have been trying to build a double pendulum with the KDL library in order to check how do the kinematic and dynamic solvers work. I checked the direct kinematics solver to see if my chain construction was fine, and I got the expected results. Nevertheless, when trying to solve the inverse dynamics to get the torques in each joint given some joints' angular positions, velocities and accelerations, I cross with some problems and unexpected results. I have solved the dynamic problem already manually with Euler-Lagrange equations and compared to some results gotten from a simulator (Adams) and they match, so that means that the problem is in my code.

Besides the unexpected torque results, I have seen that, after setting the inertia parameters of each segment and printing the rotational inertia values of the just-set segments, the values do not match with the original ones. As far as I see, they are internally changed depending on the mass and CoG vector of the segment. The inertia tensor that I am using is wrt a frame parallel to the joint frame, with its origin located at the CoM. Is that right or it should be wrt other frame?

Here is the code I used. FYI the pendulum was designed in rest position (i.e. heading downwards), therefore the z axis of the joints are meant to head 'out of the screen', the x axis downwards, and the gravity vector would be [9.8,0,0]. Also, no external forces are applied to the segments.

//PARAMETERS

1. define PARAM_Gravity_x 9.80665
2. define PARAM_Gravity_y 0.0
3. define PARAM_Gravity_z 0.0

1. define PARAM_L_1 0.4000
2. define PARAM_R_1 0.2000
3. define PARAM_M_1 23.0695778028
4. define PARAM_Ixx_1 0.062920553136
5. define PARAM_Iyy_1 0.22363523386
6. define PARAM_Izz_1 0.27694346292
7. define PARAM_Ixy_1 0.0
8. define PARAM_Ixz_1 0.0
9. define PARAM_Iyz_1 0.0
10. define PARAM_CoM_x_1 0.200
11. define PARAM_CoM_y_1 0.0
12. define PARAM_CoM_z_1 0.0

1. define PARAM_L_2 0.3000
2. define PARAM_R_2 0.1500
3. define PARAM_M_2 11.2746868912
4. define PARAM_Ixx_2 0.012262128232
5. define PARAM_Iyy_2 0.073701230588
6. define PARAM_Izz_2 0.081265572615
7. define PARAM_Ixy_2 0.0
8. define PARAM_Ixz_2 0.0
9. define PARAM_Iyz_2 0.0
10. define PARAM_CoM_x_2 0.150
11. define PARAM_CoM_y_2 0.0
12. define PARAM_CoM_z_2 0.0

...

```    m_link1_mass = PARAM_M_1;
m_link1_I = Orocos::RotationalInertia (PARAM_Ixx_1, PARAM_Iyy_1, PARAM_Izz_1, PARAM_Ixy_1, PARAM_Ixz_1, PARAM_Iyz_1);

robotChain.addSegment(Orocos::Segment(Orocos::Joint(Orocos::Joint::RotZ),Orocos::Frame::DH(PARAM_L_1 , 0 , 0 , 0)));

m_link2_I = Orocos::RotationalInertia (PARAM_Ixx_2, PARAM_Iyy_2, PARAM_Izz_2, PARAM_Ixy_2, PARAM_Ixz_2, PARAM_Iyz_2);

robotChain.addSegment(Orocos::Segment(Orocos::Joint(Orocos::Joint::RotZ),Orocos::Frame::DH(PARAM_L_2 , 0 , 0 , 0)));
```
...

```    Orocos::JntArray q (robotChain.getNrOfJoints());
Orocos::JntArray q_dot (robotChain.getNrOfJoints());
Orocos::JntArray q_dot_dot (robotChain.getNrOfJoints());

Orocos::Frame actualFrame;

Orocos::Wrenches extForces(robotChain.getNrOfSegments());

Orocos::JntArray solutionVectorIK = Orocos::JntArray(robotChain.getNrOfJoints());
Orocos::JntArray solutionVectorID = Orocos::JntArray(robotChain.getNrOfJoints());

Orocos::ChainFkSolverPos_recursive FKSolverPos (robotChain);
Orocos::ChainIdSolver_RNE IDSolverTorque(robotChain, Orocos::Vector(PARAM_Gravity_x, PARAM_Gravity_y, PARAM_Gravity_z));

//q_1(t) = pi/6 * sin(t)
//q_2(t) = pi/3 * sin(3t)
time = Orocos::PI/2;
q(0) = Orocos::PI/6 * std::sin(time);
q_dot(0) = Orocos::PI/6 * std::cos(time);
q_dot_dot(0) = - Orocos::PI/6 * std::sin(time);
q(1) = Orocos::PI/3 * std::sin(3*time);
q_dot(1) = Orocos::PI * std::cos(3*time);
q_dot_dot(1) = - 3 * Orocos::PI * std::sin(3*time);

cout << "IDSolverTorque output = " << IDSolverTorque.CartToJnt(q, q_dot, q_dot_dot, extForces, solutionVectorID) << endl;
cout << "\nActual joint torques = [ " << solutionVectorID(0) <<  " ; " << solutionVectorID(1) << " ]" << endl;
```

Btw, is there any manual for the KDL including not just the primitives and basics?

BR,

Santiago

### Re: Problem with dynamic model and inverse dynamic solver

Ok, after checking this problem again I think I figure out what is the issue, in case someone is running with the same problem. If I am wrong please someone correct me.

The CoM vector that must be used when setting the inertia parameters of the segments is the position of the CoM relative to the frame located at the tip of the segment (i.e. the segment.Joint). In my case, I was entering the vector relative to a frame located at the joint.

Also, the reason why the the inertia tensor set by the user and the one obtained via getRotationalInertia() do not match, is that they are not relative to the same reference frame. The tensor set by the user is a tensor relative to a frame located at the CoM of the segment, with its xyz-axis parallel to the xyz-axis of the tip frame (i.e. the rotation matrix between this two reference frames is the identity matrix), while the one returned by getRotationalInertia() is the tensor relative to the tip frame, calculated (from the tensor set by the user, the mass of the segment and the position of the CoM relative to the reference frame at the tip) with the Parallel Axis Theorem.

Now I am getting the expected torque results from the ID solver :) I hope it is because I finally got right how the referencing in KDL works and not just because of a coincidence.

BR,

Santiago

focke_85 wrote:
Hi everyone

I have been trying to build a double pendulum with the KDL library in order to check how do the kinematic and dynamic solvers work. I checked the direct kinematics solver to see if my chain construction was fine, and I got the expected results. Nevertheless, when trying to solve the inverse dynamics to get the torques in each joint given some joints' angular positions, velocities and accelerations, I cross with some problems and unexpected results. I have solved the dynamic problem already manually with Euler-Lagrange equations and compared to some results gotten from a simulator (Adams) and they match, so that means that the problem is in my code.

Besides the unexpected torque results, I have seen that, after setting the inertia parameters of each segment and printing the rotational inertia values of the just-set segments, the values do not match with the original ones. As far as I see, they are internally changed depending on the mass and CoG vector of the segment. The inertia tensor that I am using is wrt a frame parallel to the joint frame, with its origin located at the CoM. Is that right or it should be wrt other frame?

Here is the code I used. FYI the pendulum was designed in rest position (i.e. heading downwards), therefore the z axis of the joints are meant to head 'out of the screen', the x axis downwards, and the gravity vector would be [9.8,0,0]. Also, no external forces are applied to the segments.

//PARAMETERS

1. define PARAM_Gravity_x 9.80665
2. define PARAM_Gravity_y 0.0
3. define PARAM_Gravity_z 0.0

1. define PARAM_L_1 0.4000
2. define PARAM_R_1 0.2000
3. define PARAM_M_1 23.0695778028
4. define PARAM_Ixx_1 0.062920553136
5. define PARAM_Iyy_1 0.22363523386
6. define PARAM_Izz_1 0.27694346292
7. define PARAM_Ixy_1 0.0
8. define PARAM_Ixz_1 0.0
9. define PARAM_Iyz_1 0.0
10. define PARAM_CoM_x_1 0.200
11. define PARAM_CoM_y_1 0.0
12. define PARAM_CoM_z_1 0.0

1. define PARAM_L_2 0.3000
2. define PARAM_R_2 0.1500
3. define PARAM_M_2 11.2746868912
4. define PARAM_Ixx_2 0.012262128232
5. define PARAM_Iyy_2 0.073701230588
6. define PARAM_Izz_2 0.081265572615
7. define PARAM_Ixy_2 0.0
8. define PARAM_Ixz_2 0.0
9. define PARAM_Iyz_2 0.0
10. define PARAM_CoM_x_2 0.150
11. define PARAM_CoM_y_2 0.0
12. define PARAM_CoM_z_2 0.0

...

```    m_link1_mass = PARAM_M_1;
m_link1_I = Orocos::RotationalInertia (PARAM_Ixx_1, PARAM_Iyy_1, PARAM_Izz_1, PARAM_Ixy_1, PARAM_Ixz_1, PARAM_Iyz_1);

robotChain.addSegment(Orocos::Segment(Orocos::Joint(Orocos::Joint::RotZ),Orocos::Frame::DH(PARAM_L_1 , 0 , 0 , 0)));

m_link2_I = Orocos::RotationalInertia (PARAM_Ixx_2, PARAM_Iyy_2, PARAM_Izz_2, PARAM_Ixy_2, PARAM_Ixz_2, PARAM_Iyz_2);

robotChain.addSegment(Orocos::Segment(Orocos::Joint(Orocos::Joint::RotZ),Orocos::Frame::DH(PARAM_L_2 , 0 , 0 , 0)));
```
...

```    Orocos::JntArray q (robotChain.getNrOfJoints());
Orocos::JntArray q_dot (robotChain.getNrOfJoints());
Orocos::JntArray q_dot_dot (robotChain.getNrOfJoints());

Orocos::Frame actualFrame;

Orocos::Wrenches extForces(robotChain.getNrOfSegments());

Orocos::JntArray solutionVectorIK = Orocos::JntArray(robotChain.getNrOfJoints());
Orocos::JntArray solutionVectorID = Orocos::JntArray(robotChain.getNrOfJoints());

Orocos::ChainFkSolverPos_recursive FKSolverPos (robotChain);
Orocos::ChainIdSolver_RNE IDSolverTorque(robotChain, Orocos::Vector(PARAM_Gravity_x, PARAM_Gravity_y, PARAM_Gravity_z));

//q_1(t) = pi/6 * sin(t)
//q_2(t) = pi/3 * sin(3t)
time = Orocos::PI/2;
q(0) = Orocos::PI/6 * std::sin(time);
q_dot(0) = Orocos::PI/6 * std::cos(time);
q_dot_dot(0) = - Orocos::PI/6 * std::sin(time);
q(1) = Orocos::PI/3 * std::sin(3*time);
q_dot(1) = Orocos::PI * std::cos(3*time);
q_dot_dot(1) = - 3 * Orocos::PI * std::sin(3*time);

cout << "IDSolverTorque output = " << IDSolverTorque.CartToJnt(q, q_dot, q_dot_dot, extForces, solutionVectorID) << endl;
cout << "\nActual joint torques = [ " << solutionVectorID(0) <<  " ; " << solutionVectorID(1) << " ]" << endl;
```

Btw, is there any manual for the KDL including not just the primitives and basics?

BR,

Santiago

### Problem with dynamic model and inverse dynamic solver

On Fri, 28 Mar 2014, focke [dot] 85 [..] ... wrote:

> Ok, after checking this problem again I think I figure out what is the issue,
> in case someone is running with the same problem. If I am wrong please
> someone correct me.
>
> The CoM vector that must be used when setting the inertia parameters of the
> segments is the position of the CoM relative to the frame located at the tip
> of the segment (i.e. the segment.Joint). In my case, I was entering the
> vector relative to a frame located at the joint.
>
> Also, the reason why the the inertia tensor set by the user and the one
> obtained via getRotationalInertia() do not match, is that they are not
> relative to the same reference frame. The tensor set by the user is a tensor
> relative to a frame located at the CoM of the segment, with its xyz-axis
> parallel to the xyz-axis of the tip frame (i.e. the rotation matrix between
> this two reference frames is the identity matrix), while the one returned by
> getRotationalInertia() is the tensor relative to the tip frame, calculated
> (from the tensor set by the user, the mass of the segment and the position of
> the CoM relative to the reference frame at the tip) with the Parallel Axis
> Theorem.
>
> Now I am getting the expected torque results from the ID solver :) I hope it
> is because I finally got right how the referencing in KDL works and not just
> because of a coincidence.

Your experience is, unfortunately, all too common :-(
The cause is the lack of seriousness/time of the community to make code
that represents the semantics of frames completely, and explicitly.
A solution for this problem
<http://www.orocos.org/wiki/geometric-relations-semantics-wiki>
is waiting to be integrated for two years or so already, but noone has
found it necessary to spend the time doing it. This is _not_ a trivial
effort, not in the least because it requires a change in attitude in the
developers' minds...

> BR,
>
> Santiago

Herman

>
>
>
>

focke_85 wrote:
Hi everyone
>
> I have been trying to build a double pendulum with the KDL library in order
> to check how do the kinematic and dynamic solvers work. I checked the direct
> kinematics solver to see if my chain construction was fine, and I got the
> expected results. Nevertheless, when trying to solve the inverse dynamics to
> get the torques in each joint given some joints' angular positions,
> velocities and accelerations, I cross with some problems and unexpected
> results. I have solved the dynamic problem already manually with
> Euler-Lagrange equations and compared to some results gotten from a simulator
> (Adams) and they match, so that means that the problem is in my code.
>
> Besides the unexpected torque results, I have seen that, after setting the
> inertia parameters of each segment and printing the rotational inertia values
> of the just-set segments, the values do not match with the original ones. As
> far as I see, they are internally changed depending on the mass and CoG
> vector of the segment. The inertia tensor that I am using is wrt a frame
> parallel to the joint frame, with its origin located at the CoM. Is that
> right or it should be wrt other frame?
>
> Here is the code I used. FYI the pendulum was designed in rest position (i.e.
> heading downwards), therefore the z axis of the joints are meant to head 'out
> of the screen', the x axis downwards, and the gravity vector would be
> [9.8,0,0]. Also, no external forces are applied to the segments.
>
> //PARAMETERS
> #define PARAM_Gravity_x 9.80665
> #define PARAM_Gravity_y 0.0
> #define PARAM_Gravity_z 0.0
>
> #define PARAM_L_1 0.4000
> #define PARAM_R_1 0.2000
> #define PARAM_M_1 23.0695778028
> #define PARAM_Ixx_1 0.062920553136
> #define PARAM_Iyy_1 0.22363523386
> #define PARAM_Izz_1 0.27694346292
> #define PARAM_Ixy_1 0.0
> #define PARAM_Ixz_1 0.0
> #define PARAM_Iyz_1 0.0
> #define PARAM_CoM_x_1 0.200
> #define PARAM_CoM_y_1 0.0
> #define PARAM_CoM_z_1 0.0
>
> #define PARAM_L_2 0.3000
> #define PARAM_R_2 0.1500
> #define PARAM_M_2 11.2746868912
> #define PARAM_Ixx_2 0.012262128232
> #define PARAM_Iyy_2 0.073701230588
> #define PARAM_Izz_2 0.081265572615
> #define PARAM_Ixy_2 0.0
> #define PARAM_Ixz_2 0.0
> #define PARAM_Iyz_2 0.0
> #define PARAM_CoM_x_2 0.150
> #define PARAM_CoM_y_2 0.0
> #define PARAM_CoM_z_2 0.0
>
> ...
>
> m_link1_I = Orocos::RotationalInertia (PARAM_Ixx_1, PARAM_Iyy_1,
> PARAM_Izz_1, PARAM_Ixy_1, PARAM_Ixz_1, PARAM_Iyz_1);
> PARAM_CoM_z_1);
>
> , 0 , 0 , 0)));
>
> m_link2_I = Orocos::RotationalInertia (PARAM_Ixx_2, PARAM_Iyy_2,
> PARAM_Izz_2, PARAM_Ixy_2, PARAM_Ixz_2, PARAM_Iyz_2);
> PARAM_CoM_z_2);
>
> , 0 , 0 , 0)));
>
> ...
>
> Orocos::JntArray q (robotChain.getNrOfJoints());
> Orocos::JntArray q_dot (robotChain.getNrOfJoints());
> Orocos::JntArray q_dot_dot (robotChain.getNrOfJoints());
>
> Orocos::Frame actualFrame;
>
> Orocos::Wrenches extForces(robotChain.getNrOfSegments());
>
> Orocos::JntArray solutionVectorIK =
> Orocos::JntArray(robotChain.getNrOfJoints());
> Orocos::JntArray solutionVectorID =
> Orocos::JntArray(robotChain.getNrOfJoints());
>
> Orocos::ChainFkSolverPos_recursive FKSolverPos (robotChain);
> Orocos::ChainIdSolver_RNE IDSolverTorque(robotChain,
> Orocos::Vector(PARAM_Gravity_x, PARAM_Gravity_y, PARAM_Gravity_z));
>
> //q_1(t) = pi/6 * sin(t)
> //q_2(t) = pi/3 * sin(3t)
> time = Orocos::PI/2;
> q(0) = Orocos::PI/6 * std::sin(time);
> q_dot(0) = Orocos::PI/6 * std::cos(time);
> q_dot_dot(0) = - Orocos::PI/6 * std::sin(time);
> q(1) = Orocos::PI/3 * std::sin(3*time);
> q_dot(1) = Orocos::PI * std::cos(3*time);
> q_dot_dot(1) = - 3 * Orocos::PI * std::sin(3*time);
>
> cout
>