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

//LINK PARAMETERS

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);
    m_link1_CoMvector = Orocos::Vector(PARAM_CoM_x_1, PARAM_CoM_y_1, PARAM_CoM_z_1);

    robotChain.addSegment(Orocos::Segment(Orocos::Joint(Orocos::Joint::RotZ),Orocos::Frame::DH(PARAM_L_1 , 0 , 0 , 0)));
    robotChain.segments.at(0).setInertia(Orocos::RigidBodyInertia(m_link1_mass, m_link1_CoMvector, m_link1_I));

    m_link2_mass = PARAM_M_2;
    m_link2_I = Orocos::RotationalInertia (PARAM_Ixx_2, PARAM_Iyy_2, PARAM_Izz_2, PARAM_Ixy_2, PARAM_Ixz_2, PARAM_Iyz_2);
    m_link2_CoMvector = Orocos::Vector(PARAM_CoM_x_2, PARAM_CoM_y_2, PARAM_CoM_z_2);

    robotChain.addSegment(Orocos::Segment(Orocos::Joint(Orocos::Joint::RotZ),Orocos::Frame::DH(PARAM_L_2 , 0 , 0 , 0)));
    robotChain.segments[1].setInertia(Orocos::RigidBodyInertia(m_link2_mass, m_link2_CoMvector, m_link2_I));

    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