Ejemplo n.º 1
0
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())
{}
Ejemplo n.º 2
0
CoMTask::CoMTask(const rbd::MultiBody& mb, const Eigen::Vector3d& com,
								std::vector<double> weight):
	com_(com),
	jac_(mb, std::move(weight)),
	eval_(3),
	speed_(3),
	normalAcc_(3),
	jacMat_(3, mb.nrDof()),
	jacDotMat_(3, mb.nrDof())
{}
Ejemplo n.º 3
0
OrientationTask::OrientationTask(const rbd::MultiBody& mb, int bodyId, const Eigen::Matrix3d& ori):
	ori_(ori),
	bodyIndex_(mb.bodyIndexById(bodyId)),
	jac_(mb, bodyId),
	eval_(3),
	speed_(3),
	normalAcc_(3),
	jacMat_(3, mb.nrDof()),
	jacDotMat_(3, mb.nrDof())
{
}
Ejemplo n.º 4
0
PositionTask::PositionTask(const rbd::MultiBody& mb, int bodyId,
	const Eigen::Vector3d& pos, const Eigen::Vector3d& bodyPoint):
	pos_(pos),
	point_(bodyPoint),
	bodyIndex_(mb.bodyIndexById(bodyId)),
	jac_(mb, bodyId, bodyPoint),
	eval_(3),
	speed_(3),
	normalAcc_(3),
	jacMat_(3, mb.nrDof()),
	jacDotMat_(3, mb.nrDof())
{
}
Ejemplo n.º 5
0
MotionConstrCommon::MotionConstrCommon(const rbd::MultiBody& mb):
	fd_(mb),
	cont_(),
	fullJac_(6, mb.nrDof()),
	A_(),
	AL_(),
	AU_(),
	XL_(),
	XU_(),
	curTorque_(mb.nrDof()),
	nrDof_(0),
	nrFor_(0),
	nrTor_(0)
{
}
Ejemplo n.º 6
0
LinVelocityTask::LinVelocityTask(const rbd::MultiBody& mb, int bodyId,
	const Eigen::Vector3d& v, const Eigen::Vector3d& bodyPoint):
	vel_(v),
	point_(bodyPoint),
	bodyIndex_(mb.bodyIndexById(bodyId)),
	jac_(mb, bodyId, bodyPoint),
	eval_(3),
	speed_(3),
	normalAcc_(3),
	jacMat_(3, mb.nrDof()),
	jacDotMat_(3, mb.nrDof())
{
	// this task don't have any derivative
	speed_.setZero();
}
Ejemplo n.º 7
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});
	}
}
Ejemplo n.º 8
0
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_);
}
Ejemplo n.º 9
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);
}
Ejemplo n.º 10
0
OrientationTrackingTask::OrientationTrackingTask(const rbd::MultiBody& mb,
	int bodyId, const Eigen::Vector3d& bodyPoint, const Eigen::Vector3d& bodyAxis,
	const std::vector<int>& trackingJointsId,
	const Eigen::Vector3d& trackedPoint):
	bodyIndex_(mb.bodyIndexById(bodyId)),
	bodyPoint_(bodyPoint),
	bodyAxis_(bodyAxis),
	zeroJacIndex_(),
	trackedPoint_(trackedPoint),
	jac_(mb, bodyId),
	eval_(3),
	shortJacMat_(3, jac_.dof()),
	jacMat_(3, mb.nrDof()),
	jacDotMat_(3, mb.nrDof())
{
	std::set<int> trackingJointsIndex;
	for(int id: trackingJointsId)
	{
		trackingJointsIndex.insert(mb.jointIndexById(id));
	}

	int jacPos = 0;
	for(int i: jac_.jointsPath())
	{
		const rbd::Joint& curJoint = mb.joint(i);
		if(trackingJointsIndex.find(i) == std::end(trackingJointsIndex))
		{
			for(int j = 0; j < curJoint.dof(); ++j)
			{
				zeroJacIndex_.push_back(jacPos + j);
			}
		}

		jacPos += curJoint.dof();
	}
}
Ejemplo n.º 11
0
MotionConstr::MotionConstr(const rbd::MultiBody& mb,
													std::vector<std::vector<double>> lTorqueBounds,
													std::vector<std::vector<double>> uTorqueBounds):
	MotionConstrCommon(mb),
	torqueL_(),
	torqueU_()
{
	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_);
}
Ejemplo n.º 12
0
PostureTask::PostureTask(const rbd::MultiBody& mb, std::vector<std::vector<double> > q):
	q_(q),
	eval_(mb.nrDof()),
	jacMat_(mb.nrDof(), mb.nrDof()),
	jacDotMat_(mb.nrDof(), mb.nrDof())
{
	eval_.setZero();
	jacMat_.setIdentity();
	jacDotMat_.setZero();

	if(mb.nrDof() > 0 && mb.joint(0).type() == rbd::Joint::Free)
	{
		for(int i = 0; i < 6; ++i)
		{
			jacMat_(i, i) = 0;
		}
	}
}
Ejemplo n.º 13
0
IDIM::IDIM(const rbd::MultiBody& mb):
	Y_(Eigen::MatrixXd::Zero(mb.nrDof(), mb.nrBodies()*10))
{ }