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.88k stars 391 forks source link

Getting jacobians with float scalar type does not compile #894

Closed matteoparigi closed 5 years ago

matteoparigi commented 5 years ago

Hi all, in my group we are using your library together with casadi for automatic differentiation and optimization, with some success! However, we have troubles when trying to compute symbolic jacobians, meaning that our scalar type is casadi::SX. We also noticed that the same code does not compile even if the scalar type is just a float, as in the example below:

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

#include <urdf_parser/urdf_parser.h>

typedef float Scalar;

void generate_forward_kin(std::string urdf_string, std::string body_name)
{

    auto urdf = urdf::parseURDF(urdf_string);

    pinocchio::Model model_dbl;
    pinocchio::urdf::buildModel(urdf, model_dbl, true);
    pinocchio::Data data_dbl(model_dbl);

    auto model = model_dbl.cast<Scalar>();
    pinocchio::DataTpl<Scalar> data(model);

    Eigen::Matrix<Scalar, -1, 1> q;
    Eigen::Matrix<Scalar, 6, -1> J;
    pinocchio::computeJointJacobians(model, data, q);
    pinocchio::framesForwardKinematics(model, data, q);
    pinocchio::frameJacobian(model, data, q, pinocchio::FrameIndex(), J);
}

More specifically, the last line causes the issue, as in the gcc error message

/home/matteo/advr-superbuild/build/install/include/pinocchio/algorithm/frames.hxx:165:47: error: invalid initialization of reference of type ‘const Frame& {aka const pinocchio::FrameTpl<double>&}’ from expression of type ‘const value_type {aka const pinocchio::FrameTpl<float, 0>}’
     const Frame & frame = model.frames[frameId];
                                               ^
/home/matteo/advr-superbuild/build/install/include/pinocchio/algorithm/frames.hxx:167:23: error: no match for ‘operator=’ (operand types are ‘__gnu_cxx::__alloc_traits<Eigen::aligned_allocator<pinocchio::SE3Tpl<float, 0> > >::value_type {aka pinocchio::SE3Tpl<float, 0>}’ and ‘const SE3 {aka const pinocchio::SE3Tpl<double, 0>}’)
     data.iMf[jointId] = frame.placement;
                       ^
In file included from /home/matteo/advr-superbuild/build/install/include/pinocchio/spatial/se3.hpp:43:0,
                 from /home/matteo/advr-superbuild/build/install/include/pinocchio/multibody/model.hpp:10,
                 from /home/matteo/advr-superbuild/build/install/include/pinocchio/parsers/urdf.hpp:9,
                 from /home/matteo/advr-superbuild/external/CentroidalPlanner/include/CentroidalPlanner/Casadi/pinocchio_bug.cpp:1:
/home/matteo/advr-superbuild/build/install/include/pinocchio/spatial/se3-tpl.hpp:85:14: note: candidate: template<int O2> pinocchio::SE3Tpl<Scalar, Options>& pinocchio::SE3Tpl<Scalar, Options>::operator=(const pinocchio::SE3Tpl<typename pinocchio::traits<pinocchio::SE3Tpl<_Scalar, _Options> >::Scalar, O2>&) [with int O2 = O2; _Scalar = float; int _Options = 0]
     SE3Tpl & operator=(const SE3Tpl<Scalar,O2> & other)
              ^
/home/matteo/advr-superbuild/build/install/include/pinocchio/spatial/se3-tpl.hpp:85:14: note:   template argument deduction/substitution failed:
In file included from /home/matteo/advr-superbuild/build/install/include/pinocchio/algorithm/frames.hpp:293:0,
                 from /home/matteo/advr-superbuild/external/CentroidalPlanner/include/CentroidalPlanner/Casadi/pinocchio_bug.cpp:3:
/home/matteo/advr-superbuild/build/install/include/pinocchio/algorithm/frames.hxx:167:23: note:   mismatched types ‘float’ and ‘double’
     data.iMf[jointId] = frame.placement;
                       ^
