diff --git a/source/RobotAPI/libraries/core/FramedPose.cpp b/source/RobotAPI/libraries/core/FramedPose.cpp index facb1e3932e9b1a7ac1b4c2510f32cdfbd9bfb80..2ea1a8f0fd280b3e495cab1cdf49eed109012810 100644 --- a/source/RobotAPI/libraries/core/FramedPose.cpp +++ b/source/RobotAPI/libraries/core/FramedPose.cpp @@ -4,9 +4,8 @@ #include <VirtualRobot/Robot.h> #include <VirtualRobot/LinkedCoordinate.h> -#include <VirtualRobot/LinkedCoordinate.h> #include <VirtualRobot/VirtualRobot.h> - +#include <VirtualRobot/RobotConfig.h> using namespace Eigen; using namespace std; @@ -76,7 +75,7 @@ namespace armarx { if (frame == "") { - frame == GlobalFrame; + frame = GlobalFrame; } @@ -205,7 +204,7 @@ namespace armarx else { auto node = robotState->getRobotNode(oldFrame); - ARMARX_CHECK_EXPRESSION_W_HINT(node, "old frame: " + oldFrame); + ARMARX_CHECK_EXPRESSION_W_HINT(node, "old frame: " + oldFrame + ValueToString(robotState->getConfig()->getRobotNodeJointValueMap())); auto newNode = robotState->getRobotNode(newFrame); ARMARX_CHECK_EXPRESSION_W_HINT(newNode, newFrame); toNewFrame = node->getTransformationTo(newNode);