search

stack-of-tasks / pinocchio

A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
http://stack-of-tasks.github.io/pinocchio/
BSD 2-Clause "Simplified" License
1.78k stars 375 forks source link

Compile errors when casting model to ADScalar / CGScalar #864

Closed SebastianRiedel closed 5 years ago

SebastianRiedel commented 5 years ago

Based on the comments in #753, I'm trying to evaluate the code generation functionality of Pinocchio using the example code provided in https://github.com/stack-of-tasks/pinocchio/blob/11e195bb5ae57bb0d3010642608cf836620bf14e/unittest/cppadcg-algo.cpp#L27

On compilation, I get error at the line casting from the "double" model to the "ADScalar" model.

Any ideas or obvious mistakes (version incompatability, etc.)? I don't have much experience using / debugging pinocchio yet.

Involved software versions:

pinocchio == v2.1.6 (from source tag)
urdfdom == 1.0.3
cppad == 20180000.0
cppadcg == 2.3.0
boost == 1.65.1

Attached:

Compile Output:

make
/usr/bin/cmake -H/data/net/userstore/sr/software/pinocchio-test -B/data/net/userstore/sr/software/pinocchio-test/build --check-build-system CMakeFiles/Makefile.cmake 0
/usr/bin/cmake -E cmake_progress_start /data/net/userstore/sr/software/pinocchio-test/build/CMakeFiles /data/net/userstore/sr/software/pinocchio-test/build/CMakeFiles/progress.marks
make -f CMakeFiles/Makefile2 all
make[1]: Entering directory '/data/net/userstore/sr/software/pinocchio-test/build'
make -f CMakeFiles/cg_main.dir/build.make CMakeFiles/cg_main.dir/depend
make[2]: Entering directory '/data/net/userstore/sr/software/pinocchio-test/build'
cd /data/net/userstore/sr/software/pinocchio-test/build && /usr/bin/cmake -E cmake_depends "Unix Makefiles" /data/net/userstore/sr/software/pinocchio-test /data/net/userstore/sr/software/pinocchio-test /data/net/userstore/sr/software/pinocchio-test/build /data/net/userstore/sr/software/pinocchio-test/build /data/net/userstore/sr/software/pinocchio-test/build/CMakeFiles/cg_main.dir/DependInfo.cmake --color=
Scanning dependencies of target cg_main
make[2]: Leaving directory '/data/net/userstore/sr/software/pinocchio-test/build'
make -f CMakeFiles/cg_main.dir/build.make CMakeFiles/cg_main.dir/build
make[2]: Entering directory '/data/net/userstore/sr/software/pinocchio-test/build'
[ 50%] Building CXX object CMakeFiles/cg_main.dir/cg_main.cpp.o
/usr/bin/c++   -isystem /home/sr/.conan/data/pinocchio/2.1.6/ar/stable/package/b3cc8e77856348352b5b92a1ff72764e9cda3e91/include -isystem /home/sr/.conan/data/urdfdom/1.0.3/ar/stable/package/aeb7f93357f287889d70a446f239f9a3e07736f0/include -isystem /home/sr/.conan/data/console_bridge/0.4.3/ar/stable/package/160b4bac5177b0f9a5f4857d00317fe0862a8a02/include -isystem /home/sr/.conan/data/urdfdom_headers/1.0.3/ar/stable/package/9bfdcfa2bb925892ecf42e2a018a3f3529826676/include -isystem /home/sr/.conan/data/cppadcg/2.3.0/ar/stable/package/ca7d3d791a0c01af10c6bc9fb91b8c7ec26029ff/include -isystem /home/sr/.conan/data/cppad/20180000.0/ar/stable/package/9bfdcfa2bb925892ecf42e2a018a3f3529826676/include -isystem /home/sr/.conan/data/eigenpy/1.5.7/ar/stable/package/9bfdcfa2bb925892ecf42e2a018a3f3529826676/include -isystem /usr/include/eigen3  -DPINOCCHIO_WITH_CPPAD_SUPPORT -DPINOCCHIO_WITH_CPPADCG_SUPPORT -DPINOCCHIO_URDFDOM_TYPEDEF_SHARED_PTR -DPINOCCHIO_URDFDOM_USE_STD_SHARED_PTR -I/usr/include -DPINOCCHIO_WITH_URDFDOM -o CMakeFiles/cg_main.dir/cg_main.cpp.o -c /data/net/userstore/sr/software/pinocchio-test/cg_main.cpp
In file included from /home/sr/.conan/data/pinocchio/2.1.6/ar/stable/package/b3cc8e77856348352b5b92a1ff72764e9cda3e91/include/pinocchio/multibody/model.hpp:13:0,
                 from /data/net/userstore/sr/software/pinocchio-test/cg_main.cpp:4:
