pinocchio: Compute gravity with c++ api wrong

I use python api to compute the gravity and the result is [-0.11936972 0.98543975 0. 0.9855705 0.03554471 -0.05846138]. That’s what I expected.

q = np.asarray([0, 0, 0, 1.57, 0, 0])
v = pinocchio.utils.zero(model.nv)
a = pinocchio.utils.zero(model.nv)
gravity = pinocchio.rnea(model,data,q,v,a)

I use c++ api to compute the gravity of the same model, but the result is [0 0 0 0 0 0].

const std::string urdf_filename = "six_arm_hand/urdf/six_arm_hand_no_palm.urdf";
pinocchio::urdf::buildModel(urdf_filename, model);
pinocchio::Data data(model);
Eigen::VectorXd q = pinocchio::neutral(model);
Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
q << 0, 0, 0, 1.57, 0, 0;
gravity = pinocchio::rnea(model, data, q, v, a);

What’s wrong in my c++ code? Could you give me some hints?

About this issue

  • Original URL
  • State: closed
  • Created 5 years ago
  • Comments: 21 (14 by maintainers)

Most upvoted comments

g++ (Ubuntu 8.4.0-1ubuntu1~18.04) 8.4.0 And here is my ouput:

➜ g++ cartesian_impedence_controller_simplify.cpp -o test $(pkg-config --cflags pinocchio) $(pkg-config --libs pinocchio)

In file included from /usr/include/eigen3/unsupported/Eigen/CXX11/Tensor:105,
                 from /opt/openrobots/include/pinocchio/fwd.hpp:31,
                 from cartesian_impedence_controller_simplify.cpp:1:
/usr/include/eigen3/unsupported/Eigen/CXX11/src/Tensor/TensorEvaluator.h: In instantiation of ‘struct Eigen::TensorEvaluator<const Eigen::Tensor<double, 3, 0, long int>, Eigen::DefaultDevice>’:
/usr/include/eigen3/unsupported/Eigen/CXX11/src/Tensor/TensorEvaluator.h:232:8:   required from ‘struct Eigen::TensorEvaluator<const Eigen::TensorCwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, const Eigen::Tensor<double, 3, 0, long int> >, Eigen::DefaultDevice>’
/usr/include/eigen3/unsupported/Eigen/CXX11/src/Tensor/TensorAssign.h:96:70:   required from ‘struct Eigen::TensorEvaluator<const Eigen::TensorAssignOp<Eigen::Tensor<double, 3, 0, long int>, const Eigen::TensorCwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, const Eigen::Tensor<double, 3, 0, long int> > >, Eigen::DefaultDevice>’
/usr/include/eigen3/unsupported/Eigen/CXX11/src/Tensor/Tensor.h:416:14:   required from ‘Eigen::Tensor<Scalar_, NumIndices_, Options_, IndexType>& Eigen::Tensor<Scalar_, NumIndices_, Options_, IndexType>::operator=(const OtherDerived&) [with OtherDerived = Eigen::TensorCwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, const Eigen::Tensor<double, 3, 0, long int> >; Scalar_ = double; int NumIndices_ = 3; int Options_ = 0; IndexType_ = long int]’
/usr/include/eigen3/unsupported/Eigen/CXX11/src/Tensor/TensorBase.h:845:24:   required from ‘Derived& Eigen::TensorBase<Derived, AccessLevel>::setConstant(const Scalar&) [with Derived = Eigen::Tensor<double, 3, 0, long int>; int AccessLevel = 1; Eigen::TensorBase<Derived, AccessLevel>::Scalar = double]’
/usr/include/eigen3/unsupported/Eigen/CXX11/src/Tensor/TensorBase.h:841:14:   required from ‘Derived& Eigen::TensorBase<Derived, AccessLevel>::setZero() [with Derived = Eigen::Tensor<double, 3, 0, long int>; int AccessLevel = 1]’
/opt/openrobots/include/pinocchio/multibody/data.hxx:131:5:   required from ‘pinocchio::DataTpl<Scalar, Options, JointCollectionTpl>::DataTpl(const Model&) [with _Scalar = double; int _Options = 0; JointCollectionTpl = pinocchio::JointCollectionDefaultTpl; pinocchio::DataTpl<Scalar, Options, JointCollectionTpl>::Model = pinocchio::ModelTpl<double>]’
cartesian_impedence_controller_simplify.cpp:18:31:   required from here
/usr/include/eigen3/unsupported/Eigen/CXX11/src/Tensor/TensorEvaluator.h:162:71: warning: ignoring attributes on template argument ‘Eigen::PacketType<double, Eigen::DefaultDevice>::type’ {aka ‘__vector(2) double’} [-Wignored-attributes]
     PacketAccess = (internal::unpacket_traits<PacketReturnType>::size > 1),
                    ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~
In file included from /usr/include/eigen3/unsupported/Eigen/CXX11/Tensor:105,
                 from /opt/openrobots/include/pinocchio/fwd.hpp:31,
                 from cartesian_impedence_controller_simplify.cpp:1:
