Esempio n. 1
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. 2
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();
	}
}