Does this function pinocchio::impulseDynamics support automatic code generation via CppADCodeGen?

[Czworldy]

I tried to generate the code of function pinocchio::impulseDynamics but i got runtime error

terminate called after throwing an instance of 'CppAD::cg::CGException'
  what():  GreaterThanOrZero cannot be called for non-parameters

so this is function supports CppADCodeGen? or i miss some setup? And this there any function table can tell us which function support CppADCodeGen and which doesn't?

Thanks a lot!

[Czworldy]

also i tried computeMinverse, it can not use, too. but those follwing function succeed.

  pinocchio::forwardKinematics(model, data, qPinocchio, vPinocchio);
  pinocchio::computeJointJacobians(model, data);
  pinocchio::updateFramePlacements(model, data);
  pinocchio::crba(model, data, qPinocchio);

[ManifoldFR]

Hello mate,

Thanks for opening an issue. We can't exactly reproduce it - could you provide a complete minimum working example (a snippet of code that runs) with your robot model?

On Thu, 14 Dec 2023, 20:35 Jiyu Yu, @.***> wrote:

also i tried computeMinverse, it will can not use too. but those follwing function succeed.

pinocchio::forwardKinematics(model, data, qPinocchio, vPinocchio); pinocchio::computeJointJacobians(model, data); pinocchio::updateFramePlacements(model, data); pinocchio::crba(model, data, qPinocchio);

— Reply to this email directly, view it on GitHub https://github.com/stack-of-tasks/pinocchio/issues/2121#issuecomment-1856462454, or unsubscribe https://github.com/notifications/unsubscribe-auth/AFA427HGWEXVHYPA2FTLZUTYJNIGJAVCNFSM6AAAAABAVLRKCCVHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMYTQNJWGQ3DENBVGQ . You are receiving this because you are subscribed to this thread.Message ID: @.***>

[Czworldy]

@ManifoldFR yes, sure my robot model is anymal_c a 4-legged robot with floating base. and i build model from urdf and i try to use cppad to generate the following code:

using ad_base_t = CppAD::cg::CG<scalar_t>;
using ad_scalar_t = CppAD::AD<ad_base_t>;
using ad_vector_t = Eigen::Matrix<ad_scalar_t, Eigen::Dynamic, 1>;
using ad_matrix_t = Eigen::Matrix<ad_scalar_t, Eigen::Dynamic, Eigen::Dynamic>;

ad_vector_t my_function(const ad_vector_t& input)
{
  // the input of the function is stack of q and v
  ad_vector_t qPinocchio = input.head(qPinocchio.rows());
  ad_vector_t vPinocchio = input.head(vPinocchio.rows());
  pinocchio::forwardKinematics(model, data, qPinocchio, vPinocchio); 
  pinocchio::computeJointJacobians(model, data);
  pinocchio::updateFramePlacements(model, data);
  pinocchio::crba(model, data, qPinocchio);

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

  pinocchio::getFrameJacobian(model, data, model.getBodyId("FOOT"), pinocchio::LOCAL_WORLD_ALIGNED, jacobian);
  jacobianXyz = jacobian.topRows(3);
  pinocchio::impulseDynamics(model, data, vPinocchio, jacobianXyz, ad_scalar_t(0.1), ad_scalar_t(0.0));
  ad_vector_t impulse = data.impulse_c; // impulseDynamics compute impulse and save it in data.impulse_c

  return impulse; // i want to get the impulse by impulseDynamics as the function output
}

[jcarpent]

Could you provide a full self contained example ?

[Czworldy]

sorry i can not give u guys a code can really running, it is in a large robotics project, but here is some code i copy from the project, i think this can make the question clear. in fact i use those code to generate lots of function of pinocchio, it works well in my robot, but today, i first attempt to using impulseDynamics function and it gets error.

using ad_base_t = CppAD::cg::CG<scalar_t>;
using ad_scalar_t = CppAD::AD<ad_base_t>;
using ad_vector_t = Eigen::Matrix<ad_scalar_t, Eigen::Dynamic, 1>;
using ad_matrix_t = Eigen::Matrix<ad_scalar_t, Eigen::Dynamic, Eigen::Dynamic>;

