Discrepancies in Dynamics Computation between Raisim and Pinocchio for Quadruped Simulation #2355

kdyun0118 · 2024-07-30T02:04:43Z

kdyun0118
Jul 30, 2024

Hello,

I am testing the state transition data of a quadruped Go1 obtained from the Raisim simulation in Pinocchio C++.

In Raisim, I stored the generalized coordinate, generalized velocity, generalized acceleration, and joint torques and confirmed that the Lagrangian equation of motion $\tau =M(q) \dot{u} + h(q,u)$ holds where q is the generalized coordinate and u is the generalized velocity.
I then tried to calculate the same equation in Pinocchio, but the equation did not match.

From my investigation, I found the following differences between Raisim and Pinocchio:
First, the order of the quaternion is different, and the reference frame for the base parts of the generalized velocity, generalized acceleration, and torque is also different.
Additionally, the leg order is recognized differently, so I adjusted the joint order as well.
Raisim convention

q = [global_base_position, global_base_quaternion(w,x,y,z), joint_positions(FR,FL,RR,RL)]
v = [global_base_velocity_linear, global_base_velocity_angular, joint_velocities]
a = [global_base_acceleration_linear, global_base_acceleration_angular, joint_accelerations]

Pinocchio convention (local = joint frame)

q = [global_base_position, global_base_quaternion(x,y,z,w), joint_positions(FL,FR,RL,RR)]
v = [local_base_velocity_linear, local_base_velocity_angular, joint_velocities]
a = [local_base_acceleration_linear, local_base_acceleration_angular, joint_accelerations]

Lastly, I normalized the generalized coordinates. However, despite these efforts, there are still issues.

After several attempts, I found that while the mass matrix and generalized gravity are computed the same way, the nonlinearity or nonlinear effect parts seem to be completely different. Due to my limited knowledge, I am not sure where this discrepancy arises.
Does the function 'nonLinearEffect' has different meaning in both cases?
Another interesting point is that the forward dynamics results differ between Raisim and Pinocchio (due to the difference in nonlinearity term), but only in the base linear acceleration part. This seems to be a significant physical clue, but I haven't figured it out yet.

Below is the code and the results I used.

#include "model_interface.hpp"
#include <Eigen/Dense>
#include "raisim/World.hpp"
#include "raisim/RaisimServer.hpp"


void loadData(Eigen::MatrixXd& gc_matrix, Eigen::MatrixXd& gv_matrix, Eigen::MatrixXd& ga_matrix, Eigen::MatrixXd& tau_matrix);
Eigen::VectorXd RaisimGcToPinocchioGc(Eigen::VectorXd raisim_gc);
Eigen::VectorXd RaisimGvToPinocchioGv(Eigen::VectorXd raisim_gc,Eigen::VectorXd raisim_gv);
Eigen::VectorXd RaisimGaToPinocchioGa(Eigen::VectorXd raisim_gc,Eigen::VectorXd raisim_ga);
Eigen::VectorXd RaisimTauToPinocchioTau(Eigen::VectorXd raisim_gc,Eigen::VectorXd raisim_tau);
Eigen::MatrixXd RaisimMatToPinocchioMat(Eigen::VectorXd raisim_gc,Eigen::MatrixXd raisim_mat);
Eigen::VectorXd PinocchioGvToRaisimGv(Eigen::VectorXd pin_gc,Eigen::VectorXd pin_gv);