/home/sr/.conan/data/pinocchio/2.1.6/ar/stable/package/b3cc8e77856348352b5b92a1ff72764e9cda3e91/include/pinocchio/spatial/inertia.hpp: In instantiation of ‘static pinocchio::InertiaTpl<Scalar, Options> pinocchio::InertiaTpl<Scalar, Options>::Zero() [with _Scalar = CppAD::AD<CppAD::cg::CG<double> >; int _Options = 0]’:
/home/sr/.conan/data/pinocchio/2.1.6/ar/stable/package/b3cc8e77856348352b5b92a1ff72764e9cda3e91/include/pinocchio/multibody/model.hpp:152:32:   required from ‘pinocchio::ModelTpl<Scalar, Options, JointCollectionTpl>::ModelTpl() [with _Scalar = CppAD::AD<CppAD::cg::CG<double> >; int _Options = 0; JointCollectionTpl = pinocchio::JointCollectionDefaultTpl]’
/home/sr/.conan/data/pinocchio/2.1.6/ar/stable/package/b3cc8e77856348352b5b92a1ff72764e9cda3e91/include/pinocchio/multibody/model.hpp:173:18:   required from ‘pinocchio::ModelTpl<NewScalar, Options, JointCollectionTpl> pinocchio::ModelTpl<Scalar, Options, JointCollectionTpl>::cast() const [with NewScalar = CppAD::AD<CppAD::cg::CG<double> >; _Scalar = double; int _Options = 0; JointCollectionTpl = pinocchio::JointCollectionDefaultTpl]’
/data/net/userstore/sr/software/pinocchio-test/cg_main.cpp:43:43:   required from here
/home/sr/.conan/data/pinocchio/2.1.6/ar/stable/package/b3cc8e77856348352b5b92a1ff72764e9cda3e91/include/pinocchio/spatial/inertia.hpp:202:14: error: no matching function for call to ‘pinocchio::InertiaTpl<CppAD::AD<CppAD::cg::CG<double> >, 0>::InertiaTpl(double, const ConstantReturnType, pinocchio::Symmetric3Tpl<CppAD::AD<CppAD::cg::CG<double> >, 0>)’
       return InertiaTpl(0.,
              ^~~~~~~~~~~~~~
                         Vector3::Zero(),
                         ~~~~~~~~~~~~~~~~
                         Symmetric3::Zero());
                         ~~~~~~~~~~~~~~~~~~~
/home/sr/.conan/data/pinocchio/2.1.6/ar/stable/package/b3cc8e77856348352b5b92a1ff72764e9cda3e91/include/pinocchio/spatial/inertia.hpp:193:5: note: candidate: template<int O2> pinocchio::InertiaTpl<Scalar, Options>::InertiaTpl(const pinocchio::InertiaTpl<typename pinocchio::traits<pinocchio::InertiaTpl<T, U> >::Scalar, O2>&)
     InertiaTpl(const InertiaTpl<Scalar,O2> & clone)
     ^~~~~~~~~~
/home/sr/.conan/data/pinocchio/2.1.6/ar/stable/package/b3cc8e77856348352b5b92a1ff72764e9cda3e91/include/pinocchio/spatial/inertia.hpp:193:5: note:   template argument deduction/substitution failed:
/home/sr/.conan/data/pinocchio/2.1.6/ar/stable/package/b3cc8e77856348352b5b92a1ff72764e9cda3e91/include/pinocchio/spatial/inertia.hpp:202:14: note:   mismatched types ‘const pinocchio::InertiaTpl<CppAD::AD<CppAD::cg::CG<double> >, O2>’ and ‘double’
       return InertiaTpl(0.,
              ^~~~~~~~~~~~~~
                         Vector3::Zero(),
                         ~~~~~~~~~~~~~~~~
                         Symmetric3::Zero());
                         ~~~~~~~~~~~~~~~~~~~
/home/sr/.conan/data/pinocchio/2.1.6/ar/stable/package/b3cc8e77856348352b5b92a1ff72764e9cda3e91/include/pinocchio/spatial/inertia.hpp:188:5: note: candidate: pinocchio::InertiaTpl<Scalar, Options>::InertiaTpl(const pinocchio::InertiaTpl<Scalar, Options>&) [with _Scalar = CppAD::AD<CppAD::cg::CG<double> >; int _Options = 0]
     InertiaTpl(const InertiaTpl & clone)  // Clone constructor for std::vector
     ^~~~~~~~~~