/usr/include/eigen3/unsupported/Eigen/CXX11/src/Tensor/TensorEvaluator.h: In instantiation of ‘struct Eigen::TensorEvaluator<Eigen::Tensor<double, 3, 0, long int>, Eigen::DefaultDevice>’:
/usr/include/eigen3/unsupported/Eigen/CXX11/src/Tensor/TensorAssign.h:100:65:   required from ‘struct Eigen::TensorEvaluator<const Eigen::TensorAssignOp<Eigen::Tensor<double, 3, 0, long int>, const Eigen::TensorCwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, const Eigen::Tensor<double, 3, 0, long int> > >, Eigen::DefaultDevice>’
/usr/include/eigen3/unsupported/Eigen/CXX11/src/Tensor/Tensor.h:416:14:   required from ‘Eigen::Tensor<Scalar_, NumIndices_, Options_, IndexType>& Eigen::Tensor<Scalar_, NumIndices_, Options_, IndexType>::operator=(const OtherDerived&) [with OtherDerived = Eigen::TensorCwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, const Eigen::Tensor<double, 3, 0, long int> >; Scalar_ = double; int NumIndices_ = 3; int Options_ = 0; IndexType_ = long int]’
/usr/include/eigen3/unsupported/Eigen/CXX11/src/Tensor/TensorBase.h:845:24:   required from ‘Derived& Eigen::TensorBase<Derived, AccessLevel>::setConstant(const Scalar&) [with Derived = Eigen::Tensor<double, 3, 0, long int>; int AccessLevel = 1; Eigen::TensorBase<Derived, AccessLevel>::Scalar = double]’
/usr/include/eigen3/unsupported/Eigen/CXX11/src/Tensor/TensorBase.h:841:14:   required from ‘Derived& Eigen::TensorBase<Derived, AccessLevel>::setZero() [with Derived = Eigen::Tensor<double, 3, 0, long int>; int AccessLevel = 1]’
/opt/openrobots/include/pinocchio/multibody/data.hxx:131:5:   required from ‘pinocchio::DataTpl<Scalar, Options, JointCollectionTpl>::DataTpl(const Model&) [with _Scalar = double; int _Options = 0; JointCollectionTpl = pinocchio::JointCollectionDefaultTpl; pinocchio::DataTpl<Scalar, Options, JointCollectionTpl>::Model = pinocchio::ModelTpl<double>]’
cartesian_impedence_controller_simplify.cpp:18:31:   required from here
/usr/include/eigen3/unsupported/Eigen/CXX11/src/Tensor/TensorEvaluator.h:42:71: warning: ignoring attributes on template argument ‘Eigen::PacketType<double, Eigen::DefaultDevice>::type’ {aka ‘__vector(2) double’} [-Wignored-attributes]
     PacketAccess = (internal::unpacket_traits<PacketReturnType>::size > 1),
                    ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~
In file included from /usr/include/eigen3/unsupported/Eigen/CXX11/Tensor:139,
                 from /opt/openrobots/include/pinocchio/fwd.hpp:31,
                 from cartesian_impedence_controller_simplify.cpp:1:
/usr/include/eigen3/unsupported/Eigen/CXX11/src/Tensor/TensorExecutor.h: In instantiation of ‘static void Eigen::internal::TensorExecutor<Expression, Eigen::DefaultDevice, true>::run(const Expression&, const Eigen::DefaultDevice&) [with Expression = const Eigen::TensorAssignOp<Eigen::Tensor<double, 3, 0, long int>, const Eigen::TensorCwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, const Eigen::Tensor<double, 3, 0, long int> > >]’:
/usr/include/eigen3/unsupported/Eigen/CXX11/src/Tensor/Tensor.h:417:65:   required from ‘Eigen::Tensor<Scalar_, NumIndices_, Options_, IndexType>& Eigen::Tensor<Scalar_, NumIndices_, Options_, IndexType>::operator=(const OtherDerived&) [with OtherDerived = Eigen::TensorCwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, const Eigen::Tensor<double, 3, 0, long int> >; Scalar_ = double; int NumIndices_ = 3; int Options_ = 0; IndexType_ = long int]’
/usr/include/eigen3/unsupported/Eigen/CXX11/src/Tensor/TensorBase.h:845:24:   required from ‘Derived& Eigen::TensorBase<Derived, AccessLevel>::setConstant(const Scalar&) [with Derived = Eigen::Tensor<double, 3, 0, long int>; int AccessLevel = 1; Eigen::TensorBase<Derived, AccessLevel>::Scalar = double]’
/usr/include/eigen3/unsupported/Eigen/CXX11/src/Tensor/TensorBase.h:841:14:   required from ‘Derived& Eigen::TensorBase<Derived, AccessLevel>::setZero() [with Derived = Eigen::Tensor<double, 3, 0, long int>; int AccessLevel = 1]’
/opt/openrobots/include/pinocchio/multibody/data.hxx:131:5:   required from ‘pinocchio::DataTpl<Scalar, Options, JointCollectionTpl>::DataTpl(const Model&) [with _Scalar = double; int _Options = 0; JointCollectionTpl = pinocchio::JointCollectionDefaultTpl; pinocchio::DataTpl<Scalar, Options, JointCollectionTpl>::Model = pinocchio::ModelTpl<double>]’
cartesian_impedence_controller_simplify.cpp:18:31:   required from here
/usr/include/eigen3/unsupported/Eigen/CXX11/src/Tensor/TensorExecutor.h:61:17: warning: ignoring attributes on template argument ‘Eigen::TensorEvaluator<const Eigen::TensorAssignOp<Eigen::Tensor<double, 3, 0, long int>, const Eigen::TensorCwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, const Eigen::Tensor<double, 3, 0, long int> > >, Eigen::DefaultDevice>::PacketReturnType’ {aka ‘__vector(2) double’} [-Wignored-attributes]
       const int PacketSize = unpacket_traits<typename TensorEvaluator<Expression, DefaultDevice>::PacketReturnType>::size;
                 ^~~~~~~~~~

What is the full python code (including where you load the robot)?