int main(){
  std::cout.precision(15);
  std::string urdf_filename = "/home/kdyun/Desktop/urdf2modelcasadi/urdf2model/models/go1/go1.urdf";

  Eigen::Vector3d gravity_vector(0, 0, -9.81);

  /// raisim model
  // raisim convention
  // q = [global_base_position, global_base_quaternion(w,x,y,z), joint_positions(FR,FL,RR,RL)]
  // v = [global_base_velocity_linear, global_base_velocity_angular, joint_velocities]
  // a = [global_base_acceleration_linear, global_base_acceleration_angular, joint_accelerations]

  raisim::ArticulatedSystem *Go1_raisim = new raisim::ArticulatedSystem(urdf_filename);

  /// pinocchio model
  // pinocchio convention  *local = joint frame
  // q = [global_base_position, global_base_quaternion(x,y,z,w), joint_positions(FL,FR,RL,RR)]
  // v = [local_base_velocity_linear, local_base_velocity_angular, joint_velocities]
  // a = [local_base_acceleration_linear, local_base_acceleration_angular, joint_accelerations]
  pinocchio::Model Go1_pin_model;
  pinocchio::urdf::buildModel(urdf_filename,pinocchio::JointModelFreeFlyer(),Go1_pin_model);

  Go1_pin_model.gravity.linear(gravity_vector);

  pinocchio::Data Go1_pin_data = pinocchio::Data(Go1_pin_model);

  /// load Data
  Eigen::MatrixXd gc_matrix,gv_matrix,ga_matrix,tau_matrix;
  loadData(gc_matrix,gv_matrix,ga_matrix,tau_matrix);

  int i=5000;
  std::cout << i <<" -----------------------------------------" << std::endl;
  /// raisim dynamics test
  Eigen::VectorXd raisim_gc,raisim_gv,raisim_ga,raisim_tau;
  raisim_gc.setZero(19);raisim_gv.setZero(18);raisim_ga.setZero(18);raisim_tau.setZero(18);
  raisim_gc = gc_matrix.row(i);
  raisim_gv = gv_matrix.row(i);
  raisim_ga = ga_matrix.row(i);
  raisim_tau = tau_matrix.row(i);

  raisim::Mat<3,3> rot;raisim::quatToRotMat(raisim_gc.segment(3,4),rot);
  std::cout << "rotation matrix by raisim" << std::endl << rot.e() << std::endl;
  std::cout << "raisim gc " <<  raisim_gc.transpose()<< std::endl;
  std::cout << "raisim gv " <<  raisim_gv.transpose() << std::endl;
  std::cout << "raisim ga " <<  raisim_ga.transpose() << std::endl;
  std::cout << "raisim tau "<<  raisim_tau.transpose() << std::endl;

  Go1_raisim->setState(raisim_gc,raisim_gv);
  Eigen::MatrixXd M = Go1_raisim->getMassMatrix().e();
  Eigen::VectorXd h = Go1_raisim->getNonlinearities(gravity_vector).e();
  Eigen::MatrixXd tmp = RaisimMatToPinocchioMat(raisim_gc,M);
  Eigen::VectorXd tmp2 = RaisimTauToPinocchioTau(raisim_gc,h);

  std::cout << "raisim M to pin" << std::endl << tmp << std::endl;
  std::cout << "raisim h to pin" << std::endl <<  tmp2.transpose() << std::endl;
  std::cout << "raisim error " <<  (M*raisim_ga+h-raisim_tau).transpose()<< std::endl;
  std::cout << "raisim error norm " <<  (M*raisim_ga+h-raisim_tau).norm() << std::endl;
  std::cout << std::endl;

  std::cout << i <<" -----------------------------------------" << std::endl;
  /// pinocchio dynamics test
  Eigen::VectorXd pin_gc,pin_gv,pin_ga,pin_tau;
  pin_gc.setZero(19);pin_gv.setZero(18);pin_ga.setZero(18);pin_tau.setZero(18);
  pin_gc = RaisimGcToPinocchioGc(raisim_gc);
  pin_gv = RaisimGvToPinocchioGv(raisim_gc,raisim_gv);
  pin_ga = RaisimGaToPinocchioGa(raisim_gc,raisim_ga);
  pin_tau = RaisimTauToPinocchioTau(raisim_gc,raisim_tau);
  pinocchio::normalize(Go1_pin_model,pin_gc);
  std::cout << "pin gc " <<  pin_gc.transpose()<< std::endl;
  std::cout << "pin gv " <<  pin_gv.transpose() << std::endl;
  std::cout << "pin ga " <<  pin_ga.transpose() << std::endl;
  std::cout << "pin tau "<<  pin_tau.transpose() << std::endl;

  pinocchio::crba(Go1_pin_model,Go1_pin_data,pin_gc,pinocchio::Convention::LOCAL); // fill only upper triangular
  //  pinocchio::crba(Go1_pin_model,Go1_pin_data,pin_gc,pinocchio::Convention::WORLD); // fill only upper triangular
  Go1_pin_data.M.triangularView<Eigen::StrictlyLower>() = Go1_pin_data.M.transpose().triangularView<Eigen::StrictlyLower>();
  pinocchio::nonLinearEffects(Go1_pin_model,Go1_pin_data,pin_gc,pin_gv); // run this after rnea?

  std::cout << "crba_local " <<std::endl<<  Go1_pin_data.M << std::endl;
  std::cout << "nle " <<std::endl<<  Go1_pin_data.nle.transpose() << std::endl;

  std::cout << "pin error " <<  (Go1_pin_data.M*pin_ga+Go1_pin_data.nle-pin_tau).transpose() << std::endl;
  std::cout << "pin error norm " <<  (Go1_pin_data.M*pin_ga+Go1_pin_data.nle-pin_tau).norm() << std::endl;

  std::cout << "M diff " <<std::endl<<  (Go1_pin_data.M-tmp).norm() << std::endl;
  std::cout << "h diff " <<std::endl<<  (Go1_pin_data.nle-tmp2).transpose() << std::endl;

  /// aba test
  pinocchio::aba(Go1_pin_model, Go1_pin_data, pin_gc, pin_gv, pin_tau);
  std::cout << "aba " <<  Go1_pin_data.ddq.transpose() << std::endl;
  std::cout << "aba error1 " <<  (Go1_pin_data.ddq-pin_ga).transpose() << std::endl;
  std::cout << "aba error2 " <<  (Go1_pin_data.M*(Go1_pin_data.ddq-pin_ga)).transpose() << std::endl;

}

