Topic/contact 6 d closed loop redo #1345
Conversation
This corrects the desired contact acceleration, by accouting for drift in position and velocity
for more information, see https://pre-commit.ci
for more information, see https://pre-commit.ci
for more information, see https://pre-commit.ci
Remove bindings for old data and change operation order for dvel_dq
for more information, see https://pre-commit.ci
for more information, see https://pre-commit.ci
|
Hi ! This project doesn't usually accept pull requests on the main branch. |
|
Hi @james-p-foster ! Please update the PR and change the target branch to devel. |
|
Hi ! This project doesn't usually accept pull requests on the main branch. |
There was a problem hiding this comment.
These bindings probably won't work with the changes I've made.
There was a problem hiding this comment.
I don't know how the logic should be changed to handle multiple forces in inverse dynamics. At the moment, I've preserved functionality with the non-loop models by accessing the first force_data
| std::vector<pinocchio::FrameIndex> | ||
| id_; //!< Reference frame id of the contact | ||
| std::vector<pinocchio::SE3> placements_; //!< Placement of contact relative to parent | ||
| // TODO(jfoster): only allowing one type per contact model | ||
| pinocchio::ReferenceFrame type_; //!< Type of contact |
There was a problem hiding this comment.
We now have vectors for frame ids and placements, but I kept to just one type, else it caused a mess
There was a problem hiding this comment.
With the vector of force datas, you can condense quite a lot of the logic. There are still some coupling terms between the two forces that have to remain however
| case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED: | ||
| pinocchio.dtau_dq += d_i->dtau_dq; | ||
| break; | ||
| } |
There was a problem hiding this comment.
I probably need to add this logic back in, don't know how it got deleted
| virtual ~InteractionDataAbstractTpl() {} | ||
|
|
||
| std::size_t nf; | ||
| std::vector<ForceDataAbstractTpl<Scalar>> force_datas; |
There was a problem hiding this comment.
We keep a vector of force datas to represent multiple forces, e.g. from a kinematic loop
| const boost::shared_ptr<Base>& model) | ||
| : Base(model->get_state(), model->get_type(), model->get_nc(), | ||
| const boost::shared_ptr<Base>& model, const int nf) | ||
| : Base(model->get_state(), model->get_type(), nf, model->get_nc(), |
There was a problem hiding this comment.
This could probably call model->get_nf() internally without the extra constructor arg
| if (d3d != NULL) { | ||
| found_contact = true; | ||
| contact = it->second; | ||
| if (it->second->nf == 1) { |
There was a problem hiding this comment.
This if means we only consider contact models with nf == 1
unittest/test_contacts.cpp
Outdated
There was a problem hiding this comment.
Should probably loop over all elements of force_datas instead of just the first element
unittest/test_contacts_loop.cpp
Outdated
There was a problem hiding this comment.
Are we sure the test against num diff is working as intended here? Don't we have to use some pinocchio constraints to enforce the four bar?
This PR builds off of the LAAS PR. (Let me know if it's more prudent to set that branch as the base for this PR to see the changes I've made).
ForceDataAbstract. Now, a vector of ForceDataAbstract is stored inInteractionDataAbstract, which is a new base class forContactDataAbstractandImpulseDataAbstract.nf(number of forces) argument toInteractionDataAbstract. This is exposed inContactDataAbstract, so the kinematic loop can set it to 2, but for all impulses it is hard-coded to 1.ForceDataAbstract.TODO / Questions:
test_contacts_loop.cppadded in the LAAS PR aren't as thorough as they can be. How can we make these tests meaningful?