/home/matteo/advr-superbuild/build/install/include/pinocchio/algorithm/frames.hxx:167:23: note:   ‘const SE3 {aka const pinocchio::SE3Tpl<double, 0>}’ is not derived from ‘const pinocchio::SE3Tpl<float, O2>’
In file included from /home/matteo/advr-superbuild/build/install/include/pinocchio/spatial/se3.hpp:43:0,
                 from /home/matteo/advr-superbuild/build/install/include/pinocchio/multibody/model.hpp:10,
                 from /home/matteo/advr-superbuild/build/install/include/pinocchio/parsers/urdf.hpp:9,
                 from /home/matteo/advr-superbuild/external/CentroidalPlanner/include/CentroidalPlanner/Casadi/pinocchio_bug.cpp:1:
/home/matteo/advr-superbuild/build/install/include/pinocchio/spatial/se3-tpl.hpp:41:10: note: candidate: pinocchio::SE3Tpl<float, 0>& pinocchio::SE3Tpl<float, 0>::operator=(const pinocchio::SE3Tpl<float, 0>&)
   struct SE3Tpl : public SE3Base< SE3Tpl<_Scalar,_Options> >
          ^
/home/matteo/advr-superbuild/build/install/include/pinocchio/spatial/se3-tpl.hpp:41:10: note:   no known conversion for argument 1 from ‘const SE3 {aka const pinocchio::SE3Tpl<double, 0>}’ to ‘const pinocchio::SE3Tpl<float, 0>&’
/home/matteo/advr-superbuild/build/install/include/pinocchio/spatial/se3-tpl.hpp:41:10: note: candidate: pinocchio::SE3Tpl<float, 0>& pinocchio::SE3Tpl<float, 0>::operator=(pinocchio::SE3Tpl<float, 0>&&)
/home/matteo/advr-superbuild/build/install/include/pinocchio/spatial/se3-tpl.hpp:41:10: note:   no known conversion for argument 1 from ‘const SE3 {aka const pinocchio::SE3Tpl<double, 0>}’ to ‘pinocchio::SE3Tpl<float, 0>&&’
In file included from /home/matteo/advr-superbuild/build/install/include/pinocchio/parsers/urdf.hpp:9:0,
                 from /home/matteo/advr-superbuild/external/CentroidalPlanner/include/CentroidalPlanner/Casadi/pinocchio_bug.cpp:1:
/home/matteo/advr-superbuild/build/install/include/pinocchio/multibody/model.hpp: In instantiation of ‘pinocchio::ModelTpl<Scalar, Options, JointCollectionTpl>::ModelTpl() [with _Scalar = float; int _Options = 0; JointCollectionTpl = pinocchio::JointCollectionDefaultTpl]’:
/home/matteo/advr-superbuild/build/install/include/pinocchio/multibody/model.hpp:173:18:   required from ‘pinocchio::ModelTpl<NewScalar, Options, JointCollectionTpl> pinocchio::ModelTpl<Scalar, Options, JointCollectionTpl>::cast() const [with NewScalar = float; _Scalar = double; int _Options = 0; JointCollectionTpl = pinocchio::JointCollectionDefaultTpl]’
/home/matteo/advr-superbuild/external/CentroidalPlanner/include/CentroidalPlanner/Casadi/pinocchio_bug.cpp:20:41:   required from here

Do you have any clue about this error, or on how to generate jacobians with scalar types other than double? We are using the latest devel

Thank you! Matteo

jcarpent commented 5 years ago

Dear @matteoparigi, thanks for the issue. I've found the bug, which will be fixed by #895. Can you confirm this fix is working now?

matteoparigi commented 5 years ago

Yes, I confirm that it now compiles with both float and casadi::SX scalar type. Thank you very much for your promptness!

Best, Matteo

jcarpent commented 5 years ago

Sounds great. Thanks a lot for your quick feedback.

jcarpent commented 5 years ago

Fixed by #895.