Documenting difference between RBDL and Pinocchio

waillyam23 commented 1 year ago

Hello,

I am working on a controller for the TIAGo based upon the package pal/gravity_compensation_controller_tutorial, and I wish to use Pinocchio to replace RBDL. I have already written a bit of code, but the computation of pinocchio::rnea() and RigidBodyDynamics::InverseDynamics() give different joint torque values. I use those algorithms with zero velocities and acceleration to do get a torque to compensate gravity. Using RBDL values on the controller works well, but not with Pinocchio values. Below the codes used to do the computation and example values of the tau after both computation.

RBDL:

RigidBodyDynamics::InverseDynamics(rbdl_model_, q_act_, v_zero_, a_zero_, tau_rbdl);

Pinocchio:

Eigen::VectorXd v = Eigen::VectorXd::Zero(pin_model_.nv);
Eigen::VectorXd a = Eigen::VectorXd::Zero(pin_model_.nv);

pinocchio::Data data(pin_model_);
pinocchio::forwardKinematics(pin_model_, data, q_act_); // update joint placement to current

const Eigen::VectorXd &tau_pin = pinocchio::rnea(pin_model_, data, q_act_, v, a);

Tau RBDL/Pinocchio:

tau_rbdl: [152.74007194338722, -6.240851594239601e-17, 4.0323451307044005, 0.04226067542576087, -1.7270027144689144, 0.011168588181166662, 0.34048945657435503, -0.0009378654934529639]
tau_pin: [171.66640684338722, -8.128385682682907e-17, 4.329738059586772, 0.0329609983733339, -1.8539590797841246, 0.02527184890239765, 0.6411085473967822, -0.010446760249633603]

Do I misuse the function ?

jcarpent commented 1 year ago

The conventions between RBDL and Pinocchio are not the same for writing configurations. I guess you are at LAAS, please see directly with Pierre Fernbach (@pfernbach). I've already helped to find the right conversions between the two libraries.

After that, it would be nice to report the solution in Pinocchio documentation. @waillyam23 Is it fine for you?

olivier-stasse commented 1 year ago

Thanks @jcarpent for the pointer. We will report the solution in the Pinocchio documentation.

jcarpent commented 1 year ago

Perfect. Thanks in advance for your contribution.

jcarpent commented 1 year ago

@waillyam23 Any update on this issue?

olivier-stasse commented 1 year ago

Sorry @jcarpent but Pierre is in holidays until next week. As @waillyam23 has other things to look into we will fix the matter next week.

jcarpent commented 1 year ago

@olivier-stasse @waillyam23 What is your status on this issue?

olivier-stasse commented 1 year ago

Hi @jcarpent , @pFernbach and myself talk about it one or two weeks ago. He has to go over his archives. Will try to come with something about it during this week.

pFernbach commented 1 year ago

Hello, sorry for the very late answer but I missed the first notification, thanks Olivier for the ping.

There is several main differences of convention between rbdl and pinocchio

Main differences:

The configuration vector with a freeflyer:
- pinocchio = [x, y, z, qx, qy, qz, qw, joints ....]
- rbdl: [x, y, z, qx, qy, qz, joints ...., qw]
All the forces/momentum/ect in pinocchio are [linear, angular] in rbdl it's [angular, linear]
All the joint velocities and accelerations are expressed in the base frame in rbdl and in the world frame in pinocchio
"Body" in rbdl is called frame in pinocchio.

Example of conversion methods:

Example of conversion method for configuration vector:


/**
 * @brief convertQpin convert a rbdl configuration vector to a pinocchio configuration
 * @param a configuration vector with rbdl convention
 * @return a configuration vector with pinocchio convention
 */
Eigen::VectorXd convertQpin(const Eigen::VectorXd &Q) const{
    Eigen::VectorXd Qpin(Q.size());
    Qpin.head<6>() = Q.head<6>();
    Qpin[6] = Q[Q.size() - 1];
    Qpin.segment(7, Q.size() - 7) = Q.segment(6, Q.size() - 7);
    return Qpin;
}

/**
 * @brief convertQrbdl convert a pinocchio configuration vector to a rbdl configuration
 * @param a configuration vector with pinocchio convention
 * @return a configuration vector with rbdl convention
 */
