本文整理汇总了C++中rbd::MultiBody::transform方法的典型用法代码示例。如果您正苦于以下问题：C++ MultiBody::transform方法的具体用法？C++ MultiBody::transform怎么用？C++ MultiBody::transform使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类rbd::MultiBody的用法示例。
在下文中一共展示了MultiBody::transform方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: writeUrdf
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);
}