void loadData(Eigen::MatrixXd& gc_matrix, Eigen::MatrixXd& gv_matrix, Eigen::MatrixXd& ga_matrix, Eigen::MatrixXd& tau_matrix){
  std::string rootPath = "/home/kdyun/Desktop/ros2_ws/src/analyzer/";
  std::string dataPath = "src/swing torque, free fall/";

  std::ifstream gc_log;
  gc_matrix.setZero(100000, 19);
  gc_log.open(rootPath + dataPath + "generalized_position.txt");
  for (size_t i = 0; i < 100000; ++i) {
    for (size_t j = 0; j < 19; ++j) {
      gc_log >> gc_matrix(i, j);
    }
  }
  gc_log.close();
  std::cout << "gc log loaded" << std::endl;
  std::ifstream gv_log;
  gv_matrix.setZero(100000, 18);
  gv_log.open(rootPath + dataPath + "generalized_velocity.txt");
  for (size_t i = 0; i < 100000; ++i) {
    for (size_t j = 0; j < 18; ++j) {
      gv_log >> gv_matrix(i, j);
    }
  }
  gv_log.close();
  std::cout << "gv log loaded" << std::endl;
  std::ifstream ga_log;
  ga_matrix.setZero(100000, 18);
  ga_log.open(rootPath + dataPath + "generalized_acceleration.txt");
  for (size_t i = 0; i < 100000; ++i) {
    for (size_t j = 0; j < 18; ++j) {
      ga_log >> ga_matrix(i, j);
    }
  }
  ga_log.close();
  std::cout << "ga log loaded" << std::endl;
  std::ifstream tau_log;
  tau_matrix.setZero(100000, 18);
  tau_log.open(rootPath + dataPath + "generalized_torque.txt");
  for (size_t i = 0; i < 100000; ++i) {
    for (size_t j = 0; j < 18; ++j) {
      tau_log >> tau_matrix(i, j);
    }
  }
  tau_log.close();
  std::cout << "tau log loaded" << std::endl;
}

Eigen::VectorXd RaisimGcToPinocchioGc(Eigen::VectorXd raisim_gc){
  Eigen::VectorXd pin_gc;pin_gc.setZero(19);
  pin_gc.head(3) = raisim_gc.head(3);
  pin_gc.segment(3,4) << raisim_gc(4),raisim_gc(5),raisim_gc(6),raisim_gc(3); // (w,x,y,z) -> (x,y,z,w)

// (FR,FL,RR,RL) -> (FL,FR,RL,RR)
  pin_gc.tail(12).segment(0,3)=raisim_gc.tail(12).segment(3,3);
  pin_gc.tail(12).segment(3,3)=raisim_gc.tail(12).segment(0,3);
  pin_gc.tail(12).segment(6,3)=raisim_gc.tail(12).segment(9,3);
  pin_gc.tail(12).segment(9,3)=raisim_gc.tail(12).segment(6,3);
  return pin_gc;
}
Eigen::VectorXd RaisimGvToPinocchioGv(Eigen::VectorXd raisim_gc,Eigen::VectorXd raisim_gv){
  Eigen::VectorXd pin_gv;pin_gv.setZero(18);
  raisim::Mat<3,3> rot;raisim::quatToRotMat(raisim_gc.segment(3,4),rot);
  Eigen::Matrix3d Rot; Rot.setIdentity();Rot = rot.e();

  pin_gv.head(3) = Rot.transpose()*raisim_gv.head(3); // world frame -> base local frame
  pin_gv.segment(3,3) = Rot.transpose()*raisim_gv.segment(3,3); // world frame -> base local frame
  // (FR,FL,RR,RL) -> (FL,FR,RL,RR)
  pin_gv.tail(12).segment(0,3)=raisim_gv.tail(12).segment(3,3);
  pin_gv.tail(12).segment(3,3)=raisim_gv.tail(12).segment(0,3);
  pin_gv.tail(12).segment(6,3)=raisim_gv.tail(12).segment(9,3);
  pin_gv.tail(12).segment(9,3)=raisim_gv.tail(12).segment(6,3);
  return pin_gv;
}
Eigen::VectorXd RaisimGaToPinocchioGa(Eigen::VectorXd raisim_gc,Eigen::VectorXd raisim_ga){
  Eigen::VectorXd pin_ga;pin_ga.setZero(18);
  raisim::Mat<3,3> rot;raisim::quatToRotMat(raisim_gc.segment(3,4),rot);
  Eigen::Matrix3d Rot; Rot.setIdentity();Rot = rot.e();

  pin_ga.head(3) = Rot.transpose()*raisim_ga.head(3); // world frame -> base local frame
  pin_ga.segment(3,3) = Rot.transpose()*raisim_ga.segment(3,3); // world frame -> base local frame
  // (FR,FL,RR,RL) -> (FL,FR,RL,RR)
  pin_ga.tail(12).segment(0,3)=raisim_ga.tail(12).segment(3,3);
  pin_ga.tail(12).segment(3,3)=raisim_ga.tail(12).segment(0,3);
  pin_ga.tail(12).segment(6,3)=raisim_ga.tail(12).segment(9,3);
  pin_ga.tail(12).segment(9,3)=raisim_ga.tail(12).segment(6,3);
  return pin_ga;
}
Eigen::VectorXd RaisimTauToPinocchioTau(Eigen::VectorXd raisim_gc,Eigen::VectorXd raisim_tau){
  Eigen::VectorXd pin_tau;pin_tau.setZero(18);
  raisim::Mat<3,3> rot;raisim::quatToRotMat(raisim_gc.segment(3,4),rot);
  Eigen::Matrix3d Rot; Rot.setIdentity();Rot = rot.e();

  pin_tau.head(3) = Rot.transpose()*raisim_tau.head(3); // world frame -> base local frame
  pin_tau.segment(3,3) = Rot.transpose()*raisim_tau.segment(3,3); // world frame -> base local frame
  // (FR,FL,RR,RL) -> (FL,FR,RL,RR)
  pin_tau.tail(12).segment(0,3)=raisim_tau.tail(12).segment(3,3);
  pin_tau.tail(12).segment(3,3)=raisim_tau.tail(12).segment(0,3);
  pin_tau.tail(12).segment(6,3)=raisim_tau.tail(12).segment(9,3);
  pin_tau.tail(12).segment(9,3)=raisim_tau.tail(12).segment(6,3);
  return pin_tau;
}
Eigen::MatrixXd RaisimMatToPinocchioMat(Eigen::VectorXd raisim_gc,Eigen::MatrixXd raisim_mat){
  Eigen::MatrixXd pin_mat1;pin_mat1.setZero(18,18);
  Eigen::MatrixXd pin_mat2;pin_mat2.setZero(18,18);

  raisim::Mat<3,3> rot;raisim::quatToRotMat(raisim_gc.segment(3,4),rot);
  Eigen::Matrix3d Rot; Rot.setIdentity();Rot = rot.e();
  // input transformation
  pin_mat1.leftCols(3) = raisim_mat.leftCols(3)*Rot;
  pin_mat1.middleCols(3,3) = raisim_mat.middleCols(3,3)*Rot;
  // (FR,FL,RR,RL) -> (FL,FR,RL,RR)
  pin_mat1.rightCols(12).middleCols(0,3) = raisim_mat.rightCols(12).middleCols(3,3);
  pin_mat1.rightCols(12).middleCols(3,3) = raisim_mat.rightCols(12).middleCols(0,3);
  pin_mat1.rightCols(12).middleCols(6,3) = raisim_mat.rightCols(12).middleCols(9,3);
  pin_mat1.rightCols(12).middleCols(9,3) = raisim_mat.rightCols(12).middleCols(6,3);

  // output transformation
  pin_mat2.topRows(3) = Rot.transpose()*pin_mat1.topRows(3);
  pin_mat2.middleRows(3,3) = Rot.transpose()*pin_mat1.middleRows(3,3);
  // (FR,FL,RR,RL) -> (FL,FR,RL,RR)
  pin_mat2.bottomRows(12).middleRows(0,3) = pin_mat1.bottomRows(12).middleRows(3,3);
  pin_mat2.bottomRows(12).middleRows(3,3) = pin_mat1.bottomRows(12).middleRows(0,3);
  pin_mat2.bottomRows(12).middleRows(6,3) = pin_mat1.bottomRows(12).middleRows(9,3);
  pin_mat2.bottomRows(12).middleRows(9,3) = pin_mat1.bottomRows(12).middleRows(6,3);

  return pin_mat2;
}

