示例#1
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())
{
}
示例#2
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())
{
}
示例#3
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();
}
示例#4
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();
	}
}