diff --git a/data/RobotAPI/robots/Armar3/ArmarIII-Head.xml b/data/RobotAPI/robots/Armar3/ArmarIII-Head.xml index 68b453a3fafa5be3e9edfaae1e15455ae349d4db..045cd6af4de2f49f643ccb75c6469682b3717e4a 100644 --- a/data/RobotAPI/robots/Armar3/ArmarIII-Head.xml +++ b/data/RobotAPI/robots/Armar3/ArmarIII-Head.xml @@ -189,7 +189,7 @@ </Transform> <Joint type="revolute"> <Axis x="0" y="0" z="-1"/> - <Limits unit="degree" lo="-28" hi="5"/> <!-- Real high limit is 27. Deactivated to avoid unnatural poses--> + <Limits unit="degree" lo="-24" hi="5"/> <!-- Real high limit is 27. Deactivated to avoid unnatural poses--> <MaxVelocity unit="radian" value="1"/> <MaxAcceleration value="10"/> <MaxTorque value="3000"/> diff --git a/source/RobotAPI/components/units/HeadIKUnit.cpp b/source/RobotAPI/components/units/HeadIKUnit.cpp index 0ab897d7df74739d7cd7921a6cf0f81a1c25a8e2..46dcd0ab50fc6bca4e71b3015614707f949fde63 100644 --- a/source/RobotAPI/components/units/HeadIKUnit.cpp +++ b/source/RobotAPI/components/units/HeadIKUnit.cpp @@ -280,47 +280,56 @@ namespace armarx if (!solutionFound) { - ARMARX_WARNING << "IKSolver found no solution!"; - } - else - { - ARMARX_DEBUG << "Solution found"; - - if (drawer && localRobot->hasRobotNode("Cameras")) + Eigen::Vector3f position = globalPos->toEigen() - kinematicChain->getTCP()->getGlobalPose().block(0, 3, 3, 1); + float error = position.norm(); + if (error < 200) + { + ARMARX_INFO << "IKSolver found no solution! applying best solution with " << error << "mm error"; + } + else { - Vector3Ptr startPos = new Vector3(localRobot->getRobotNode("Cameras")->getGlobalPose()); - drawer->setSphereDebugLayerVisu("HeadViewTargetSolution", - startPos, - DrawColor {0, 1, 1, 0.2}, - 17); + ARMARX_WARNING << "IKSolver found no solution! " << error << "mm error"; + return; } + } + ARMARX_DEBUG << "Solution found"; - NameValueMap targetJointAngles; - NameControlModeMap controlModes; + if (drawer && localRobot->hasRobotNode("Cameras")) + { + Vector3Ptr startPos = new Vector3(localRobot->getRobotNode("Cameras")->getGlobalPose()); + drawer->setSphereDebugLayerVisu("HeadViewTargetSolution", + startPos, + DrawColor {0, 1, 1, 0.2}, + 17); + } - for (int i = 0; i < (signed int)kinematicChain->getSize(); i++) - { - if (kinematicChain->getNode(i)->getName().compare("VirtualCentralGaze") != 0) - { - targetJointAngles[kinematicChain->getNode(i)->getName()] = kinematicChain->getNode(i)->getJointValue(); - controlModes[kinematicChain->getNode(i)->getName()] = ePositionControl; - } - ARMARX_DEBUG << kinematicChain->getNode(i)->getName() << ": " << kinematicChain->getNode(i)->getJointValue(); - } + NameValueMap targetJointAngles; + NameControlModeMap controlModes; - try - { - kinematicUnitPrx->switchControlMode(controlModes); - ARMARX_DEBUG << "setting new joint angles" << targetJointAngles; - kinematicUnitPrx->setJointAngles(targetJointAngles); - } - catch (...) + for (int i = 0; i < (signed int)kinematicChain->getSize(); i++) + { + if (kinematicChain->getNode(i)->getName().compare("VirtualCentralGaze") != 0) { - ARMARX_IMPORTANT << "setJointAngles failed"; + targetJointAngles[kinematicChain->getNode(i)->getName()] = kinematicChain->getNode(i)->getJointValue(); + controlModes[kinematicChain->getNode(i)->getName()] = ePositionControl; } + + ARMARX_DEBUG << kinematicChain->getNode(i)->getName() << ": " << kinematicChain->getNode(i)->getJointValue(); + } + + try + { + kinematicUnitPrx->switchControlMode(controlModes); + ARMARX_DEBUG << "setting new joint angles" << targetJointAngles; + kinematicUnitPrx->setJointAngles(targetJointAngles); } + catch (...) + { + ARMARX_IMPORTANT << "setJointAngles failed"; + } + } } diff --git a/source/RobotAPI/libraries/core/FramedPose.cpp b/source/RobotAPI/libraries/core/FramedPose.cpp index 70e4e3cfc57840b9924e6169ab7021873c50e367..facb1e3932e9b1a7ac1b4c2510f32cdfbd9bfb80 100644 --- a/source/RobotAPI/libraries/core/FramedPose.cpp +++ b/source/RobotAPI/libraries/core/FramedPose.cpp @@ -74,8 +74,10 @@ namespace armarx void FramedDirection::changeFrame(const VirtualRobot::RobotPtr& robot, const string& newFrame) { - if(frame == "") + if (frame == "") + { frame == GlobalFrame; + } if (getFrame() == newFrame) @@ -513,6 +515,13 @@ namespace armarx return; } + if (newFrame.empty()) + { + ARMARX_WARNING_S << "Frame Conversion: Frame is empty, always set frame names! ...assuming GlobalFrame"; + changeToGlobal(referenceRobot); + return; + } + Eigen::Matrix4f oldFrameTransformation = Eigen::Matrix4f::Identity(); if (!referenceRobot->hasRobotNode(newFrame))