Eigen::VectorXd convertQrbdl(const Eigen::VectorXd &Q) const{
    Eigen::VectorXd Qrbdl(Q.size());
    Qrbdl.head<6>() = Q.head<6>();
    Qrbdl[Q.size() - 1] = Q[6];
    Qrbdl.segment(6, Q.size() - 7) = Q.segment(7, Q.size() - 7);
    return Qrbdl;
}

/**
 * @brief basePlacement return a SE3 placement of the freeflyer from a configuration vector
 * @param Q a joint configuration vector (with rbdl convention)
 * @return
 */
pinocchio::SE3 basePlacement(const Eigen::VectorXd &Q){
  const Eigen::Quaterniond base_quat(Q[Q.size() - 1], Q[3], Q[4], Q[5]);
  pinocchio::SE3 placement(pinocchio::SE3::Identity());
  placement.rotation(base_quat.matrix());
  placement.translation(Q.head<3>());
  return placement;
}

/**
 * @brief transformQDotWorld transform a joint velocity vector from the base frame to the world frame
 * @param Q a joint configuration vector (with rbdl convention)
 * @param QDot a joint velocity vector (with rbdl convention)
 * @return QDot (with pinocchio convention)
 */
Eigen::VectorXd transformQDotBase(const Eigen::VectorXd &Q, const Eigen::VectorXd &QDot){
  pinocchio::SE3 placement(basePlacement(Q));
  Eigen::VectorXd res(QDot);
  res.head<3>() =  placement.rotation().transpose() * res.head<3>() ;
  return res;
}

/**
 * @brief transformQDDotBase transform a joint acceleration vector from the base frame to the world frame
 * @param Q a joint configuration vector (with rbdl convention)
 * @param QDot a joint velocity vector (with rbdl convention)
 * @param QDDot a joint velocity vector (with rbdl convention)
 * @return QDDot (with pinocchio convention)
 */
Eigen::VectorXd transformQDDotBase(const Eigen::VectorXd &Q, const Eigen::VectorXd &QDot,  const Eigen::VectorXd &QDDot){
  pinocchio::SE3 placement(basePlacement(Q));
  Eigen::VectorXd res(QDDot);
  const eVector3 v_pin(placement.rotation().transpose() * QDot.head<3>());
  res.head<3>() = -QDot.segment<3>(3).cross(v_pin) + (placement.rotation().transpose() * QDDot.head<3>());
  return res;
}

Specific methods :

The following methods use inputs and outputs in rbdl conventions but use pinocchio for all the computations.

Example of running forward kinematic and retrieving a body placement:

Eigen::Isometry3d computeFK(const Eigen::VectorXd &Q, unsigned int bodyId, bool update_kinematics)
{
  assert(bodyId<model_.frames.size(), "Invalid body Id");
  if (update_kinematics)
  {
    pinocchio::forwardKinematics(model_, data_, convertQpin(Q));
    pinocchio::updateFramePlacements(model_, data_ );
  }
  const pinocchio::Frame & f = model_.frames[bodyId];
  return Eigen::Isometry3d(data_.oMi[f.parent].act(f.placement).toHomogeneousMatrix());
}

Example of running forward kinematics and retrieving body velocity:

Eigen::Vector3d computeCartesianLinearVelocity(const Eigen::VectorXd &Q,
                                                                  const Eigen::VectorXd &QDot,
                                                                  unsigned int bodyId,
                                                                  bool update_kinematics)
{
  assert(bodyId<model_.frames.size(), "Invalid body Id");
  if (update_kinematics)
  {
    pinocchio::forwardKinematics(model_, data_, convertQpin(Q), transformQDotBase(Q, QDot));
    pinocchio::updateFramePlacements(model_, data_ );
  }
  const pinocchio::Frame & f = model_.frames[bodyId];
  pinocchio::Motion v_local(f.placement.actInv(data_.v[f.parent]));
  // express the result with rbdl convention
  pinocchio::SE3 w_rotation(pinocchio::SE3::Identity());
  w_rotation.rotation(data_.oMi[f.parent].act(f.placement).rotation());
  return w_rotation.act(v_local).linear();
}

Example of running an inverse dynamic:

