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);
}