Eigen::VectorXd PinocchioGvToRaisimGv(Eigen::VectorXd pin_gc,Eigen::VectorXd pin_gv){
  Eigen::VectorXd raisim_gv;raisim_gv.setZero(18);

  Eigen::VectorXd quat_wxyz;quat_wxyz.setZero(4);
  quat_wxyz << pin_gc(6),pin_gc(3),pin_gc(4),pin_gc(5); // (x,y,z,w) -> (w,x,y,z)
  raisim::Mat<3,3> rot;raisim::quatToRotMat(quat_wxyz,rot);
  Eigen::Matrix3d Rot; Rot.setIdentity();Rot = rot.e();

  raisim_gv.head(3) = Rot*pin_gv.head(3); // base local frame -> world frame
  raisim_gv.segment(3,3) = Rot*pin_gv.segment(3,3); // base local frame -> world frame
  // (FL,FR,RL,RR) -> (FR,FL,RR,RL)
  raisim_gv.tail(12).segment(0,3)=pin_gv.tail(12).segment(3,3);
  raisim_gv.tail(12).segment(3,3)=pin_gv.tail(12).segment(0,3);
  raisim_gv.tail(12).segment(6,3)=pin_gv.tail(12).segment(9,3);
  raisim_gv.tail(12).segment(9,3)=pin_gv.tail(12).segment(6,3);
  return raisim_gv;
}

Results

/home/kdyun/Desktop/urdf2modelcasadi/urdf2model/cmake-build-debug/examples/pinocchioTest
gc log loaded
gv log loaded
ga log loaded
tau log loaded
5000 -----------------------------------------
rotation matrix by raisim
  0.995013273410479  0.0989291967982209  -0.012712189340218
-0.0972885397770921   0.990714946252576  0.0949675486668843
 0.0219892192904575 -0.0932572211285258   0.995399198785282
