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; }
std::string MotionConstrCommon::descBound(const rbd::MultiBody& mb, int line) { int start = 0; int end = 0; for(const ContactData& cd: cont_) { end += int(cd.points.size()); if(line >= start && line < end) { return std::string("Body: ") + mb.body(cd.body).name(); } start = end; } return ""; }
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); }