Eigen::VectorXd multiBodyToInertialVector(const rbd::MultiBody& mb) { Eigen::VectorXd vec(mb.nrBodies()*10, 1); for(int i = 0; i < mb.nrBodies(); ++i) { vec.segment(i*10, 10).noalias() = inertiaToVector(mb.body(i).inertia()); } return vec; }
void checkMultiBodyEq(const rbd::MultiBody& mb, std::vector<rbd::Body> bodies, std::vector<rbd::Joint> joints, std::vector<int> pred, std::vector<int> succ, std::vector<int> parent, std::vector<sva::PTransformd> Xt) { // bodies BOOST_CHECK_EQUAL_COLLECTIONS(mb.bodies().begin(), mb.bodies().end(), bodies.begin(), bodies.end()); // joints BOOST_CHECK_EQUAL_COLLECTIONS(mb.joints().begin(), mb.joints().end(), joints.begin(), joints.end()); // pred BOOST_CHECK_EQUAL_COLLECTIONS(mb.predecessors().begin(), mb.predecessors().end(), pred.begin(), pred.end()); // succ BOOST_CHECK_EQUAL_COLLECTIONS(mb.successors().begin(), mb.successors().end(), succ.begin(), succ.end()); // parent BOOST_CHECK_EQUAL_COLLECTIONS(mb.parents().begin(), mb.parents().end(), parent.begin(), parent.end()); // Xt BOOST_CHECK_EQUAL_COLLECTIONS(mb.transforms().begin(), mb.transforms().end(), Xt.begin(), Xt.end()); // nrBodies BOOST_CHECK_EQUAL(mb.nrBodies(), bodies.size()); // nrJoints BOOST_CHECK_EQUAL(mb.nrJoints(), bodies.size()); int params = 0, dof = 0; for(int i = 0; i < static_cast<int>(joints.size()); ++i) { BOOST_CHECK_EQUAL(mb.jointPosInParam(i), params); BOOST_CHECK_EQUAL(mb.jointsPosInParam()[i], params); BOOST_CHECK_EQUAL(mb.sJointPosInParam(i), params); BOOST_CHECK_EQUAL(mb.jointPosInDof(i), dof); BOOST_CHECK_EQUAL(mb.jointsPosInDof()[i], dof); BOOST_CHECK_EQUAL(mb.sJointPosInDof(i), dof); params += joints[i].params(); dof += joints[i].dof(); } BOOST_CHECK_EQUAL(params, mb.nrParams()); BOOST_CHECK_EQUAL(dof, mb.nrDof()); BOOST_CHECK_THROW(mb.sJointPosInParam(mb.nrJoints()), std::out_of_range); BOOST_CHECK_THROW(mb.sJointPosInDof(mb.nrJoints()), std::out_of_range); }
void QPSolver::nrVars(const rbd::MultiBody& mb, std::vector<UnilateralContact> uni, std::vector<BilateralContact> bi) { data_.normalAccB_.resize(mb.nrBodies()); data_.alphaD_ = mb.nrDof(); data_.lambda_ = 0; data_.torque_ = (mb.nrDof() - mb.joint(0).dof()); data_.uniCont_ = uni; data_.biCont_ = bi; // counting unilateral contact for(const UnilateralContact& c: data_.uniCont_) { for(std::size_t i = 0; i < c.points.size(); ++i) { data_.lambda_ += c.nrLambda(int(i)); } } data_.lambdaUni_ = data_.lambda_; // counting bilateral contact for(const BilateralContact& c: data_.biCont_) { for(std::size_t i = 0; i < c.points.size(); ++i) { data_.lambda_ += c.nrLambda(int(i)); } } data_.lambdaBi_ = data_.lambda_ - data_.lambdaUni_; data_.nrVars_ = data_.alphaD_ + data_.lambda_; for(Task* t: tasks_) { t->updateNrVars(mb, data_); } for(Constraint* c: constr_) { c->updateNrVars(mb, data_); } solver_->updateSize(data_.nrVars_, maxEqLines_, maxInEqLines_, maxGenInEqLines_); }
NormalAccCoMUpdater(const rbd::MultiBody& mb): normalAccB(mb.nrBodies()) {}
IDIM::IDIM(const rbd::MultiBody& mb): Y_(Eigen::MatrixXd::Zero(mb.nrDof(), mb.nrBodies()*10)) { }