void computeInverseDynamics(const Eigen::VectorXd &Q, const Eigen::VectorXd &QDot,
                                               const Eigen::VectorXd &QDDot, Eigen::VectorXd &Tau,
                                               std::vector<RigidBodyDynamics::Math::SpatialVectord> *spatialForces)
{
  // convert spatialVector to pinocchio forces
  pinocchio::container::aligned_vector<pinocchio::Force> fext(model_.joints.size(), pinocchio::Force::Zero());
  if (spatialForces)
  {
    for (size_t i = 0 ; i < spatialForces->size(); ++i)
    {
      fext[i].linear((*spatialForces)[i].tail<3>());
      fext[i].angular((*spatialForces)[i].head<3>());
    }
  }

  Tau = pinocchio::rnea(model_ ,data_, convertQpin(Q), transformQDotBase(Q, QDot),
                        transformQDDotBase(Q, QDot, QDDot), fext);
  // express the forces on the base in the base frame
  pinocchio::SE3 placement(basePlacement(Q));
  Tau.head<3>() =  placement.rotation() * Tau.head<3>() ;
}

I may add more example here if you need, actually we have made the switch from rbdl to pinocchio last year but to limit the scope of the refactoring to one package we are still using the rbdl convention everywhere but use pinocchio for all the computations.

@jcarpent Please tell me if you want me to add this examples somewhere with more visibility.

jcarpent commented 1 year ago

Thanks a lot for your help @pFernbach. I think the current details are sufficient for a new beginner to shift from RBDL to Pinocchio.

Renkunzhao commented 8 months ago

Hello, sorry for the very late answer but I missed the first notification, thanks Olivier for the ping.

There is several main differences of convention between rbdl and pinocchio

Main differences:

The configuration vector with a freeflyer:
- pinocchio = [x, y, z, qx, qy, qz, qw, joints ....]
- rbdl: [x, y, z, qx, qy, qz, joints ...., qw]
All the forces/momentum/ect in pinocchio are [linear, angular] in rbdl it's [angular, linear]
All the joint velocities and accelerations are expressed in the base frame in rbdl and in the world frame in pinocchio
"Body" in rbdl is called frame in pinocchio.

Example of conversion methods:

Example of conversion method for configuration vector:

/**
 * @brief convertQpin convert a rbdl configuration vector to a pinocchio configuration
 * @param a configuration vector with rbdl convention
 * @return a configuration vector with pinocchio convention
 */
Eigen::VectorXd convertQpin(const Eigen::VectorXd &Q) const{
    Eigen::VectorXd Qpin(Q.size());
    Qpin.head<6>() = Q.head<6>();
    Qpin[6] = Q[Q.size() - 1];
    Qpin.segment(7, Q.size() - 7) = Q.segment(6, Q.size() - 7);
    return Qpin;
}

/**
 * @brief convertQrbdl convert a pinocchio configuration vector to a rbdl configuration
 * @param a configuration vector with pinocchio convention
 * @return a configuration vector with rbdl convention
 */
Eigen::VectorXd convertQrbdl(const Eigen::VectorXd &Q) const{
    Eigen::VectorXd Qrbdl(Q.size());
    Qrbdl.head<6>() = Q.head<6>();
    Qrbdl[Q.size() - 1] = Q[6];
    Qrbdl.segment(6, Q.size() - 7) = Q.segment(7, Q.size() - 7);
    return Qrbdl;
}

/**
 * @brief basePlacement return a SE3 placement of the freeflyer from a configuration vector
 * @param Q a joint configuration vector (with rbdl convention)
 * @return
 */
pinocchio::SE3 basePlacement(const Eigen::VectorXd &Q){
  const Eigen::Quaterniond base_quat(Q[Q.size() - 1], Q[3], Q[4], Q[5]);
  pinocchio::SE3 placement(pinocchio::SE3::Identity());
  placement.rotation(base_quat.matrix());
  placement.translation(Q.head<3>());
  return placement;
}

/**
 * @brief transformQDotWorld transform a joint velocity vector from the base frame to the world frame
 * @param Q a joint configuration vector (with rbdl convention)
 * @param QDot a joint velocity vector (with rbdl convention)
 * @return QDot (with pinocchio convention)
 */
Eigen::VectorXd transformQDotBase(const Eigen::VectorXd &Q, const Eigen::VectorXd &QDot){
  pinocchio::SE3 placement(basePlacement(Q));
  Eigen::VectorXd res(QDot);
  res.head<3>() =  placement.rotation().transpose() * res.head<3>() ;
  return res;
}

