void Grasp::GetStance(Stance& s) const { s.clear(); for(size_t i=0;i<constraints.size();i++) s[constraints[i].link].ikConstraint = constraints[i]; for(size_t i=0;i<contacts.size();i++) s[contactLinks[i]].contacts.push_back(contacts[i]); }
void LocalContactsToStance(const ContactFormation& contacts,const RobotKinematics3D& robot,Stance& stance) { stance.clear(); for(size_t i=0;i<contacts.contacts.size();i++) { Hold h; LocalContactsToHold(contacts.contacts[i],contacts.links[i],robot,h); if(!contacts.targets.empty()) h.ikConstraint.destLink = contacts.targets[i]; stance.insert(h); } }