void PostureTask::update(const rbd::MultiBody& mb, const rbd::MultiBodyConfig& mbc) { using namespace Eigen; int pos = mb.jointPosInDof(1); // we drop the first joint (fixed or free flyier). for(int i = 1; i < mb.nrJoints(); ++i) { // if dof == 1 is a prismatic/revolute joint // else if dof == 4 is a spherical one // else is a fixed one if(mb.joint(i).dof() == 1) { eval_(pos) = q_[i][0] - mbc.q[i][0]; ++pos; } else if(mb.joint(i).dof() == 4) { Matrix3d orid( Quaterniond(q_[i][0], q_[i][1], q_[i][2], q_[i][3]).matrix()); Vector3d err = sva::rotationError(mbc.jointConfig[i].rotation(), orid); eval_.segment(pos, 3) = err; pos += 3; } } }
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 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 MotionPolyConstr::update(const rbd::MultiBody& mb, const rbd::MultiBodyConfig& mbc, const SolverData& /* data */) { computeMatrix(mb, mbc); for(std::size_t i = 0; i < jointIndex_.size(); ++i) { int index = jointIndex_[i]; int dofPos = mb.jointPosInDof(index); AL_(dofPos) += Eigen::poly_eval(torqueL_[i], mbc.q[index][0]); AU_(dofPos) += Eigen::poly_eval(torqueU_[i], mbc.q[index][0]); } }