/**
 * @brief transformQDDotBase transform a joint acceleration vector from the base frame to the world frame
 * @param Q a joint configuration vector (with rbdl convention)
 * @param QDot a joint velocity vector (with rbdl convention)
 * @param QDDot a joint velocity vector (with rbdl convention)
 * @return QDDot (with pinocchio convention)
 */
Eigen::VectorXd transformQDDotBase(const Eigen::VectorXd &Q, const Eigen::VectorXd &QDot,  const Eigen::VectorXd &QDDot){
  pinocchio::SE3 placement(basePlacement(Q));
  Eigen::VectorXd res(QDDot);
  const eVector3 v_pin(placement.rotation().transpose() * QDot.head<3>());
  res.head<3>() = -QDot.segment<3>(3).cross(v_pin) + (placement.rotation().transpose() * QDDot.head<3>());
  return res;
}

Specific methods :

The following methods use inputs and outputs in rbdl conventions but use pinocchio for all the computations.

Example of running forward kinematic and retrieving a body placement:

Eigen::Isometry3d computeFK(const Eigen::VectorXd &Q, unsigned int bodyId, bool update_kinematics)
{
  assert(bodyId<model_.frames.size(), "Invalid body Id");
  if (update_kinematics)
  {
    pinocchio::forwardKinematics(model_, data_, convertQpin(Q));
    pinocchio::updateFramePlacements(model_, data_ );
  }
  const pinocchio::Frame & f = model_.frames[bodyId];
  return Eigen::Isometry3d(data_.oMi[f.parent].act(f.placement).toHomogeneousMatrix());
}

Example of running forward kinematics and retrieving body velocity:

Eigen::Vector3d computeCartesianLinearVelocity(const Eigen::VectorXd &Q,
                                                                  const Eigen::VectorXd &QDot,
                                                                  unsigned int bodyId,
                                                                  bool update_kinematics)
{
  assert(bodyId<model_.frames.size(), "Invalid body Id");
  if (update_kinematics)
  {
    pinocchio::forwardKinematics(model_, data_, convertQpin(Q), transformQDotBase(Q, QDot));
    pinocchio::updateFramePlacements(model_, data_ );
  }
  const pinocchio::Frame & f = model_.frames[bodyId];
  pinocchio::Motion v_local(f.placement.actInv(data_.v[f.parent]));
  // express the result with rbdl convention
  pinocchio::SE3 w_rotation(pinocchio::SE3::Identity());
  w_rotation.rotation(data_.oMi[f.parent].act(f.placement).rotation());
  return w_rotation.act(v_local).linear();
}

Example of running an inverse dynamic:

void computeInverseDynamics(const Eigen::VectorXd &Q, const Eigen::VectorXd &QDot,
                                               const Eigen::VectorXd &QDDot, Eigen::VectorXd &Tau,
                                               std::vector<RigidBodyDynamics::Math::SpatialVectord> *spatialForces)
{
  // convert spatialVector to pinocchio forces
  pinocchio::container::aligned_vector<pinocchio::Force> fext(model_.joints.size(), pinocchio::Force::Zero());
  if (spatialForces)
  {
    for (size_t i = 0 ; i < spatialForces->size(); ++i)
    {
      fext[i].linear((*spatialForces)[i].tail<3>());
      fext[i].angular((*spatialForces)[i].head<3>());
    }
  }

  Tau = pinocchio::rnea(model_ ,data_, convertQpin(Q), transformQDotBase(Q, QDot),
                        transformQDDotBase(Q, QDot, QDDot), fext);
  // express the forces on the base in the base frame
  pinocchio::SE3 placement(basePlacement(Q));
  Tau.head<3>() =  placement.rotation() * Tau.head<3>() ;
}

I may add more example here if you need, actually we have made the switch from rbdl to pinocchio last year but to limit the scope of the refactoring to one package we are still using the rbdl convention everywhere but use pinocchio for all the computations.

@jcarpent Please tell me if you want me to add this examples somewhere with more visibility.

did you mean all floating base velocities and accelerations are expressed in the world frame in rbdl and in the base frame in pinocchio?

stack-of-tasks / pinocchio