Skip to content
Snippets Groups Projects
Commit 76baa62c authored by Christian Dreher's avatar Christian Dreher
Browse files

fix: Transfer gaze targets to a dedicated gaze root frame (relative to head...

fix: Transfer gaze targets to a dedicated gaze root frame (relative to head base frame); Normalize calculation of pitch depending on selected yaw.

The issue here was that the calibration using the quadratic regression worked okay for yaw=0. However, as soon as there is a small tilt of the head base frame (i.e., the robot root frame and the head root frame where not parallel as in ARMAR-6), this calculation failed and resulted in a sinuidal movement of the head when the platform was rotated.
To fix this, the gaze targets are now transformed to a gaze root frame which in turn is relative to the head frame. Additionally, the target yaw is compensated before calculating the pitch.
parent 0063153e
No related branches found
No related tags found
Loading
{
"params": {
"gazeRootNodeName": "Gaze_Root",
"cameraNodeName": "VirtualCentralGaze",
"hemiANodeName": "Neck_2_Hemisphere_A",
"hemiBNodeName": "Neck_3_Hemisphere_B",
......
......@@ -19,9 +19,10 @@ armarx_add_library(gaze_controller_hemisphere
RobotUnit # RobotAPI
Simox::VirtualRobot
Simox::SimoxUtility
armarx_control::interface
armarx_view_selection::gaze_targets
armarx_view_selection::gaze_controller_aron
armarx_control::interface
armarx_view_selection::gaze_controller_hemisphere_aron
DEPENDENCIES_PRIVATE
range-v3::range-v3
)
#include "GazeController.h"
#include <ArmarXCore/core/logging/Logging.h>
......@@ -42,6 +41,8 @@ namespace armarx::view_selection::gaze_controller::hemisphere
_rtRobot->setThreadsafe(false);
ARMARX_CHECK_NOT_NULL(_rtRobot);
_rtGazeNodeName = configData.params.gazeRootNodeName;
_rtGazeNode = _rtRobot->getRobotNode(_rtGazeNodeName);
_rtCameraNode = _rtRobot->getRobotNode(configData.params.cameraNodeName);
_rtHemiANode = _rtRobot->getRobotNode(configData.params.hemiANodeName);
_rtHemiBNode = _rtRobot->getRobotNode(configData.params.hemiBNodeName);
......@@ -89,11 +90,13 @@ namespace armarx::view_selection::gaze_controller::hemisphere
void
GazeController::onInitNJointController()
{
;
}
void
GazeController::onConnectNJointController()
{
;
}
std::string
......@@ -109,7 +112,7 @@ namespace armarx::view_selection::gaze_controller::hemisphere
{
const float currentYawAngle = _rtYawNode->getJointValue();
// const float currentPitchAngle = _rtPitchNode->getJointValue();
const float h = _rtCameraNode->getPositionInRootFrame().z();
//const float h = _rtCameraNode->getPositionInRootFrame().z();
// report debugging variables
_publishCurrentYawAngle = currentYawAngle;
......@@ -135,57 +138,68 @@ namespace armarx::view_selection::gaze_controller::hemisphere
target.frame = GlobalFrame;
}
const Eigen::Vector3f targetPoint = target.toRootEigen(_rtRobot);
target.changeFrame(_rtRobot, _rtGazeNodeName); // Change frame to gaze root frame.
Eigen::Vector3f targetPoint = target.toEigen(); // In gaze root frame.
// compute spherical targets: pitch and yaw
float targetYawAngle = -std::atan2(targetPoint.x(), targetPoint.y());
const float targetPitchAngle = std::atan2(h - targetPoint.z(), targetPoint.y());
// derive hemisphere targets from pitch
float targetHemiA = rtGetControlStruct().params.yawToHemiFactorLinear * targetPitchAngle +
rtGetControlStruct().params.yawToHemiFactorQuadratic *
targetPitchAngle * targetPitchAngle;
float targetHemiB = rtGetControlStruct().params.yawToHemiFactorLinear * targetPitchAngle +
rtGetControlStruct().params.yawToHemiFactorQuadratic *
targetPitchAngle * targetPitchAngle;
// check reachability
const bool targetReachable = _rtYawNode->checkJointLimits(targetYawAngle) &&
_rtHemiANode->checkJointLimits(targetHemiA) &&
_rtHemiBNode->checkJointLimits(targetHemiB);
if (not targetReachable)
// The pitch depends on the yaw because in most but one case the head reference frame
// (Gaze_Root) is not axis aligned. We need to rotate the target point by the negative yaw
// angle to account for that. If this is not done, the head will show a sinuidal movement
// when the platform is rotated.
targetPoint = Eigen::AngleAxisf(-targetYawAngle, Eigen::Vector3f::UnitZ()) * targetPoint;
// Limit yaw range to avoid damage.
const bool yawTargetReachable = _rtYawNode->checkJointLimits(targetYawAngle);
if (not yawTargetReachable)
{
// limit joint ranges to avoid damage
targetYawAngle = std::clamp(
targetYawAngle, _rtYawNode->getJointLimitLow(), _rtYawNode->getJointLimitHigh());
}
targetHemiA = std::clamp(
targetHemiA, _rtHemiANode->getJointLimitLow(), _rtHemiANode->getJointLimitHigh());
// Derive target head pitch angle.
float targetPitchAngle = -std::atan2(targetPoint.z(), targetPoint.y());
// Adjust pitch to target position of hemisphere actuators using a simplified quadratic
// regression.
targetPitchAngle = (rtGetControlStruct().params.yawToHemiFactorLinear * targetPitchAngle) +
(rtGetControlStruct().params.yawToHemiFactorQuadratic *
targetPitchAngle * targetPitchAngle);
// limit pitch range to avoid damage.
const bool pitchTargetReachable = _rtHemiANode->checkJointLimits(targetPitchAngle) and
_rtHemiBNode->checkJointLimits(targetPitchAngle);
if (not pitchTargetReachable)
{
targetPitchAngle = std::clamp(targetPitchAngle,
_rtHemiANode->getJointLimitLow(),
_rtHemiANode->getJointLimitHigh());
targetHemiB = std::clamp(
targetHemiB, _rtHemiBNode->getJointLimitLow(), _rtHemiBNode->getJointLimitHigh());
targetPitchAngle = std::clamp(targetPitchAngle,
_rtHemiBNode->getJointLimitLow(),
_rtHemiBNode->getJointLimitHigh());
}
// report debugging variables
_publishTargetYawAngle = targetYawAngle;
// _publishTargetPitchAngle = targetPitchAngle;
// update control positions
_rtYawCtrlTarget->position = targetYawAngle;
_rtHemiACtrlTarget->position = targetHemiA;
_rtHemiBCtrlTarget->position = targetHemiB;
_rtHemiACtrlTarget->position = targetPitchAngle;
_rtHemiBCtrlTarget->position = targetPitchAngle;
}
void
GazeController::rtPreActivateController()
{
;
}
void
GazeController::rtPostDeactivateController()
{
;
}
void
......
......@@ -34,16 +34,15 @@
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/PIDController.h>
#include <armarx/view_selection/gaze_controller/types.h>
#include <armarx/view_selection/gaze_controller/hemisphere/aron/ControllerConfig.aron.generated.h>
#include <armarx/control/interface/ConfigurableNJointControllerInterface.h>
#include <armarx/view_selection/gaze_controller/hemisphere/aron/ControllerConfig.aron.generated.h>
#include <armarx/view_selection/gaze_controller/types.h>
#include <armarx/view_selection/gaze_targets/GazeTarget.h>
namespace armarx::view_selection::gaze_controller::hemisphere
{
TYPEDEF_PTRS_HANDLE(GazeController);
struct Config
{
arondto::Params params;
......@@ -77,7 +76,8 @@ namespace armarx::view_selection::gaze_controller::hemisphere
void updateConfig(const ::armarx::aron::data::dto::DictPtr& dto,
const Ice::Current& iceCurrent = Ice::emptyCurrent) override;
::armarx::aron::data::dto::DictPtr getConfig(const ::Ice::Current& = ::Ice::emptyCurrent) override
::armarx::aron::data::dto::DictPtr
getConfig(const ::Ice::Current& = ::Ice::emptyCurrent) override
{
ARMARX_ERROR << "NYI";
return nullptr;
......@@ -111,6 +111,8 @@ namespace armarx::view_selection::gaze_controller::hemisphere
// rt variables
VirtualRobot::RobotPtr _rtRobot;
std::string _rtGazeNodeName;
VirtualRobot::RobotNodePtr _rtGazeNode;
VirtualRobot::RobotNodePtr _rtYawNode;
VirtualRobot::RobotNodePtr _rtHemiANode;
VirtualRobot::RobotNodePtr _rtHemiBNode;
......
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<AronIncludes>
......@@ -9,6 +8,10 @@
<Object name='armarx::view_selection::gaze_controller::hemisphere::arondto::Params'>
<ObjectChild key="gazeRootNodeName">
<string />
</ObjectChild>
<ObjectChild key='cameraNodeName'>
<string />
</ObjectChild>
......@@ -32,10 +35,8 @@
<ObjectChild key='yawToHemiFactorQuadratic'>
<float32 />
</ObjectChild>
</Object>
</Object>
<Object name='armarx::view_selection::gaze_controller::hemisphere::arondto::Config' extends='armarx::view_selection::gaze_controller::arondto::ConfigBase' >
<ObjectChild key='params'>
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment