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