Fix FrameTracking component

source/RobotAPI/components/FrameTracking/FrameTracking.cpp

@@ -169,22 +169,33 @@ void FrameTracking::_lookAtFrame(const std::string& frameName)
     syncronizeLocalClone();
     auto frame = localRobot->getRobotNode(frameName);
     auto posInRobotFrame = localRobot->toLocalCoordinateSystemVec(frame->getGlobalPosition());
+    // do nothing if the robot works above his head
+    // he should already look upwards because if this component runs continously
+    if (std::sqrt(posInRobotFrame.x()*posInRobotFrame.x() + posInRobotFrame.y()*posInRobotFrame.y()) < 300.f)
+    {
+        return;
+    }
     _lookAtPoint(posInRobotFrame);
 }
 
 void FrameTracking::_lookAtPoint(const Eigen::Vector3f& point)
 {
-    float yaw = std::acos(point.x() / point.block<2, 1>(0, 0).norm()) - static_cast<float>(M_PI) / 2.f;
+    float yaw = -std::atan2(point.x(), point.y());
+    // make shure the joint value satisfies the joint limits
     yaw = std::max(headYawJoint->getJointLimitLo(), yaw);
     yaw = std::min(headYawJoint->getJointLimitHi(), yaw);
+    // we dont want the robot to move from one limit to the other in one step
+    float currentYaw = headYawJoint->getJointValue();
+    if (!headYawJoint->isLimitless() && std::abs(currentYaw - yaw) > headYawJoint->getJointLimitHi() - headYawJoint->getJointLimitLo() - static_cast<float>(M_PI) / 8)
+    {
+        yaw = currentYaw;
+    }
 
-    //    auto pointInPitchJointFrame = headPitchJoint->toLocalCoordinateSystemVec(localRobot->toGlobalCoordinateSystemVec(point));
-    //    Eigen::Vector2f pj{pointInPitchJointFrame.y(), pointInPitchJointFrame.z() + 10};
+    auto pointInPitchJointFrame = headPitchJoint->toLocalCoordinateSystemVec(localRobot->toGlobalCoordinateSystemVec(point));
+    Eigen::Vector2f pj{pointInPitchJointFrame.y(), pointInPitchJointFrame.z()};
     float headHeightRealativeToPitchJoint = headPitchJoint->toLocalCoordinateSystemVec(cameraNode->getGlobalPosition()).z();
-    //    float pitch = static_cast<float>(M_PI) / 2.f - std::asin(pj.x() / pj.norm());// - std::asin(headHeightRealativeToPitchJoint / pj.norm());
-    auto pointInCameraFrame = cameraNode->toLocalCoordinateSystemVec(localRobot->toGlobalCoordinateSystemVec(point));
-    Eigen::Vector2f pj{pointInCameraFrame.y(), pointInCameraFrame.z() + headHeightRealativeToPitchJoint};
-    float pitch = std::asin(pj.x() / pj.norm());
+    float pitch = headPitchJoint->getJointValue() - std::asin(pj.x() / pj.norm()) + std::asin(headHeightRealativeToPitchJoint / pj.norm());
+    // make shure the joint value satisfies the joint limits
     pitch = std::max(headPitchJoint->getJointLimitLo(), pitch);
     pitch = std::min(headPitchJoint->getJointLimitHi(), pitch);
 
@@ -192,7 +203,7 @@ void FrameTracking::_lookAtPoint(const Eigen::Vector3f& point)
     auto controlModes = kinematicUnitInterfacePrx->getControlModes();
     kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::ePositionControl}, {headPitchJoint->getName(), ControlMode::ePositionControl}});
     kinematicUnitInterfacePrx->setJointAngles({{headYawJoint->getName(), yaw}, {headPitchJoint->getName(), pitch}});
-    //    kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), controlModes[headYawJoint->getName()]}, {headPitchJoint->getName(), controlModes[headPitchJoint->getName()]}});
+    kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), controlModes[headYawJoint->getName()]}, {headPitchJoint->getName(), controlModes[headPitchJoint->getName()]}});
 }
 
 void FrameTracking::_enableTracking(bool enable)