Ejemplo n.º 1
0
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;
}
Ejemplo n.º 2
0
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 "";
}
Ejemplo n.º 3
0
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);
}