/home/sr/.conan/data/pinocchio/2.1.6/ar/stable/package/b3cc8e77856348352b5b92a1ff72764e9cda3e91/include/pinocchio/spatial/inertia.hpp:188:5: note:   candidate expects 1 argument, 3 provided
/home/sr/.conan/data/pinocchio/2.1.6/ar/stable/package/b3cc8e77856348352b5b92a1ff72764e9cda3e91/include/pinocchio/spatial/inertia.hpp:184:5: note: candidate: pinocchio::InertiaTpl<Scalar, Options>::InertiaTpl(pinocchio::InertiaTpl<Scalar, Options>::Scalar, const Vector3&, const Symmetric3&) [with _Scalar = CppAD::AD<CppAD::cg::CG<double> >; int _Options = 0; pinocchio::InertiaTpl<Scalar, Options>::Scalar = CppAD::AD<CppAD::cg::CG<double> >; pinocchio::InertiaTpl<Scalar, Options>::Vector3 = Eigen::Matrix<CppAD::AD<CppAD::cg::CG<double> >, 3, 1, 0, 3, 1>; pinocchio::InertiaTpl<Scalar, Options>::Symmetric3 = pinocchio::Symmetric3Tpl<CppAD::AD<CppAD::cg::CG<double> >, 0>]
     InertiaTpl(Scalar mass, const Vector3 & com, const Symmetric3 & rotational_inertia)
     ^~~~~~~~~~
/home/sr/.conan/data/pinocchio/2.1.6/ar/stable/package/b3cc8e77856348352b5b92a1ff72764e9cda3e91/include/pinocchio/spatial/inertia.hpp:184:5: note:   no known conversion for argument 1 from ‘double’ to ‘pinocchio::InertiaTpl<CppAD::AD<CppAD::cg::CG<double> >, 0>::Scalar {aka CppAD::AD<CppAD::cg::CG<double> >}’
In file included from /home/sr/.conan/data/pinocchio/2.1.6/ar/stable/package/b3cc8e77856348352b5b92a1ff72764e9cda3e91/include/pinocchio/multibody/model.hpp:13:0,
                 from /data/net/userstore/sr/software/pinocchio-test/cg_main.cpp:4:
/home/sr/.conan/data/pinocchio/2.1.6/ar/stable/package/b3cc8e77856348352b5b92a1ff72764e9cda3e91/include/pinocchio/spatial/inertia.hpp:170:5: note: candidate: pinocchio::InertiaTpl<Scalar, Options>::InertiaTpl(const Matrix6&) [with _Scalar = CppAD::AD<CppAD::cg::CG<double> >; int _Options = 0; pinocchio::InertiaTpl<Scalar, Options>::Matrix6 = Eigen::Matrix<CppAD::AD<CppAD::cg::CG<double> >, 6, 6, 0, 6, 6>]
     InertiaTpl(const Matrix6 & I6)
     ^~~~~~~~~~
/home/sr/.conan/data/pinocchio/2.1.6/ar/stable/package/b3cc8e77856348352b5b92a1ff72764e9cda3e91/include/pinocchio/spatial/inertia.hpp:170:5: note:   candidate expects 1 argument, 3 provided
/home/sr/.conan/data/pinocchio/2.1.6/ar/stable/package/b3cc8e77856348352b5b92a1ff72764e9cda3e91/include/pinocchio/spatial/inertia.hpp:166:5: note: candidate: pinocchio::InertiaTpl<Scalar, Options>::InertiaTpl(pinocchio::InertiaTpl<Scalar, Options>::Scalar, const Vector3&, const Matrix3&) [with _Scalar = CppAD::AD<CppAD::cg::CG<double> >; int _Options = 0; pinocchio::InertiaTpl<Scalar, Options>::Scalar = CppAD::AD<CppAD::cg::CG<double> >; pinocchio::InertiaTpl<Scalar, Options>::Vector3 = Eigen::Matrix<CppAD::AD<CppAD::cg::CG<double> >, 3, 1, 0, 3, 1>; pinocchio::InertiaTpl<Scalar, Options>::Matrix3 = Eigen::Matrix<CppAD::AD<CppAD::cg::CG<double> >, 3, 3, 0, 3, 3>]
     InertiaTpl(const Scalar mass, const Vector3 & com, const Matrix3 & rotational_inertia)
     ^~~~~~~~~~
/home/sr/.conan/data/pinocchio/2.1.6/ar/stable/package/b3cc8e77856348352b5b92a1ff72764e9cda3e91/include/pinocchio/spatial/inertia.hpp:166:5: note:   no known conversion for argument 1 from ‘double’ to ‘pinocchio::InertiaTpl<CppAD::AD<CppAD::cg::CG<double> >, 0>::Scalar {aka CppAD::AD<CppAD::cg::CG<double> >}’
/home/sr/.conan/data/pinocchio/2.1.6/ar/stable/package/b3cc8e77856348352b5b92a1ff72764e9cda3e91/include/pinocchio/spatial/inertia.hpp:163:5: note: candidate: pinocchio::InertiaTpl<Scalar, Options>::InertiaTpl() [with _Scalar = CppAD::AD<CppAD::cg::CG<double> >; int _Options = 0]
     InertiaTpl()
     ^~~~~~~~~~
