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 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; } } }
MotionPolyConstr::MotionPolyConstr(const rbd::MultiBody& mb, const std::vector<std::vector<Eigen::VectorXd>>& lTorqueBounds, const std::vector<std::vector<Eigen::VectorXd>>& uTorqueBounds): MotionConstrCommon(mb), torqueL_(), torqueU_() { // remove non managed joint for(int i = 0; i < mb.nrJoints(); ++i) { if(mb.joint(i).dof() == 1) { jointIndex_.push_back(i); torqueL_.push_back(lTorqueBounds[i][0]); torqueU_.push_back(uTorqueBounds[i][0]); } } }
/// Compute normal acceleration (like QPSolverData::computeNormalAccB) void computeNormalAccB(const rbd::MultiBody& mb, const rbd::MultiBodyConfig& mbc, std::vector<sva::MotionVecd>& normalAccB) { const std::vector<int>& pred = mb.predecessors(); const std::vector<int>& succ = mb.successors(); for(int i = 0; i < mb.nrJoints(); ++i) { const sva::PTransformd& X_p_i = mbc.parentToSon[i]; const sva::MotionVecd& vj_i = mbc.jointVelocity[i]; const sva::MotionVecd& vb_i = mbc.bodyVelB[i]; if(pred[i] != -1) normalAccB[succ[i]] = X_p_i*normalAccB[pred[i]] + vb_i.cross(vj_i); else normalAccB[succ[i]] = vb_i.cross(vj_i); } }
void writeUrdf(const std::string& filename, const std::string& robotName, const rbd::MultiBody& mb, const Limits& limits) { urdf::ModelInterface model; model.name_ = robotName; for(const rbd::Body& b: mb.bodies()) { urdf::Link link; link.name = b.name(); if(b.inertia().mass() != 0.) { urdf::Inertial inertial; Eigen::Vector3d com = b.inertia().momentum()/b.inertia().mass(); Eigen::Matrix3d inertiaInCoM = sva::inertiaToOrigin<double>( b.inertia().inertia(), -b.inertia().mass(), com, Eigen::Matrix3d::Identity()); inertial.ixx = inertiaInCoM(0,0); inertial.ixy = inertiaInCoM(0,1); inertial.ixz = inertiaInCoM(0,2); inertial.iyy = inertiaInCoM(1,1); inertial.iyz = inertiaInCoM(1,2); inertial.izz = inertiaInCoM(2,2); inertial.origin = transformToPose(sva::PTransformd(com)); inertial.mass = b.inertia().mass(); link.inertial = boost::make_shared<urdf::Inertial>(inertial); } model.links_[b.name()] = boost::make_shared<urdf::Link>(link); } // don't read the first joint (that a virtual joint add by MultiBodyGraph) for(int i = 1; i < mb.nrJoints(); ++i) { const rbd::Joint& j = mb.joint(i); urdf::Joint joint; joint.name = j.name(); joint.child_link_name = mb.body(mb.successor(i)).name(); joint.parent_link_name = mb.body(mb.predecessor(i)).name(); joint.parent_to_joint_origin_transform = transformToPose(mb.transform(i)); fillUrdfJoint(j, limits, joint); // only export limits if there is position limits or // velocity and effort limits if((exist(limits.ql, j.id()) && exist(limits.qu, j.id())) || ((exist(limits.vl, j.id()) && exist(limits.vu, j.id())) && (exist(limits.tl, j.id()) && exist(limits.tu, j.id())))) { urdf::JointLimits urdfLimits; if(exist(limits.ql, j.id()) && exist(limits.qu, j.id())) { urdfLimits.lower = limits.ql.at(j.id()); urdfLimits.upper = limits.qu.at(j.id()); } if(exist(limits.vl, j.id()) && exist(limits.vu, j.id())) { urdfLimits.velocity = std::min(std::abs(limits.vl.at(j.id())), limits.vu.at(j.id())); } if(exist(limits.tl, j.id()) && exist(limits.tu, j.id())) { urdfLimits.effort = std::min(std::abs(limits.tl.at(j.id())), limits.tu.at(j.id())); } joint.limits = boost::make_shared<urdf::JointLimits>(urdfLimits); } model.joints_[j.name()] = boost::make_shared<urdf::Joint>(joint); } model.root_link_ = model.links_[mb.body(0).name()]; writeUrdf(filename, model); }