C++ rbd::MultiBody类代码示例

MotionSpringConstr::MotionSpringConstr(const rbd::MultiBody& mb,
																		 std::vector<std::vector<double>> lTorqueBounds,
																		 std::vector<std::vector<double>> uTorqueBounds,
																		 const std::vector<SpringJoint>& springs):
	MotionConstrCommon(mb),
	torqueL_(),
	torqueU_(),
	springs_()
{
	int vars = mb.nrDof() - mb.joint(0).dof();
	torqueL_.resize(vars);
	torqueU_.resize(vars);

	// remove the joint 0
	lTorqueBounds[0] = {};
	uTorqueBounds[0] = {};

	rbd::paramToVector(lTorqueBounds, torqueL_);
	rbd::paramToVector(uTorqueBounds, torqueU_);

	springs_.reserve(springs.size());
	for(const SpringJoint& sj: springs)
	{
		int index = mb.jointIndexById(sj.jointId);
		int posInDof = mb.jointPosInDof(index) - mb.joint(0).dof();
		springs_.push_back({index, posInDof, sj.K, sj.C, sj.O});
	}
}

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

CoMTask::CoMTask(const rbd::MultiBody& mb, const Eigen::Vector3d& com):
	com_(com),
	jac_(mb),
	eval_(3),
	speed_(3),
	normalAcc_(3),
	jacMat_(3, mb.nrDof()),
	jacDotMat_(3, mb.nrDof())
{}

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

void checkMultiBodyNames(const rbd::MultiBody& mb,
											 const std::vector<std::string>& bodies,
											 const std::vector<std::string>& joints)
{
	for(const rbd::Body& b: mb.bodies())
	{
		BOOST_CHECK(std::find(bodies.begin(), bodies.end(), b.name()) != bodies.end());
	}
	for(const rbd::Joint& j: mb.joints())
	{
		BOOST_CHECK(std::find(joints.begin(), joints.end(), j.name()) != joints.end());
	}
}

void MotionConstr::update(const rbd::MultiBody& mb, const rbd::MultiBodyConfig& mbc,
	const SolverData& /* data */)
{
	computeMatrix(mb, mbc);

	AL_.segment(mb.joint(0).dof(), nrTor_) += torqueL_;
	AU_.segment(mb.joint(0).dof(), nrTor_) += torqueU_;
}

void QPSolver::nrVars(const rbd::MultiBody& mb,
	std::vector<UnilateralContact> uni,
	std::vector<BilateralContact> bi)
{
	data_.normalAccB_.resize(mb.nrBodies());
	data_.alphaD_ = mb.nrDof();
	data_.lambda_ = 0;
	data_.torque_ = (mb.nrDof() - mb.joint(0).dof());
	data_.uniCont_ = uni;
	data_.biCont_ = bi;

	// counting unilateral contact
	for(const UnilateralContact& c: data_.uniCont_)
	{
		for(std::size_t i = 0; i < c.points.size(); ++i)
		{
			data_.lambda_ += c.nrLambda(int(i));
		}
	}
	data_.lambdaUni_ = data_.lambda_;

	// counting bilateral contact
	for(const BilateralContact& c: data_.biCont_)
	{
		for(std::size_t i = 0; i < c.points.size(); ++i)
		{
			data_.lambda_ += c.nrLambda(int(i));
		}
	}
	data_.lambdaBi_ = data_.lambda_ - data_.lambdaUni_;

	data_.nrVars_ = data_.alphaD_ + data_.lambda_;

	for(Task* t: tasks_)
	{
		t->updateNrVars(mb, data_);
	}

	for(Constraint* c: constr_)
	{
		c->updateNrVars(mb, data_);
	}

	solver_->updateSize(data_.nrVars_, maxEqLines_, maxInEqLines_, maxGenInEqLines_);
}

/// 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 MotionConstrCommon::torque(const rbd::MultiBody& mb, rbd::MultiBodyConfig& mbc) const
{
	int pos = mb.joint(0).dof();
	for(std::size_t i = 1; i < mbc.jointTorque.size(); ++i)
	{
		for(double& d: mbc.jointTorque[i])
		{
			d = curTorque_(pos);
			++pos;
		}
	}
}

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

IDIM::IDIM(const rbd::MultiBody& mb):
	Y_(Eigen::MatrixXd::Zero(mb.nrDof(), mb.nrBodies()*10))
{ }

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

	NormalAccCoMUpdater(const rbd::MultiBody& mb):
		normalAccB(mb.nrBodies())
	{}

std::string MotionConstrCommon::descInEq(const rbd::MultiBody& mb, int line)
{
	int jIndex = findJointFromVector(mb, line, true);
	return std::string("Joint: ") + mb.joint(jIndex).name();
}