void my_function(const ad_vector_t& input, ad_vector_t& output)
{
  // the input of the function is stack of q and v
  ad_vector_t qPinocchio = input.head(qPinocchio.rows());
  ad_vector_t vPinocchio = input.head(vPinocchio.rows());
  pinocchio::forwardKinematics(model, data, qPinocchio, vPinocchio); 
  pinocchio::computeJointJacobians(model, data);
  pinocchio::updateFramePlacements(model, data);
  pinocchio::crba(model, data, qPinocchio);

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

  pinocchio::getFrameJacobian(model, data, model.getBodyId("FOOT"), pinocchio::LOCAL_WORLD_ALIGNED, jacobian);
  jacobianXyz = jacobian.topRows(3);
  pinocchio::impulseDynamics(model, data, vPinocchio, jacobianXyz, ad_scalar_t(0.1), ad_scalar_t(0.0));
  ad_vector_t output = data.impulse_c; // impulseDynamics compute impulse and save it in data.impulse_c

  //return output; // i want to get the impulse by impulseDynamics as the function output
}

//the following is where i use cppad:
{
  ad_vector_t xp(variableDim_);
  xp.setOnes();  // Ones are better than zero, to prevent devision by zero in taping
  CppAD::Independent(xp);

  my_function(xp, y);
  rangeDim_ = y.rows();
  // create f: xp -> y and stop tape recording
  ad_fun_t fun(xp, y);
  // Optimize the operation sequence
  fun.optimize();

  // generates source code
  CppAD::cg::ModelCSourceGen<scalar_t> sourceGen(fun, modelName_);
  setApproximationOrder(approximationOrder, sourceGen, fun);

  // Compiler objects, compile to temporary shared library file to avoid interference between processes
  CppAD::cg::ModelLibraryCSourceGen<scalar_t> libraryCSourceGen(sourceGen);
  CppAD::cg::GccCompiler<scalar_t> gccCompiler;
  CppAD::cg::DynamicModelLibraryProcessor<scalar_t> libraryProcessor(libraryCSourceGen, libraryName_ + tmpName_);

  // Compile and store the library
  dynamicLib_ = libraryProcessor.createDynamicLibrary(gccCompiler);
}

[Czworldy]

..or you guys have a example to use CppAdCodeGen to genorate pinocchio::impulseDynamics? maybe i should change the setting of CppAdCodeGen?

Thanks a lot!

[jcarpent]

You must understand with have limited time to help you. So the quicker we are able to dive into the topic, the quicker you will get an answer. So, could you provide a simple reproducible example ?

[Czworldy]

try this

#include <pinocchio/fwd.hpp>

#include <pinocchio/algorithm/contact-dynamics.hpp>
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/crba.hpp>
#include <pinocchio/algorithm/aba.hpp>
#include <pinocchio/parsers/urdf.hpp>

#include <cppad/cg.hpp>
#include <algorithm>
#include <functional>
#include <vector>

#include <urdf_model/model.h>
#include <urdf_parser/urdf_parser.h>

using scalar_t = double;
using ad_base_t = CppAD::cg::CG<scalar_t>;
using ad_scalar_t = CppAD::AD<ad_base_t>;
using ad_vector_t = Eigen::Matrix<ad_scalar_t, Eigen::Dynamic, 1>;
using ad_matrix_t = Eigen::Matrix<ad_scalar_t, Eigen::Dynamic, Eigen::Dynamic>;

