joaoleal / CppADCodeGen

Source Code Generation for Automatic Differentiation using Operator Overloading
Other
162 stars 36 forks source link

error when complie function library #86

Open Czworldy opened 6 months ago

Czworldy commented 6 months ago

Hi guys! I tired to use some pinocchio dynamics function in my code, and i try to use cppadcodegen to get jacobian matrix of my function.

  ad_vector_t qPinocchio = input.head(q_size);
  ad_vector_t vPinocchio = input.tail(v_size);

  pinocchio::forwardKinematics(model, data, qPinocchio, vPinocchio);
  pinocchio::computeJointJacobians(model, data);
  pinocchio::updateFramePlacements(model, data);
  pinocchio::crba(model, data, qPinocchio);
  // data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();

  ad_matrix_t jacobian = ad_matrix_t::Zero(6, model.nv), jacobianXyz = ad_matrix_t::Zero(3, model.nv);

  pinocchio::getFrameJacobian(model, data, 
    model.getBodyId("foot"), pinocchio::LOCAL_WORLD_ALIGNED, jacobian);
  jacobianXyz = jacobian.topRows(3);
  ad_matrix_t Minv = pinocchio::computeMinverse(model, data, qPinocchio);
  Minv.triangularView<Eigen::StrictlyLower>() = Minv.transpose().triangularView<Eigen::StrictlyLower>();

  Eigen::Matrix<ad_scalar_t, 3, 3> JMinvJt = jacobianXyz * Minv * jacobianXyz.transpose() + ad_scalar_t(1e-12) * ad_matrix_t::Identity(3, 3);

  ad_vector_t output = JMinvJt.inverse() * (-ad_scalar_t(config_.r_coeff)-ad_scalar_t(1.0)) * jacobianXyz * vPinocchio;

but when i complie the .c file it gets error, is this cause by my function? do you guys have any idea when using cppadcodegen with pinocchio?

<stdin>:64:12: error: wrong type argument to unary minus
<stdin>:64:27: error: wrong type argument to unary minus
<stdin>:64:42: error: ‘inf’ undeclared (first use in this function); did you mean ‘in’?
<stdin>:64:42: note: each undeclared identifier is reported only once for each function it appears in

Thanks a lot!