raisim gc 0.000653218336270782  0.00137226274068773      9877.6184323708    0.997638138110249  -0.0471675957957938 -0.00869589065039347    -0.04917056823504    0.307044744888622    0.914448918980967    -2.27717634922347   -0.295966291873608    0.930860937357035    -2.28483427159087    0.296118642944207     1.36959496564128    -2.27861049519463   -0.357939130502407     1.46872923422833    -2.32319648770892
raisim gv -0.0164282059769289 0.00518747479139204    -49.023875871426   0.215393903695747   0.389743407747753  -0.080197234264595   0.577028695418999  -0.683317797788219  0.0195572050931019  0.0553493371968555   -1.46853637519625   0.273117587767061  -0.509071779238783   -2.48421710414137  -0.280717514789893   -1.32425012047053   -2.65101540345195   -1.40574433347683
raisim ga -0.173312591481154 -0.252064333898804  -9.63695123969188  -22.0547541180422   9.48409512954967   1.30555730919958   4.97068854730276   1.21515620053914  -2.88591892144991   27.8736641297344  -8.36275155496491   9.94494845798488    49.657566360661  -25.4499537908539   6.48374389719575   141.954771867411  -130.538215920757   53.8813411212549
raisim tau                   0                   0                   0                   0                   0                   0  -0.251296729948159   0.156611776413461 -0.0325982308335908 -0.0744348413727838 -0.0224516184887041   0.128655244751776   0.175130623965581  -0.203648861022478 -0.0542070998739904   0.776701702735531   -1.18410913242876  -0.274019607737757
raisim M to pin
            11.309932 -1.14491749414469e-16 -5.55111512312578e-17  2.62919026827735e-18    -0.235275614196089   -0.0218829745737257  6.50521303491303e-19   -0.0487368712376508  -0.00593565867229854 -4.33680868994202e-19   -0.0494838137858627   -0.0056591343405866  4.33680868994202e-19   -0.0249571560608876   -0.0203285812890398 -4.33680868994202e-19   -0.0310494260879338   -0.0189427854649564
 1.07552855510562e-16             11.309932  2.22044604925031e-16     0.235275614196089                     0    -0.105772933056775    0.0676590036912417   0.00908605217662198  -0.00920562900934481    0.0688491142621062  -0.00918540876975114   0.00955485615548751    0.0486505290802055    0.0178409753311979  -0.00871060820330564     0.050629197135231   -0.0143052561834123   0.00756789421571252
-2.77555756156289e-17  2.22044604925031e-16             11.309932    0.0218829745737257     0.105772933056775                     0    0.0548007893988291    0.0297979511360053   -0.0301901065572168   -0.0534283934749487    0.0289694653765602   -0.0301346495853236    0.0588363489470588    0.0476965338974533   -0.0232871696599396   -0.0595567802997716    0.0468888633279078   -0.0248055646827265
-2.62919026827735e-18     0.235275614196089    0.0218829745737257     0.102021730158946  0.000488769286772206   -0.0041283824367645    0.0162992971400164   0.00343677653127648  -0.00393303092461733    0.0163412896498388   -0.0032650853931813   0.00394120274637957    0.0137510783732825   0.00578373629673001  -0.00307502865174099    0.0144800265554885  -0.00550417022560986   0.00323719871145339
   -0.235275614196089                     0     0.105772933056775  0.000488769286772195     0.348070082465893 -0.000260988601303885  -0.00909958820585543    0.0102427060476686   0.00972871025102235   0.00899417401870013    0.0104597374099598   0.00974030914063295    0.0139652682167315    0.0237972985158262 -0.000158493847881934   -0.0138404222412221    0.0243733293810722 -0.000281700298820506
  -0.0218829745737257    -0.105772933056775                     0   -0.0041283824367645 -0.000260988601303892     0.372718681727794   0.00971637153331962   0.00295604407368442  -0.00219312960680936   0.00999064899986954  -0.00281053577472222   0.00234834018606862   -0.0115753075206879  -0.00559909009094648   0.00274376406959104   -0.0122613027422822   0.00345196439883632  -0.00255791343197508
 6.50521303491303e-19    0.0676590036912417    0.0548007893988291    0.0162992971400164  -0.00909958820585543   0.00971637153331962    0.0137373602356001    0.0020437223156648  -0.00252164344306397                     0                     0                     0                     0                     0                     0                     0                     0                     0
  -0.0487368712376508   0.00908605217662198    0.0297979511360053   0.00343677653127648    0.0102427060476686   0.00295604407368442    0.0020437223156648    0.0154595036990023   0.00408941477559803                     0                     0                     0                     0                     0                     0                     0                     0                     0
 -0.00593565867229854  -0.00920562900934481   -0.0301901065572168  -0.00393303092461733   0.00972871025102235  -0.00219312960680936  -0.00252164344306397   0.00408941477559803   0.00872703343113965                     0                     0                     0                     0                     0                     0                     0                     0                     0
-4.33680868994202e-19    0.0688491142621062   -0.0534283934749487    0.0163412896498388   0.00899417401870013   0.00999064899986954                     0                     0                     0    0.0138435122549267  -0.00191076288683135   0.00253240787827011                     0                     0                     0                     0                     0                     0
  -0.0494838137858627  -0.00918540876975114    0.0289694653765602   -0.0032650853931813    0.0104597374099598  -0.00281053577472222                     0                     0                     0  -0.00191076288683135    0.0155367923012737   0.00412805907678121                     0                     0                     0                     0                     0                     0
  -0.0056591343405866   0.00955485615548751   -0.0301346495853236   0.00394120274637957   0.00974030914063295   0.00234834018606862                     0                     0                     0   0.00253240787827011   0.00412805907678121   0.00872703343113634                     0                     0                     0                     0                     0                     0
 4.33680868994202e-19    0.0486505290802055    0.0588363489470588    0.0137510783732825    0.0139652682167315   -0.0115753075206879                     0                     0                     0                     0                     0                     0     0.011000479059972   0.00355392333700946  -0.00198635347013168                     0                     0                     0
  -0.0249571560608876    0.0178409753311979    0.0476965338974533   0.00578373629673001    0.0237972985158262  -0.00559909009094648                     0                     0                     0                     0                     0                     0   0.00355392333700946    0.0150806053408847   0.00389996559657653                     0                     0                     0
  -0.0203285812890398  -0.00871060820330564   -0.0232871696599396  -0.00307502865174099 -0.000158493847881934   0.00274376406959104                     0                     0                     0                     0                     0                     0  -0.00198635347013168   0.00389996559657653   0.00872703343113771                     0                     0                     0
