Esempio n. 1
0
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;
		}
	}
}
Esempio n. 2
0
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});
	}
}
Esempio n. 3
0
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);
}
Esempio n. 4
0
void MotionPolyConstr::update(const rbd::MultiBody& mb, const rbd::MultiBodyConfig& mbc,
	const SolverData& /* data */)
{
	computeMatrix(mb, mbc);

	for(std::size_t i = 0; i < jointIndex_.size(); ++i)
	{
		int index = jointIndex_[i];
		int dofPos = mb.jointPosInDof(index);
		AL_(dofPos) += Eigen::poly_eval(torqueL_[i], mbc.q[index][0]);
		AU_(dofPos) += Eigen::poly_eval(torqueU_[i], mbc.q[index][0]);
	}
}