CoMTask::CoMTask(const rbd::MultiBody& mb, const Eigen::Vector3d& com): com_(com), jac_(mb), eval_(3), speed_(3), normalAcc_(3), jacMat_(3, mb.nrDof()), jacDotMat_(3, mb.nrDof()) {}
CoMTask::CoMTask(const rbd::MultiBody& mb, const Eigen::Vector3d& com, std::vector<double> weight): com_(com), jac_(mb, std::move(weight)), eval_(3), speed_(3), normalAcc_(3), jacMat_(3, mb.nrDof()), jacDotMat_(3, mb.nrDof()) {}
OrientationTask::OrientationTask(const rbd::MultiBody& mb, int bodyId, const Eigen::Matrix3d& ori): ori_(ori), bodyIndex_(mb.bodyIndexById(bodyId)), jac_(mb, bodyId), eval_(3), speed_(3), normalAcc_(3), jacMat_(3, mb.nrDof()), jacDotMat_(3, mb.nrDof()) { }
PositionTask::PositionTask(const rbd::MultiBody& mb, int bodyId, const Eigen::Vector3d& pos, const Eigen::Vector3d& bodyPoint): pos_(pos), point_(bodyPoint), bodyIndex_(mb.bodyIndexById(bodyId)), jac_(mb, bodyId, bodyPoint), eval_(3), speed_(3), normalAcc_(3), jacMat_(3, mb.nrDof()), jacDotMat_(3, mb.nrDof()) { }
MotionConstrCommon::MotionConstrCommon(const rbd::MultiBody& mb): fd_(mb), cont_(), fullJac_(6, mb.nrDof()), A_(), AL_(), AU_(), XL_(), XU_(), curTorque_(mb.nrDof()), nrDof_(0), nrFor_(0), nrTor_(0) { }
LinVelocityTask::LinVelocityTask(const rbd::MultiBody& mb, int bodyId, const Eigen::Vector3d& v, const Eigen::Vector3d& bodyPoint): vel_(v), point_(bodyPoint), bodyIndex_(mb.bodyIndexById(bodyId)), jac_(mb, bodyId, bodyPoint), eval_(3), speed_(3), normalAcc_(3), jacMat_(3, mb.nrDof()), jacDotMat_(3, mb.nrDof()) { // this task don't have any derivative speed_.setZero(); }
MotionSpringConstr::MotionSpringConstr(const rbd::MultiBody& mb, std::vector<std::vector<double>> lTorqueBounds, std::vector<std::vector<double>> uTorqueBounds, const std::vector<SpringJoint>& springs): MotionConstrCommon(mb), torqueL_(), torqueU_(), springs_() { int vars = mb.nrDof() - mb.joint(0).dof(); torqueL_.resize(vars); torqueU_.resize(vars); // remove the joint 0 lTorqueBounds[0] = {}; uTorqueBounds[0] = {}; rbd::paramToVector(lTorqueBounds, torqueL_); rbd::paramToVector(uTorqueBounds, torqueU_); springs_.reserve(springs.size()); for(const SpringJoint& sj: springs) { int index = mb.jointIndexById(sj.jointId); int posInDof = mb.jointPosInDof(index) - mb.joint(0).dof(); springs_.push_back({index, posInDof, sj.K, sj.C, sj.O}); } }
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_); }
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); }
OrientationTrackingTask::OrientationTrackingTask(const rbd::MultiBody& mb, int bodyId, const Eigen::Vector3d& bodyPoint, const Eigen::Vector3d& bodyAxis, const std::vector<int>& trackingJointsId, const Eigen::Vector3d& trackedPoint): bodyIndex_(mb.bodyIndexById(bodyId)), bodyPoint_(bodyPoint), bodyAxis_(bodyAxis), zeroJacIndex_(), trackedPoint_(trackedPoint), jac_(mb, bodyId), eval_(3), shortJacMat_(3, jac_.dof()), jacMat_(3, mb.nrDof()), jacDotMat_(3, mb.nrDof()) { std::set<int> trackingJointsIndex; for(int id: trackingJointsId) { trackingJointsIndex.insert(mb.jointIndexById(id)); } int jacPos = 0; for(int i: jac_.jointsPath()) { const rbd::Joint& curJoint = mb.joint(i); if(trackingJointsIndex.find(i) == std::end(trackingJointsIndex)) { for(int j = 0; j < curJoint.dof(); ++j) { zeroJacIndex_.push_back(jacPos + j); } } jacPos += curJoint.dof(); } }
MotionConstr::MotionConstr(const rbd::MultiBody& mb, std::vector<std::vector<double>> lTorqueBounds, std::vector<std::vector<double>> uTorqueBounds): MotionConstrCommon(mb), torqueL_(), torqueU_() { int vars = mb.nrDof() - mb.joint(0).dof(); torqueL_.resize(vars); torqueU_.resize(vars); // remove the joint 0 lTorqueBounds[0] = {}; uTorqueBounds[0] = {}; rbd::paramToVector(lTorqueBounds, torqueL_); rbd::paramToVector(uTorqueBounds, torqueU_); }
PostureTask::PostureTask(const rbd::MultiBody& mb, std::vector<std::vector<double> > q): q_(q), eval_(mb.nrDof()), jacMat_(mb.nrDof(), mb.nrDof()), jacDotMat_(mb.nrDof(), mb.nrDof()) { eval_.setZero(); jacMat_.setIdentity(); jacDotMat_.setZero(); if(mb.nrDof() > 0 && mb.joint(0).type() == rbd::Joint::Free) { for(int i = 0; i < 6; ++i) { jacMat_(i, i) = 0; } } }
IDIM::IDIM(const rbd::MultiBody& mb): Y_(Eigen::MatrixXd::Zero(mb.nrDof(), mb.nrBodies()*10)) { }