dynamics-autodiff-test.cpp

#include "pinocchio/autodiff/casadi.hpp"

#include "pinocchio/algorithm/aba.hpp"       // joint acceleration, Minverse
#include "pinocchio/algorithm/crba.hpp"      // joint-space inertia (upper-triangle)
#include "pinocchio/algorithm/jacobian.hpp"  // Jacobian
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/rnea.hpp"  // Coriolis, gravity, joint torque

#include "pinocchio/parsers/urdf.hpp"

#include <iostream>

int main(int argc, char ** argv)
{
  using namespace pinocchio;

  //* typedef for CasADi Auto-Diff *//
  typedef double Scalar;
  typedef ::casadi::SX ADScalar;

  typedef ModelTpl<Scalar> Model;
  typedef Model::Data Data;

  typedef ModelTpl<ADScalar> ADModel;
  typedef ADModel::Data ADData;

  typedef ADModel::ConfigVectorType ConfigVectorAD;
  typedef ADModel::TangentVectorType TangentVectorAD;

  //* load Franka Research 3 URDF file *//
  const std::string urdf_filename = std::string("../model/fr3.urdf");

  Model model;
  pinocchio::urdf::buildModel(urdf_filename, model);
  // std::cout << "model name: " << model.name << std::endl;
  Data data(model);

  // set end-effector's ID (joint 8)
  const std::string ee_name = "fr_joint8";
  JointIndex ee_joint_id = model.getJointId(ee_name);
  if (ee_joint_id == 0)
  {
    std::cerr << "Invalid joint ID for end-effector: " << ee_name << std::endl;
    return -1;
  }
  else
  {
    std::cout << "End-effector ID: " << ee_joint_id << std::endl;  // 8
  }

  //* Pick up random configuration, velocity and acceleration vectors. *//
  Eigen::VectorXd q = randomConfiguration(model);
  Eigen::VectorXd v(Eigen::VectorXd::Random(model.nv));
  Eigen::VectorXd a(Eigen::VectorXd::Random(model.nv));

  std::cout << "q: " << q.transpose() << std::endl;
  std::cout << "v: " << v.transpose() << std::endl;
  std::cout << "a: " << a.transpose() << std::endl << std::endl;

  //* Create CasADi model and data from Pinocchio model *//
  ADModel ad_model = model.cast<ADScalar>();
  ADData ad_data(ad_model);

  //* Create symbolic CasADi vectors & Matices *//
  // joint configuration
  ::casadi::SX cs_q = ::casadi::SX::sym("q", model.nq);
  ConfigVectorAD q_ad(model.nq);
  q_ad = Eigen::Map<ConfigVectorAD>(static_cast<std::vector<ADScalar>>(cs_q).data(), model.nq, 1);

  // joint velocity
  ::casadi::SX cs_v = ::casadi::SX::sym("v", model.nv);
  TangentVectorAD v_ad(model.nv);
  v_ad = Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_v).data(), model.nv, 1);

  // joint acceleration
  ::casadi::SX cs_a = ::casadi::SX::sym("a", model.nv);
  TangentVectorAD a_ad(model.nv);
  a_ad = Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_a).data(), model.nv, 1);

  // Upper-triangular Mass Matrix
  ::casadi::SX cs_M = ::casadi::SX::zeros(model.nv, model.nv);
  // ::casadi::SX cs_M_dot = ::casadi::SX::zeros(model.nv, model.nv);
  ::casadi::SX cs_Minv = ::casadi::SX::zeros(model.nv, model.nv);
  ::casadi::SX cs_Minv_dot = ::casadi::SX::zeros(model.nv, model.nv);

  // end-effector Jacobian Matrix
  ::casadi::SX cs_J = ::casadi::SX::zeros(6, model.nv);
  ::casadi::SX cs_J_dot = ::casadi::SX::zeros(6, model.nv);

  //* Create CasADi "symbolic" function *//
  /* forward kinematics */
  // "Numerical" forward kinematics
  // ! joint : universe, joint1, joint2, ... joint7 => selecting joint8 would cause segmentation
  // error pinocchio::forwardKinematics(model, data, q); for (JointIndex joint_id = 0; joint_id <
  // (JointIndex) model.njoints; ++joint_id)
  // {
  //   std::cout << std::setw(24) << std::left
  //             << model.names[joint_id] << ": "
  //             << std::fixed << std::setprecision(2)
  //             << data.oMi[joint_id].translation().transpose()
  //             << std::endl;
  // }

  // "Symbolic" forward kinematics
  // pinocchio::forwardKinematics(ad_model, ad_data, q_ad);
  // for (JointIndex joint_id = 0; joint_id < (JointIndex) ad_model.njoints; ++joint_id)
  // {
  //   std::cout << std::setw(24) << std::left
  //             << ad_model.names[joint_id] << ": "
  //             << std::fixed << std::setprecision(2)
  //             << ad_data.oMi[joint_id].translation().transpose()
  //             << std::endl;
  // }

  /* CRBA algorithm : compute Mass Matrix */
  pinocchio::crba(ad_model, ad_data, q_ad);
  for (int i = 0; i < model.nv; ++i)
  {
    for (int j = 0; j < model.nv; ++j)
    {
      cs_M(i, j) = ad_data.M(i, j);  // copy symbolic M(q) into CasADi matrix
    }
  }
  // std::cout << "Symbolic mass matrix:\n" << cs_M << std::endl;

  // derivative of M w.r.t time
  // for (int i = 0; i < model.nv; ++i)
  // {
  //   for (int j = 0; j < model.nv; ++j)
  //   {
  //     ::casadi::SX dMij_dq = jacobian(cs_M(i,j), cs_q); // extract derivative of M[i, j] w.r.t
  //     all q cs_M_dot(i, j) = mtimes(dMij_dq, cs_v);           // multiply by v and sum over nq
  //   }
  // }

  /* compute derivative of Inverse Mass Matrix */
  pinocchio::computeMinverse(ad_model, ad_data, q_ad);
  for (int i = 0; i < model.nv; ++i)
  {
    for (int j = 0; j < model.nv; ++j)
    {
      cs_Minv(i, j) = ad_data.Minv(i, j);  // copy symbolic M(q) into CasADi matrix
    }
  }

  for (int i = 0; i < model.nv; ++i)
  {
    for (int j = 0; j < model.nv; ++j)
    {
      ::casadi::SX dMinv_dq = jacobian(cs_Minv(i, j), cs_q);
      cs_Minv_dot(i, j) = mtimes(dMinv_dq, cs_v);
    }
  }

  /* compute derivative of (end-effector) Jacobian matrix */
  // !!!!! Segmentation fault
  // Eigen::Matrix<ADScalar, 6, Eigen::Dynamic> J(6, model.nv);
  // Data::Matrix6x J(6, model.nv);
  // J.setZero();
  // pinocchio::computeJointJacobian(model, data, q, ee_joint_id, J);
  // pinocchio::computeJointJacobian(ad_model, ad_data, q_ad, ee_joint_id, J);
  // pinocchio::computeJointJacobian(ad_model, ad_data, q_ad, ee_joint_id, ad_data.J);
  // std::cout << "Jacobian J:\n" << J << std::endl;

  // compute "Numerical" Jacobian
  // pinocchio::computeJointJacobians(model, data, q);
  // std::cout << "Jacobian J:\n" << data.J << std::endl;

  // compute "Symbolic" Jacobian
  pinocchio::computeJointJacobians(ad_model, ad_data, q_ad);
  for (int i = 0; i < 6; ++i)
  {
    for (int j = 0; j < model.nv; ++j)
    {
      cs_J(i, j) = ad_data.J(i, j);  // copy symbolic J(q) into CasADi matrix
    }
  }
  // std::cout << "Symbolic mass matrix:\n" << cs_J << std::endl;

  // Jacobian derivative
  for (int i = 0; i < 6; ++i)
  {
    for (int j = 0; j < model.nv; ++j)
    {
      ::casadi::SX dJij_dq =
        jacobian(cs_J(i, j), cs_q);            // extract derivative of J[i, j] w.r.t all q
      cs_J_dot(i, j) = mtimes(dJij_dq, cs_v);  // multiply by v and sum over nq
    }
  }

  /* Inverse Dynamics -> joint torque */
  pinocchio::rnea(ad_model, ad_data, q_ad, v_ad, a_ad);
  ::casadi::SX tau_ad(model.nv, 1);
  for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
  {
    tau_ad(k) = ad_data.tau[k];
  }

  //* Build (symbolic) CasADi function for evaluation *//
  ::casadi::Function eval_rnea("eval_rnea", ::casadi::SXVector{ cs_q, cs_v, cs_a },
                               ::casadi::SXVector{ tau_ad });

  ::casadi::Function eval_Minv_dot("eval_Minv_dot", ::casadi::SXVector{ cs_q, cs_v },
                                   ::casadi::SXVector{ cs_Minv_dot });
  // std::cout << cs_Minv_dot << std::endl;

  ::casadi::Function eval_Jdot("eval_Jdot", ::casadi::SXVector{ cs_q, cs_v },
                               ::casadi::SXVector{ cs_J_dot });
  // std::cout << cs_J_dot << std::endl;

  //* Evaluate CasADi expression with real value *//
  std::vector<double> q_vec((size_t)model.nq);
  Eigen::Map<Eigen::VectorXd>(q_vec.data(), model.nq, 1) = q;

  std::vector<double> v_vec((size_t)model.nv);
  Eigen::Map<Eigen::VectorXd>(v_vec.data(), model.nv, 1) = v;

  std::vector<double> a_vec((size_t)model.nv);
  Eigen::Map<Eigen::VectorXd>(a_vec.data(), model.nv, 1) = a;

  // Inverse dynamics
  ::casadi::DM tau_casadi_res = eval_rnea(::casadi::DMVector{ q_vec, v_vec, a_vec })[0];
  Data::TangentVectorType tau_casadi_vec = Eigen::Map<Data::TangentVectorType>(
    static_cast<std::vector<double>>(tau_casadi_res).data(), model.nv, 1);

  pinocchio::rnea(model, data, q, v, a);
  std::cout << "pinocchio double:\n"
            << "\ttau = " << data.tau.transpose() << std::endl;
  std::cout << "pinocchio CasADi:\n"
            << "\ttau = " << tau_casadi_vec.transpose() << std::endl;

  // Minv_dot
  ::casadi::DM Minv_dot_res = eval_Minv_dot(::casadi::DMVector{ q_vec, v_vec })[0];
  std::cout << "(CasADi) Minv_dot:" << Minv_dot_res << std::endl << std::endl;

  // J_dot
  ::casadi::DM Jdot_res = eval_Jdot(::casadi::DMVector{ q_vec, v_vec })[0];
  std::cout << "(CasADi) Jdot:" << Jdot_res << std::endl << std::endl;

  pinocchio::computeJointJacobiansTimeVariation(model, data, q, v);
  std::cout << "(pinocchio) Jdot:\n" << data.dJ << std::endl << std::endl;
}