int main() {
  // create model
  using joint_pair_t = std::pair<const std::string, std::shared_ptr<::urdf::Joint>>;
  const std::string urdfFilePath = "~/anymal_c_simple_description/urdf/anymal.urdf";
  urdf::ModelInterfaceSharedPtr urdfTree = urdf::parseURDFFile(urdfFilePath);
  if (urdfTree == nullptr) {
    throw std::invalid_argument("The file " + urdfFilePath + " does not contain a valid URDF model!");
  }

  // remove extraneous joints from urdf
  urdf::ModelInterfaceSharedPtr newModel = std::make_shared<::urdf::ModelInterface>(*urdfTree);
  std::vector<std::string> jointNames{"LF_HAA", "LF_HFE", "LF_KFE", "RF_HAA", "RF_HFE", "RF_KFE",
                                      "LH_HAA", "LH_HFE", "LH_KFE", "RH_HAA", "RH_HFE", "RH_KFE"};
  for (joint_pair_t& jointPair : newModel->joints_) {
    if (std::find(jointNames.begin(), jointNames.end(), jointPair.first) == jointNames.end()) {
      jointPair.second->type = urdf::Joint::FIXED;
    }
  }
  // add 6 DoF for the floating base
  pinocchio::JointModelComposite jointComposite(2);
  jointComposite.addJoint(pinocchio::JointModelTranslation());
  jointComposite.addJoint(pinocchio::JointModelSphericalZYX());
  pinocchio::ModelTpl<scalar_t> model;
  pinocchio::urdf::buildModel(urdfTree, jointComposite, model);

  //function
  ad_vector_t input(model.nq + model.nv);
  input.setOnes();  // Ones are better than zero, to prevent devision by zero in taping
  CppAD::Independent(input);
  // the input of the function is stack of q and v
  ad_vector_t qPinocchio = input.head(model.nq);
  ad_vector_t vPinocchio = input.tail(model.nv);

  auto cppAdModel = model.cast<ad_scalar_t>();
  pinocchio::DataTpl<ad_scalar_t> data(cppAdModel);

  pinocchio::forwardKinematics(cppAdModel, data, qPinocchio, vPinocchio); 
  pinocchio::computeJointJacobians(cppAdModel, data);
  pinocchio::updateFramePlacements(cppAdModel, data);
  pinocchio::crba(cppAdModel, data, qPinocchio);

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

  pinocchio::getFrameJacobian(cppAdModel, data, model.getBodyId("LF_FOOT"), pinocchio::LOCAL_WORLD_ALIGNED, jacobian);
  jacobianXyz = jacobian.topRows(3);
  std::cout << "here\n";
  pinocchio::impulseDynamics(cppAdModel, data, vPinocchio, jacobianXyz, ad_scalar_t(0.1), ad_scalar_t(0.0));
  ad_vector_t output = data.impulse_c; // impulseDynamics compute impulse and save it in data.impulse_c

  CppAD::ADFun<ad_base_t> fun(input, output);
  fun.optimize();

  // generates source code
  CppAD::cg::ModelCSourceGen<scalar_t> sourceGen(fun, "test");
  sourceGen.setCreateSparseJacobian(true);
  auto createJacobianSparsity = [&](CppAD::ADFun<ad_base_t>& fun){
    using SparsityPattern = std::vector<std::set<size_t>>;
    auto trueSparsity = CppAD::cg::jacobianSparsitySet<SparsityPattern>(fun);

    SparsityPattern jacobianSparsity(3);
    for (auto& sparsityRow : jacobianSparsity) {
      for (size_t i = 0; i < model.nq + model.nv; i++) {
        sparsityRow.insert(i);
      }
    }

    auto getIntersection = [&](const SparsityPattern& p0, const SparsityPattern& p1) {
      assert(p0.size() == p1.size());
      const auto numRows = p0.size();

      SparsityPattern result(p0.size());
      for (int row = 0; row < numRows; row++) {
        std::set_intersection(p0[row].begin(), p0[row].end(), p1[row].begin(), p1[row].end(), std::inserter(result[row], result[row].begin()));
      }
      return result;
    };
    return getIntersection(trueSparsity, jacobianSparsity);
  };
  sourceGen.setCustomSparseJacobianElements(createJacobianSparsity(fun));

  // Compiler objects, compile to temporary shared library file to avoid interference between processes
  CppAD::cg::ModelLibraryCSourceGen<scalar_t> libraryCSourceGen(sourceGen);
  CppAD::cg::GccCompiler<scalar_t> gccCompiler;
  CppAD::cg::DynamicModelLibraryProcessor<scalar_t> libraryProcessor(libraryCSourceGen, "/tmp/main/test");
  gccCompiler.setCompileLibFlags({"-O3", "-g", "-march=native", "-mtune=native", "-ffast-math"});
  gccCompiler.addCompileLibFlag("-shared");
  gccCompiler.addCompileLibFlag("-rdynamic");
  gccCompiler.setTemporaryFolder("/tmp/main");

  // Save sources
  gccCompiler.setSourcesFolder("/tmp/main");
  gccCompiler.setSaveToDiskFirst(true);

  auto dynamicLib_ = libraryProcessor.createDynamicLibrary(gccCompiler);
  auto model_ = dynamicLib_->model("test");

  return 0;
}

run the code i get

here
terminate called after throwing an instance of 'CppAD::cg::CGException'
  what():  GreaterThanZero cannot be called for non-parameters
