posture task read error

[dalinel]

Hi,
If I compile tsid devel branch in DEBUG and run the following test:
valgrind ./tsid-formulation
I obtain the following read error:

[SolverHQuadProg.solver-eiquadprog] Resizing Hessian from 0 to 61
==7320== Invalid read of size 16
==7320==    at 0x48C019: _mm_loadu_pd (emmintrin.h:131)
==7320==    by 0x48C019: double __vector(2) Eigen::internal::ploadu<double __vector(2)>(Eigen::internal::unpacket_traits<double __vector(2)>::type const*) (PacketMath.h:336)
==7320==    by 0x4CE910: ploadt<__vector(2) double, 0> (GenericPacketMath.h:465)
==7320==    by 0x4CE910: double __vector(2) Eigen::internal::mapbase_evaluator<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false>, Eigen::Matrix<double, -1, 1, 0, -1, 1> >::packet<0, double __vector(2)>(long) const (CoreEvaluators.h:867)
==7320==    by 0x4C7931: void Eigen::internal::generic_dense_assignment_kernel<Eigen::internal::evaluator<Eigen::Matrix<double, -1, 1, 0, -1, 1> >, Eigen::internal::evaluator<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> >, Eigen::internal::assign_op<double, double>, 0>::assignPacket<16, 0, double __vector(2)>(long) (AssignEvaluator.h:658)
==7320==    by 0x4C12D8: Eigen::internal::dense_assignment_loop<Eigen::internal::generic_dense_assignment_kernel<Eigen::internal::evaluator<Eigen::Matrix<double, -1, 1, 0, -1, 1> >, Eigen::internal::evaluator<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> >, Eigen::internal::assign_op<double, double>, 0>, 3, 0>::run(Eigen::internal::generic_dense_assignment_kernel<Eigen::internal::evaluator<Eigen::Matrix<double, -1, 1, 0, -1, 1> >, Eigen::internal::evaluator<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> >, Eigen::internal::assign_op<double, double>, 0>&) (AssignEvaluator.h:416)
==7320==    by 0x4B9F4B: void Eigen::internal::call_dense_assignment_loop<Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false>, Eigen::internal::assign_op<double, double> >(Eigen::Matrix<double, -1, 1, 0, -1, 1>&, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> const&, Eigen::internal::assign_op<double, double> const&) (AssignEvaluator.h:741)
==7320==    by 0x4B156F: Eigen::internal::Assignment<Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false>, Eigen::internal::assign_op<double, double>, Eigen::internal::Dense2Dense, void>::run(Eigen::Matrix<double, -1, 1, 0, -1, 1>&, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> const&, Eigen::internal::assign_op<double, double> const&) (AssignEvaluator.h:879)
==7320==    by 0x4A9F2F: void Eigen::internal::call_assignment_no_alias<Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false>, Eigen::internal::assign_op<double, double> >(Eigen::Matrix<double, -1, 1, 0, -1, 1>&, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> const&, Eigen::internal::assign_op<double, double> const&) (AssignEvaluator.h:836)
==7320==    by 0x5C54E30: void Eigen::internal::call_assignment<Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false>, Eigen::internal::assign_op<double, double> >(Eigen::Matrix<double, -1, 1, 0, -1, 1>&, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> const&, Eigen::internal::assign_op<double, double> const&, Eigen::internal::enable_if<!Eigen::internal::evaluator_assume_aliasing<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false>, Eigen::internal::evaluator_traits<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> >::Shape>::value, void*>::type) (AssignEvaluator.h:804)
==7320==    by 0x5C54774: void Eigen::internal::call_assignment<Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> >(Eigen::Matrix<double, -1, 1, 0, -1, 1>&, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> const&) (AssignEvaluator.h:782)
==7320==    by 0x5C534A6: Eigen::Matrix<double, -1, 1, 0, -1, 1>& Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >::_set<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> >(Eigen::DenseBase<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> > const&) (PlainObjectBase.h:710)
==7320==    by 0x5C52DEA: Eigen::Matrix<double, -1, 1, 0, -1, 1>& Eigen::Matrix<double, -1, 1, 0, -1, 1>::operator=<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> >(Eigen::DenseBase<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> > const&) (Matrix.h:225)
==7320==    by 0x5C521B4: tsid::tasks::TaskJointPosture::compute(double, Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, 0, Eigen::InnerStride<1> >, Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, 0, Eigen::InnerStride<1> >, pinocchio::DataTpl<double, 0, pinocchio::JointCollectionDefaultTpl>&) (task-joint-posture.cpp:162)
==7320==  Address 0x8d44ff0 is 48 bytes inside a block of size 296 free'd
==7320==    at 0x4C32D3B: free (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==7320==    by 0x48B4CA: Eigen::internal::aligned_free(void*) (Memory.h:177)
==7320==    by 0x48FC3F: void Eigen::internal::conditional_aligned_free<true>(void*) (Memory.h:230)
==7320==    by 0x4964C7: void Eigen::internal::conditional_aligned_delete_auto<double, true>(double*, unsigned long) (Memory.h:416)
==7320==    by 0x496820: Eigen::DenseStorage<double, -1, -1, 1, 0>::resize(long, long, long) (DenseStorage.h:555)
==7320==    by 0x4A9899: Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >::resize(long, long) (PlainObjectBase.h:293)
==7320==    by 0x4C11D0: void Eigen::internal::resize_if_allowed<Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false>, double, double>(Eigen::Matrix<double, -1, 1, 0, -1, 1>&, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> const&, Eigen::internal::assign_op<double, double> const&) (AssignEvaluator.h:720)
==7320==    by 0x4B9EFF: void Eigen::internal::call_dense_assignment_loop<Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false>, Eigen::internal::assign_op<double, double> >(Eigen::Matrix<double, -1, 1, 0, -1, 1>&, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> const&, Eigen::internal::assign_op<double, double> const&) (AssignEvaluator.h:734)
==7320==    by 0x4B156F: Eigen::internal::Assignment<Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false>, Eigen::internal::assign_op<double, double>, Eigen::internal::Dense2Dense, void>::run(Eigen::Matrix<double, -1, 1, 0, -1, 1>&, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> const&, Eigen::internal::assign_op<double, double> const&) (AssignEvaluator.h:879)
==7320==    by 0x4A9F2F: void Eigen::internal::call_assignment_no_alias<Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false>, Eigen::internal::assign_op<double, double> >(Eigen::Matrix<double, -1, 1, 0, -1, 1>&, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> const&, Eigen::internal::assign_op<double, double> const&) (AssignEvaluator.h:836)
==7320==    by 0x5C54E30: void Eigen::internal::call_assignment<Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false>, Eigen::internal::assign_op<double, double> >(Eigen::Matrix<double, -1, 1, 0, -1, 1>&, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> const&, Eigen::internal::assign_op<double, double> const&, Eigen::internal::enable_if<!Eigen::internal::evaluator_assume_aliasing<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false>, Eigen::internal::evaluator_traits<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> >::Shape>::value, void*>::type) (AssignEvaluator.h:804)
==7320==    by 0x5C54774: void Eigen::internal::call_assignment<Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> >(Eigen::Matrix<double, -1, 1, 0, -1, 1>&, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> const&) (AssignEvaluator.h:782)
==7320==  Block was alloc'd at
==7320==    at 0x4C31B0F: malloc (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==7320==    by 0x48B463: Eigen::internal::aligned_malloc(unsigned long) (Memory.h:159)
==7320==    by 0x48FC25: void* Eigen::internal::conditional_aligned_malloc<true>(unsigned long) (Memory.h:214)
==7320==    by 0x49F496: double* Eigen::internal::conditional_aligned_new_auto<double, true>(unsigned long) (Memory.h:374)
==7320==    by 0x496833: Eigen::DenseStorage<double, -1, -1, 1, 0>::resize(long, long, long) (DenseStorage.h:557)
==7320==    by 0x48FE4E: Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >::resize(long) (PlainObjectBase.h:319)
==7320==    by 0x49A865: void Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >::_init1<int>(long, Eigen::internal::enable_if<((((Eigen::DenseBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >::{unnamed type#1})-1)!=(1))||(!Eigen::internal::is_convertible<int, double>::value))&&((!((Eigen::internal::is_same<Eigen::MatrixXpr, Eigen::ArrayXpr>::{unnamed type#1})0))||((({unnamed type#1})-1)==Eigen::Dynamic)), Eigen::internal::is_convertible>::type*) (PlainObjectBase.h:776)
==7320==    by 0x492C60: Eigen::Matrix<double, -1, 1, 0, -1, 1>::Matrix<int>(int const&) (Matrix.h:296)
==7320==    by 0x5C533FF: Eigen::internal::plain_matrix_type<pinocchio::helper::argument_type<void (Eigen::Matrix<double, -1, 1, 0, -1, 1>)>::type, Eigen::internal::traits<pinocchio::helper::argument_type<void (Eigen::Matrix<double, -1, 1, 0, -1, 1>)>::type>::StorageKind>::type pinocchio::difference<pinocchio::LieGroupMap, double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, 0, Eigen::InnerStride<1> > >(pinocchio::ModelTpl<double, 0, pinocchio::JointCollectionDefaultTpl> const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, Eigen::MatrixBase<Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, 0, Eigen::InnerStride<1> > > const&) (joint-configuration.hxx:399)
==7320==    by 0x5C52DA8: Eigen::internal::plain_matrix_type<pinocchio::helper::argument_type<void (Eigen::Matrix<double, -1, 1, 0, -1, 1>)>::type, Eigen::internal::traits<pinocchio::helper::argument_type<void (Eigen::Matrix<double, -1, 1, 0, -1, 1>)>::type>::StorageKind>::type pinocchio::difference<double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, 0, Eigen::InnerStride<1> > >(pinocchio::ModelTpl<double, 0, pinocchio::JointCollectionDefaultTpl> const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, Eigen::MatrixBase<Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, 0, Eigen::InnerStride<1> > > const&) (joint-configuration.hpp:845)
==7320==    by 0x5C52129: tsid::tasks::TaskJointPosture::compute(double, Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, 0, Eigen::InnerStride<1> >, Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, 0, Eigen::InnerStride<1> >, pinocchio::DataTpl<double, 0, pinocchio::JointCollectionDefaultTpl>&) (task-joint-posture.cpp:161)
==7320==    by 0x5E9193D: tsid::InverseDynamicsFormulationAccForce::computeProblemData(double, Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, 0, Eigen::InnerStride<1> >, Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, 0, Eigen::InnerStride<1> >) (inverse-dynamics-formulation-acc-force.cpp:355)```

I think there is a Eigen Vector size problem here

m_p_error = pinocchio::difference(m_robot.model(), m_ref_q_augmented, q);
m_p_error = m_p_error.tail(m_robot.na());

Changing it to
m_p_error = pinocchio::difference(m_robot.model(), m_ref_q_augmented, q.tail(m_ref_q_augmented.size())).tail(m_robot.na());
solves the issue on my computer

Would that be ok ?

[nim65s]

Hi @dalinel,

I can reproduce your issue and your fix, so this looks OK for me, thanks !

Can you open a PR ?

ping @ABonnefoy : you might want to check if this help with your current issues :)

[andreadelprete]

Thanks for spotting this issue!
My guess is that taking the tail of q is not necessary, as q and m_ref_q_augmented should have the same size already, so it would make more sense to do:

 m_p_error = pinocchio::difference(m_robot.model(), m_ref_q_augmented, q).tail(m_robot.na());

[dalinel]

Ok perfect thanks !
I have opened #169

[nim65s]

Fixed in #169