Python invdyn.getContactForces assumes 12dim contact

[jviereck]

When calling the invdyn.checkContact(contact.name, sol) and invdyn.getContact(contact.name, sol) for a ContactPoint, there is an assertion error at

#4  0x00007fffc8496682 in tsid::InverseDynamicsFormulationAccForce::getContactForces (this=0x1bd8980, 
    name="BL_contact", sol=..., f=...)
    at /home/jviereck/dev/tools/tsid/src/formulations/inverse-dynamics-formulation-acc-force.cpp:468
468	      assert(f.size()==k);
(gdb) p f.size
$1 = {Eigen::EigenBase<Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1>, 0, Eigen::InnerStride<1> > >::Index (const Eigen::EigenBase<Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1>, 0, Eigen::InnerStride<1> > > * const)} 0x7fffc8200b0a
     <Eigen::EigenBase<Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1>, 0, Eigen::InnerStride<1> > >::size() const>
(gdb) p f.size()
$2 = 12
(gdb) p k
$3 = 3
(gdb) list
463	  for(std::vector<ContactLevel*>::iterator it=m_contacts.begin(); it!=m_contacts.end(); it++)
464	  {
465	    if((*it)->contact.name()==name)
466	    {
467	      const int k = (*it)->contact.n_force();
468	      assert(f.size()==k);
469	      f = m_f.segment((*it)->index, k);
470	      return true;
471	    }
472	  }

That is as the python wrapper calls the getContactForces method with a 12 dimensional force vector, though a ContactPoint has only 3 dimensional force vector. See 1.

@andreadelprete, so you have a good idea how to solve this? One way would be to add a way from python / C++ to query the dimension of the contact. Once the dimensionality is obtained, call the getContactForce with the right sized vector.

[andreadelprete]

Fixed by commit 5f7453c

[jviereck]

Thanks for the fast fixes @andreadelprete !