Aborted (core dumped)

[Czworldy]

@jcarpent so any idea?

[jcarpent]

I don't understand; what do you mean here?

[Czworldy]

@jcarpent i mean i provide a reproducible code above, and this run this code can reproduce my problem. If you need more information or details, plz tell me.

Thanks again!

[Czworldy]

update: may be i should use no_compare_op?

[jorisv]

Hello @Czworldy,

I have almost been able to reproduce your issue. My setup is not able to compile the last part of your program. But If y comment the last part and build it in Debug mode, then, when I run the software, I see the following assert:

cppad-20230000.0 error from a known source:
A dependent variable value is not equal to its tape evaluation value,
perhaps it is nan.
Dependent variable value = nan
Tape evaluation value    = nan
Difference               = nan

Error detected by false result for
    0
at line 524 in the file
    $CONDA_PREFIX/include/cppad/core/fun_construct.hpp
a.out: $CONDA_PREFIX/include/cppad/utility/error_handler.hpp:206: static void CppAD::ErrorHandler::Default(bool, int, const char*, const char*, const char*): Assertion `false' failed.

This assert is raised from CppAD::ADFun<ad_base_t> fun(input, output);.

I'm not familiar with CppAD and can't help you right now, but I will try to have a look this week.

[jorisv]

Hello @Czworldy,

I investigated a little more. When running your code, the data.M matrix contains some nan. I don't really understand why, because when I'm running the same code with double as scalar type, the data.M matrix is then well computed (no nan inside). I check this, because data.impulse_c is computed from data.M.

I replace anymal by solo and then the data.M matrix computed with CppAD::AD<ad_base_t> as scalar type is well computed.

But, the data.impulse_c still contains nan. By adding some damping in the pinocchio::impulseDynamics call, the nan disappear.

  pinocchio::impulseDynamics(cppAdModel, data, vPinocchio, jacobianXyz,
                             ad_scalar_t(0.1), ad_scalar_t(0.1));

I can't run the last part of the progarm. The createDynamicLibrary function is calling my compiler with some bad arguments…

I hope this will help you.

[Czworldy]

@jorisv Thank for your help, i will add some damping and try again, by the way, maybe the nan is caused by the floating base i add to it? // add 6 DoF for the floating base

  // add 6 DoF for the floating base
  pinocchio::JointModelComposite jointComposite(2);
  jointComposite.addJoint(pinocchio::JointModelTranslation());
  jointComposite.addJoint(pinocchio::JointModelSphericalZYX());
  pinocchio::ModelTpl<scalar_t> model;
  pinocchio::urdf::buildModel(urdfTree, jointComposite, model); // the q of the model is 12+6=18

or

  // add 6 DoF for the floating base
  pinocchio::JointModelComposite jointComposite(2);
  pinocchio::ModelTpl<scalar_t> model;
  pinocchio::urdf::buildModel(urdfTree, pinocchio::JointModelFreeFlyer(), model);

[jorisv]

@Czworldy, I tried with and without floating base and I still have nan. I think is more related to CPPAD, but I have to understand how this scalar type work to know why these nan appear…

[jorisv]

@Czworldy, @jcarpent , @ManifoldFR

I have dug a little more about the nan. They appear when calling Model::cast<ad_scalar_t>.

First nan is computed when adding the "universe" frame to the returned model. A new inertia is computed with null mass because ::Eigen::NumTraits::epsilon() value is 0 for ad_scalar_t.

This issue is not visible, because the Model::frames are overwritted after.

But null division happen when casting Model::jointPlacements. The SE3Tpl::cast normalize the rotation matrix. The matrix is normalized if the epsilon of the new scalar type is inferior than the old one. Since epsilon is returning 0 for ad_scalar_t, we always enter in this condition. The normalization then use a SVD that at some point divide by 0 (the SVD code call std::numeric_limits::min that also return 0…). A lot of jointPlacements contains nan (but not all of them) for anymal C.

I don't know if we are already aware of this issue and if we know a workaround.

[jorisv]

@Czworldy, Ok, the issue is well know and solved by adding the following header before all headers :

#include "pinocchio/codegen/cppadcg.hpp" // this file should be included first before all the others!

[jorisv]

@Czworldy Did you try with the include ?

[jcarpent]

Closing this issue as there is no reply from @Czworldy