-4.33680868994202e-19     0.050629197135231   -0.0595567802997716    0.0144800265554885   -0.0138404222412221   -0.0122613027422822                     0                     0                     0                     0                     0                     0                     0                     0                     0    0.0116957470764872  -0.00331211586503303   0.00207753856253745
  -0.0310494260879338   -0.0143052561834123    0.0468888633279078  -0.00550417022560986    0.0243733293810722   0.00345196439883632                     0                     0                     0                     0                     0                     0                     0                     0                     0  -0.00331211586503303    0.0155222768054771   0.00412080132889465
  -0.0189427854649564   0.00756789421571252   -0.0248055646827265   0.00323719871145339 -0.000281700298820506  -0.00255791343197508                     0                     0                     0                     0                     0                     0                     0                     0                     0   0.00207753856253745   0.00412080132889465   0.00872703343110606
raisim h to pin
  2.65799605655583  -10.5673763834764    111.09302042108 0.0347454660919724   1.13063297304008  0.255930467683892   0.48163862272246  0.270920370410725 -0.293611371002356 -0.583018001990172  0.283879935360434 -0.307710828451623  0.564738564836899   0.48537426358684 -0.254790992781442 -0.648721304205201  0.470680377814726 -0.276892912816715
raisim error -5.85280435227986e-11 -5.78434827813723e-10 -1.92699189938139e-11 -5.74865155478221e-12 -1.17734710869399e-11 -3.17744719424695e-11  5.21360732363974e-13  3.20021786848201e-14  1.32935329411055e-13 -5.28271870692265e-13  7.60242563346836e-13  1.61842761414732e-13 -5.76538816687844e-13  7.14234227316979e-13  4.81739648172663e-13  1.51811896387244e-12 -2.47801779096335e-12  1.48814294220756e-12
raisim error norm 5.82733128062276e-10