/home/sr/.conan/data/pinocchio/2.1.6/ar/stable/package/b3cc8e77856348352b5b92a1ff72764e9cda3e91/include/pinocchio/spatial/inertia.hpp:163:5: note:   candidate expects 0 arguments, 3 provided
CMakeFiles/cg_main.dir/build.make:65: recipe for target 'CMakeFiles/cg_main.dir/cg_main.cpp.o' failed
make[2]: *** [CMakeFiles/cg_main.dir/cg_main.cpp.o] Error 1
make[2]: Leaving directory '/data/net/userstore/sr/software/pinocchio-test/build'
CMakeFiles/Makefile2:70: recipe for target 'CMakeFiles/cg_main.dir/all' failed
make[1]: *** [CMakeFiles/cg_main.dir/all] Error 2
make[1]: Leaving directory '/data/net/userstore/sr/software/pinocchio-test/build'
Makefile:86: recipe for target 'all' failed
make: *** [all] Error 2

Code:

#include <cppad/cg.hpp>

#include <pinocchio/fwd.hpp>
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/multibody/data.hpp>

#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/algorithm/crba.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/algorithm/aba.hpp>
#include <pinocchio/algorithm/joint-configuration.hpp>

// include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/parsers/sample-models.hpp>

#include <iostream>

int main(int argc, char ** argv)
{
//  std::string filename = (argc<=1) ? "ur5.urdf" : argv[1];

  typedef double Scalar;
  typedef CppAD::cg::CG<Scalar> CGScalar;
  typedef CppAD::AD<CGScalar> ADScalar;

  typedef Eigen::Matrix<ADScalar,Eigen::Dynamic,1> ADVector;

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

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

  Model model;
//  pinocchio::urdf::buildModel(filename,model);
  pinocchio::buildModels::humanoidRandom(model);
  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  Data data(model);

  ADModel ad_model = model.cast<ADScalar>();
  ADData ad_data(ad_model);

  // Sample random configuration
  typedef Model::ConfigVectorType CongigVectorType;
  typedef Model::TangentVectorType TangentVectorType;
  CongigVectorType q(model.nq);
  q = pinocchio::randomConfiguration(model);

  TangentVectorType v(TangentVectorType::Random(model.nv));
  TangentVectorType a(TangentVectorType::Random(model.nv));

  typedef ADModel::ConfigVectorType ADCongigVectorType;
  typedef ADModel::TangentVectorType ADTangentVectorType;

  ADCongigVectorType ad_q = q.cast<ADScalar>();
  ADTangentVectorType ad_v = v.cast<ADScalar>();
  ADTangentVectorType ad_a = a.cast<ADScalar>();

  ADTangentVectorType & X = ad_a;
  CppAD::Independent(X);

  pinocchio::rnea(ad_model,ad_data,ad_q,ad_v,ad_a);
  ADVector Y(model.nv); Y = ad_data.tau;

  CppAD::ADFun<CGScalar> fun(X,Y);

  // generates source code
  CppAD::cg::ModelCSourceGen<Scalar> cgen(fun, "rnea");
  cgen.setCreateJacobian(true);
  cgen.setCreateForwardZero(true);
  cgen.setCreateForwardOne(true);
  cgen.setCreateReverseOne(true);
  cgen.setCreateReverseTwo(true);
  CppAD::cg::ModelLibraryCSourceGen<Scalar> libcgen(cgen);

  // compile source code
  CppAD::cg::DynamicModelLibraryProcessor<Scalar> p(libcgen);

  CppAD::cg::GccCompiler<Scalar> compiler;
  std::unique_ptr<CppAD::cg::DynamicLib<Scalar>> dynamicLib = p.createDynamicLibrary(compiler);

  // save to files (not really required)
  CppAD::cg::SaveFilesModelLibraryProcessor<Scalar> p2(libcgen);
  p2.saveSources();
}
jcarpent commented 5 years ago

@SebastianRiedel Thanks a lot for raising this issue. I think commit b35bb4bc29e1b56298a40797742e04f1511f032d has solved your issue. Could you try with the current devel branch https://github.com/stack-of-tasks/pinocchio/tree/devel?

jcarpent commented 5 years ago

@SebastianRiedel I confirm that your example is passing with the current devel branch. Do you need a release?

SebastianRiedel commented 5 years ago

@jcarpent Thanks a lot for your quick reply, for now I think it is just fine if I can test/play with it. I can use devel for that then and will give some feedback and/or close the issue on Monday.

SebastianRiedel commented 5 years ago

Was able to compile the example code on devel. Thanks!

jcarpent commented 5 years ago

@SebastianRiedel Happy that your issue is now solved.