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