5000 -----------------------------------------
pin gc 0.000653218336270782  0.00137226274068773      9877.6184323708  -0.0471675957957938 -0.00869589065039347    -0.04917056823504    0.997638138110249   -0.295966291873608    0.930860937357035    -2.28483427159087    0.307044744888622    0.914448918980967    -2.27717634922347   -0.357939130502407     1.46872923422833    -2.32319648770892    0.296118642944207     1.36959496564128    -2.27861049519463
pin gv    -1.0948477218579    4.57534451230598    -48.797625283537   0.174638751590708   0.414912336356674 -0.0455534147754004  0.0553493371968555   -1.46853637519625   0.273117587767061   0.577028695418999  -0.683317797788219  0.0195572050931019   -1.32425012047053   -2.65101540345195   -1.40574433347683  -0.509071779238783   -2.48421710414137  -0.280717514789893
pin ga -0.359834392099047  0.631845714283773   -9.6143482921409  -22.8387586695491   7.09242303936596    2.4805961755229   27.8736641297344  -8.36275155496491   9.94494845798488   4.97068854730276   1.21515620053914  -2.88591892144991   141.954771867411  -130.538215920757   53.8813411212549    49.657566360661  -25.4499537908539   6.48374389719575
pin tau                   0                   0                   0                   0                   0                   0 -0.0744348413727838 -0.0224516184887041   0.128655244751776  -0.251296729948159   0.156611776413461 -0.0325982308335908   0.776701702735531   -1.18410913242876  -0.274019607737757   0.175130623965581  -0.203648861022478 -0.0542070998739904
crba_local 
            11.309932                     0                     0                    -0    -0.235275614166936   -0.0218829745709944                     0   -0.0487368712357578  -0.00593565867227143                     0   -0.0494838137852069  -0.00565913434063116                     0   -0.0249571560594591   -0.0203285812890434                     0   -0.0310494260875778   -0.0189427854651481
                    0             11.309932                     0     0.235275614166936                    -0    -0.105772933056131    0.0676590036915489   0.00908605217660885    -0.009205629009345    0.0688491142613654  -0.00918540876974668   0.00955485615548719    0.0486505290799364    0.0178409753311856  -0.00871060820330561    0.0506291971340853     -0.01430525618341   0.00756789421571126
                    0                     0             11.309932    0.0218829745709944     0.105772933056131                    -0    0.0548007893988578    0.0297979511359622   -0.0301901065572174    -0.053428393475018    0.0289694653765462   -0.0301346495853226    0.0588363489470336    0.0476965338974206   -0.0232871696599395    -0.059556780299879    0.0468888633279001   -0.0248055646827224
                   -0     0.235275614166936    0.0218829745709944     0.102021730158794  0.000488769286726896  -0.00412838243631476    0.0162992971399589   0.00343677653124918  -0.00393303092461638    0.0163412896498475  -0.00326508539318068   0.00394120274638896    0.0137510783731768   0.00578373629667104  -0.00307502865173439    0.0144800265555144  -0.00550417022561548   0.00323719871146359
   -0.235275614166936                    -0     0.105772933056131  0.000488769286726896      0.34807008246575 -0.000260988601458878  -0.00909958820586208     0.010242706047472   0.00972871025101735   0.00899417401871155    0.0104597374099489   0.00974030914064754    0.0139652682167216     0.023797298515696 -0.000158493847893426   -0.0138404222412435    0.0243733293811505 -0.000281700298753565
  -0.0218829745709944    -0.105772933056131                    -0  -0.00412838243631476 -0.000260988601458878     0.372718681727746   0.00971637153338379   0.00295604407349201   -0.0021931296068116   0.00999064899974468  -0.00281053577465585   0.00234834018606568   -0.0115753075206001  -0.00559909009109118   0.00274376406959005   -0.0122613027420431   0.00345196439888347  -0.00255791343198766
                    0    0.0676590036915489    0.0548007893988578    0.0162992971399589  -0.00909958820586208   0.00971637153338379    0.0137373602355623   0.00204372231564295  -0.00252164344306647                     0                     0                     0                     0                     0                     0                     0                     0                     0
  -0.0487368712357578   0.00908605217660885    0.0297979511359622   0.00343677653124918     0.010242706047472   0.00295604407349201   0.00204372231564295    0.0154595036988387    0.0040894147755937                     0                     0                     0                     0                     0                     0                     0                     0                     0
 -0.00593565867227143    -0.009205629009345   -0.0301901065572174  -0.00393303092461638   0.00972871025101735   -0.0021931296068116  -0.00252164344306647    0.0040894147755937   0.00872703343113835                     0                     0                     0                     0                     0                     0                     0                     0                     0
                    0    0.0688491142613654    -0.053428393475018    0.0163412896498475   0.00899417401871155   0.00999064899974468                     0                     0                     0    0.0138435122548904  -0.00191076288682715   0.00253240787827513                     0                     0                     0                     0                     0                     0
  -0.0494838137852069  -0.00918540876974668    0.0289694653765462  -0.00326508539318068    0.0104597374099489  -0.00281053577465585                     0                     0                     0  -0.00191076288682715    0.0155367923012341   0.00412805907679137                     0                     0                     0                     0                     0                     0
 -0.00565913434063116   0.00955485615548719   -0.0301346495853226   0.00394120274638896   0.00974030914064754   0.00234834018606568                     0                     0                     0   0.00253240787827513   0.00412805907679137   0.00872703343113835                     0                     0                     0                     0                     0                     0
                    0    0.0486505290799364    0.0588363489470336    0.0137510783731768    0.0139652682167216   -0.0115753075206001                     0                     0                     0                     0                     0                     0     0.011000479059903   0.00355392333696663  -0.00198635347013221                     0                     0                     0
  -0.0249571560594591    0.0178409753311856    0.0476965338974206   0.00578373629667104     0.023797298515696  -0.00559909009109118                     0                     0                     0                     0                     0                     0   0.00355392333696663    0.0150806053408224   0.00389996559658553                     0                     0                     0
  -0.0203285812890434  -0.00871060820330561   -0.0232871696599395  -0.00307502865173439 -0.000158493847893426   0.00274376406959005                     0                     0                     0                     0                     0                     0  -0.00198635347013221   0.00389996559658553   0.00872703343113835                     0                     0                     0
                    0    0.0506291971340853    -0.059556780299879    0.0144800265555144   -0.0138404222412435   -0.0122613027420431                     0                     0                     0                     0                     0                     0                     0                     0                     0     0.011695747076495  -0.00331211586503615   0.00207753856254632
  -0.0310494260875778     -0.01430525618341    0.0468888633279001  -0.00550417022561548    0.0243733293811505   0.00345196439888347                     0                     0                     0                     0                     0                     0                     0                     0                     0  -0.00331211586503615    0.0155222768055448   0.00412080132894676
  -0.0189427854651481   0.00756789421571126   -0.0248055646827224   0.00323719871146359 -0.000281700298753565  -0.00255791343198766                     0                     0                     0                     0                     0                     0                     0                     0                     0   0.00207753856254632   0.00412080132894676   0.00872703343113835
nle 
  -223.973974360336     86.379442697405    125.267738743115    2.07891402531738    5.97772467632028  -0.212239257743252    1.13028186333887     1.3627549261415   -0.29141707717605 -0.0598167841341922    1.23302375615015  -0.150176414635887    1.05550214452269    1.19818139579147  0.0487078792982909   -0.28937882276054    1.02900203364219   0.136470507432689
