Using ct_rbd Kinematics classes with SCALAR type

[awa152]

I created a custom System of my Robot using the Dynamics and Kinematics wrappers that ct_rbd provides.
I am retrieving the Jacobian matrix using:

    ct::rbd::MyUR::Kinematics::Jacobians::Type_fr_world_J_fr_ee_link jacobian_instance;
    auto J = jacobian_instance.update(state.segment(0, 5));

and the inverse of the inertia matrix using

    ct::rbd::MyUR::RobCoGenContainer robcogen_inst;
    auto M = robcogen_inst.jSim();  
    M.update(state.segment(0, 5)); 
    M.computeL();                   
    M.computeInverse();           
    auto M_inv = M.getInverse();  

So far I was using the ct::core::SystemLinearizer and everything worked well. Now I am trying to switch to ct::core::ADCodegenLinearizer and I get the following error when compiling:

Errors     << my_ur:make /home/awahl/GitLab/panda/workspaces/ws_control_toolbox/logs/my_ur/build.make.215.log                        
In file included from /usr/include/eigen3/Eigen/Core:347:0,
                 from /usr/local/include/cppad/example/cppad_eigen.hpp:16,
                 from /home/awahl/GitLab/panda/workspaces/ws_control_toolbox/devel/include/ct/core/core.h:21,
                 from /home/awahl/GitLab/panda/workspaces/ws_control_toolbox/src/my_ur/src/myUR_03_MPC_basic_integration.h:3,
                 from /home/awahl/GitLab/panda/workspaces/ws_control_toolbox/src/my_ur/src/myUR_03_MPC_CG_basic_integration.cpp:2:
/usr/include/eigen3/Eigen/src/Core/AssignEvaluator.h: In instantiation of ‘void Eigen::internal::call_assignment_no_alias(Dst&, const Src&, const Func&) [with Dst = Eigen::Matrix<double, 6, 1>; Src = Eigen::Block<const Eigen::Matrix<CppAD::AD<CppAD::cg::CG<double> >, 18, 1, 0, 18, 1>, -1, 1, false>; Func = Eigen::internal::assign_op<double, CppAD::AD<CppAD::cg::CG<double> > >]’:
/usr/include/eigen3/Eigen/src/Core/PlainObjectBase.h:728:41:   required from ‘Derived& Eigen::PlainObjectBase<Derived>::_set_noalias(const Eigen::DenseBase<OtherDerived>&) [with OtherDerived = Eigen::Block<const Eigen::Matrix<CppAD::AD<CppAD::cg::CG<double> >, 18, 1, 0, 18, 1>, -1, 1, false>; Derived = Eigen::Matrix<double, 6, 1>]’
/usr/include/eigen3/Eigen/src/Core/PlainObjectBase.h:537:19:   required from ‘Eigen::PlainObjectBase<Derived>::PlainObjectBase(const Eigen::DenseBase<OtherDerived>&) [with OtherDerived = Eigen::Block<const Eigen::Matrix<CppAD::AD<CppAD::cg::CG<double> >, 18, 1, 0, 18, 1>, -1, 1, false>; Derived = Eigen::Matrix<double, 6, 1>]’
/usr/include/eigen3/Eigen/src/Core/Matrix.h:379:29:   required from ‘Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::Matrix(const Eigen::EigenBase<OtherDerived>&) [with OtherDerived = Eigen::Block<const Eigen::Matrix<CppAD::AD<CppAD::cg::CG<double> >, 18, 1, 0, 18, 1>, -1, 1, false>; _Scalar = double; int _Rows = 6; int _Cols = 1; int _Options = 0; int _MaxRows = 6; int _MaxCols = 1]’
/home/awahl/GitLab/panda/workspaces/ws_control_toolbox/src/my_ur/src/myUR_03_MPC_basic_integration.h:82:10:   required from ‘void my_sys::tpl::MySys<SCALAR>::computeControlledDynamics(const ct::core::StateVector<18, SCALAR>&, const time_t&, const ct::core::ControlVector<6, SCALAR>&, ct::core::StateVector<18, SCALAR>&) [with SCALAR = CppAD::AD<CppAD::cg::CG<double> >; my_sys::tpl::MySys<SCALAR>::time_t = CppAD::AD<CppAD::cg::CG<double> >]’
/home/awahl/GitLab/panda/workspaces/ws_control_toolbox/src/my_ur/src/myUR_03_MPC_CG_basic_integration.cpp:538:1:   required from here
/usr/include/eigen3/Eigen/src/Core/AssignEvaluator.h:834:3: error: static assertion failed: YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY
   EIGEN_CHECK_BINARY_COMPATIBILIY(Func,typename ActualDstTypeCleaned::Scalar,typename Src::Scalar);
   ^