pin error   -226.631970416892    96.9468190808832    14.1747183220155    2.04416855922541    4.84709170328002  -0.468169725427154   0.648643240616328    1.09183455573073 0.00219429382635458   0.523201217856082    0.94914382078967   0.157534413815788   0.490763579685702   0.712807132204546   0.303498872079771   0.359342481444768   0.558321655827387   0.413363420249449
pin error norm 246.96926031484
M diff 
5.87249918842271e-11
h diff 
  -226.631970416892    96.9468190808815    14.1747183220347    2.04416855922541    4.84709170328019  -0.468169725427144    0.64864324061641    1.09183455573078 0.00219429382630565    0.52320121785598   0.949143820789718   0.157534413815736   0.490763579685792   0.712807132204625   0.303498872079733    0.35934248144466   0.558321655827466   0.413363420249404
aba  19.6784797566414 -7.93998469821407 -10.8676465720965 -22.8387586695519  7.09242303936558  2.48059617552313  27.8736641297296 -8.36275155496315  9.94494845798136  4.97068854729946  1.21515620053036 -2.88591892144259  141.954771867414 -130.538215920754  53.8813411212544  49.6575663606676 -25.4499537908453   6.4837438971947
aba error1      20.0383141487404     -8.57183041249785     -1.25329827995564 -2.78177481050079e-12 -3.75699471533153e-13  2.20712337295481e-13 -4.78195261166547e-12  1.75681691416685e-12  -3.5189628988519e-12 -3.30047100760567e-12 -8.78119799097021e-12  7.32169880279798e-12  2.89901436190121e-12  2.64321897702757e-12 -5.32907051820075e-13  6.57252030578093e-12  8.58690896166081e-12 -1.05071507050525e-12
aba error2     226.631970416892    -96.9468190808832    -14.1747183220154    -2.04416855922563    -4.84709170327995    0.468169725427126   -0.648643240616415    -1.09183455573074 -0.00219429382636111   -0.523201217856135   -0.949143820789764   -0.157534413815784   -0.490763579685701   -0.712807132204524   -0.303498872079762   -0.359342481444758   -0.558321655827272   -0.413363420249418

Process finished with exit code 0

Does anyone have insights into the possible cause of this issue? I would greatly appreciate any assistance in solving this mystery.

Thank you.

Answered by kdyun0118

Aug 7, 2024

Hello,
I found that adding the derivative of the rotation matrix term resolved the errors.

Here's the corrected conversion function:

Eigen::VectorXd RaisimGaToPinocchioGa(Eigen::VectorXd raisim_gc,Eigen::VectorXd raisim_gv,Eigen::VectorXd raisim_ga){
  Eigen::VectorXd pin_ga;pin_ga.setZero(18);
  raisim::Mat<3,3> rot;raisim::quatToRotMat(raisim_gc.segment(3,4),rot);
  Eigen::Matrix3d Rot,dRot;
  Rot.setIdentity(); dRot.setIdentity();
  Rot = rot.e(); dRot = skewMat(raisim_gv.segment(3,3))*Rot;

  pin_ga.head(3) = Rot.transpose()*raisim_ga.head(3) + dRot.transpose()*raisim_gv.head(3); // world frame -> base local frame
  pin_ga.segment(3,3) = Rot.transpose()*raisim_ga.segment(3,3)+ dRot.tr…

View full answer

kdyun0118 · 2024-08-07T01:31:51Z

kdyun0118
Aug 7, 2024
Author

Hello,
I found that adding the derivative of the rotation matrix term resolved the errors.

Here's the corrected conversion function:

Eigen::VectorXd RaisimGaToPinocchioGa(Eigen::VectorXd raisim_gc,Eigen::VectorXd raisim_gv,Eigen::VectorXd raisim_ga){
  Eigen::VectorXd pin_ga;pin_ga.setZero(18);
  raisim::Mat<3,3> rot;raisim::quatToRotMat(raisim_gc.segment(3,4),rot);
  Eigen::Matrix3d Rot,dRot;
  Rot.setIdentity(); dRot.setIdentity();
  Rot = rot.e(); dRot = skewMat(raisim_gv.segment(3,3))*Rot;

  pin_ga.head(3) = Rot.transpose()*raisim_ga.head(3) + dRot.transpose()*raisim_gv.head(3); // world frame -> base local frame
  pin_ga.segment(3,3) = Rot.transpose()*raisim_ga.segment(3,3)+ dRot.transpose()*raisim_gv.segment(3,3); // world frame -> base local frame
  // (FR,FL,RR,RL) -> (FL,FR,RL,RR)
  pin_ga.tail(12).segment(0,3)=raisim_ga.tail(12).segment(3,3);
  pin_ga.tail(12).segment(3,3)=raisim_ga.tail(12).segment(0,3);
  pin_ga.tail(12).segment(6,3)=raisim_ga.tail(12).segment(9,3);
  pin_ga.tail(12).segment(9,3)=raisim_ga.tail(12).segment(6,3);
  return pin_ga;
}

This resolved the remaining errors in my project. I hope this helps anyone facing the same issue.

0 replies

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Discrepancies in Dynamics Computation between Raisim and Pinocchio for Quadruped Simulation #2355

{{title}}

{{editor}}'s edit

{{editor}}'s edit

Replies: 1 comment

{{title}}

Select a reply

Discrepancies in Dynamics Computation between Raisim and Pinocchio for Quadruped Simulation #2355

kdyun0118 Jul 30, 2024

Replies: 1 comment

kdyun0118 Aug 7, 2024 Author

kdyun0118
Jul 30, 2024

kdyun0118
Aug 7, 2024
Author