compilation terminated due to -Wfatal-errors.
make[2]: *** [CMakeFiles/myUR_03_MPC_CG_basic_integration_node.dir/src/myUR_03_MPC_CG_basic_integration.cpp.o] Error 1
make[1]: *** [CMakeFiles/myUR_03_MPC_CG_basic_integration_node.dir/all] Error 2
make: *** [all] Error 2
cd /home/awahl/GitLab/panda/workspaces/ws_control_toolbox/build/my_ur; catkin build --get-env my_ur | catkin env -si  /usr/bin/make --jobserver-fds=6,7 -j; cd -

Compilation works, if I comment out the part where I retrieve the Jacobian and the inverted Inertia matrix, using only ct::core types that are templated on the SCALAR Type.
My guess would be, that the way I am trying to access the Jacobian and the Inertia is not quite correct and they don't use the SCALAR Type, which results in the above error. It would be great if you could help me out with that one.

[awa152]

So far I got to the point, where

    typename ct::rbd::MyUR::tpl::Kinematics<SCALAR>::Jacobians::Type_fr_world_J_fr_ee_link jacobian_instance;
    J = jacobian_instance.update(state.segment(0, 5));

seems to give me the properly templated Jacobian.
As for the inertia Matrix

    typename ct::rbd::MyUR::tpl::RobCoGenContainer<SCALAR> robcogen_inst;
    auto M = robcogen_inst.jSim();  
    M.update(state.segment(0, 5));  
    // M.computeL();                   
    // M.computeInverse();          
    // auto M_inv = M.getInverse();    

seems to work as well, but ass soon as I comment out the line M.computeL(); I end up with a new error message:

In file included from /home/awahl/GitLab/panda/workspaces/ws_control_toolbox/src/my_ur/include/my_ur/generated/jsim.h:110:0,
                 from /home/awahl/GitLab/panda/workspaces/ws_control_toolbox/src/my_ur/include/my_ur/MyUR.h:9,
                 from /home/awahl/GitLab/panda/workspaces/ws_control_toolbox/src/my_ur/src/myUR_03_MPC_basic_integration.h:5,
                 from /home/awahl/GitLab/panda/workspaces/ws_control_toolbox/src/my_ur/src/myUR_03_MPC_CG_basic_integration.cpp:2:
/home/awahl/GitLab/panda/workspaces/ws_control_toolbox/src/my_ur/include/my_ur/generated/jsim.impl.h: In instantiation of ‘void iit::myUR_EE_Model::dyn::tpl::JSIM<TRAIT>::computeL() [with TRAIT = ct::core::internal::CppADCodegenTrait]’:
/home/awahl/GitLab/panda/workspaces/ws_control_toolbox/src/my_ur/src/myUR_03_MPC_basic_integration.h:88:5:   required from ‘void my_sys::tpl::MySys<SCALAR>::computeControlledDynamics(const ct::core::StateVector<18, SCALAR>&, const time_t&, const ct::core::ControlVector<6, SCALAR>&, ct::core::StateVector<18, SCALAR>&) [with SCALAR = CppAD::AD<CppAD::cg::CG<double> >; my_sys::tpl::MySys<SCALAR>::time_t = CppAD::AD<CppAD::cg::CG<double> >]’
/home/awahl/GitLab/panda/workspaces/ws_control_toolbox/src/my_ur/src/myUR_03_MPC_CG_basic_integration.cpp:535:1:   required from here
/home/awahl/GitLab/panda/workspaces/ws_control_toolbox/src/my_ur/include/my_ur/generated/jsim.impl.h:137:24: error: no matching function for call to ‘sqrt(Eigen::DenseCoeffsBase<Eigen::Matrix<CppAD::AD<CppAD::cg::CG<double> >, 6, 6, 0, 6, 6>, 1>::Scalar&)’
     L(5, 5) = std::sqrt(L(5, 5));
               ~~~~~~~~~^~~~~~~~~
compilation terminated due to -Wfatal-errors.

[awa152]

As stated in the error message, the problem seems to be the use of the function std::sqrt() with the CppAD Type in the file jsim.impl.h which was generated by RobCoGen. I think that there is probably a proper way of using the function right. The way I made it work now is exchanging every occurrence of std::sqrt() in jsim.impl.h with CppAD::sqrt().
I thought that I may loose backwards compatibility, but CppAD::sqrt() seems to work on the standard double type as well. So using a numerical linearizer still works.