head IK is now working fine

source/RobotAPI/motioncontrol/MotionControl.cpp

@@ -57,7 +57,6 @@ void MoveJoints::defineParameters()
     addToInput("jointMaxSpeed", VariantType::List(VariantType::Float), true);
 
     addToInput("jointTargetTolerance", VariantType::Float, false);
-
     addToInput("timeoutInMs", VariantType::Int, false);
 
     addToOutput("jointDistancesToTargets", VariantType::List(VariantType::Float), true);
@@ -86,8 +85,6 @@ void MoveJoints::onEnter()
     // TODO: Set Max Velocities
 
     // now install the condition
-
-    //ExpressionPtr cond = Expression::create();
     Term cond;
     float tolerance = getInput<float>("jointTargetTolerance");
     for (int i=0; i<jointNames->getSize(); i++)
@@ -515,7 +512,8 @@ void CalculateJointAngleConfiguration::run()
 //    ARMARX_INFO << "GOAL TCP:" << goalTmpTCP << endl;
 
     ARMARX_INFO << "Calculating IK" << flush;
-    if (!ikSolver->solve(goalGlobal, VirtualRobot::IKSolver::Position, 50))
+    //if (!ikSolver->solve(goalGlobal, VirtualRobot::IKSolver::Position, 50))
+    if (!ikSolver->solve(goalGlobal, VirtualRobot::IKSolver::All, 50))
     {
         ARMARX_WARNING << "IKSolver found no solution! " << flush;
         sendEvent<EvFailure>();
@@ -763,6 +761,8 @@ void HeadLookAtTarget::defineParameters()
 {
     addToInput("target", VariantType::FramedPosition, false);
     addToInput("headIKKinematicChainName", VariantType::String, false);
+    addToInput("jointTargetTolerance", VariantType::Float, false);
+    addToInput("timeoutInMs", VariantType::Int, false);
 }
 
 
@@ -777,9 +777,10 @@ void HeadLookAtTarget::defineSubstates()
     ParameterMappingPtr transitionMapping = ParameterMapping::createMapping()->mapFromOutput("*", "*")->mapFromParent("*", "*");
 
     setInitState(stateCalculateHeadIK, initialMapping);
-    addTransition<EvSuccess>(stateCalculateHeadIK, stateMoveHeadJoints, transitionMapping);
-    addTransition<EvSuccess>(stateMoveHeadJoints, stateSuccess, transitionMapping);
+    addTransition<EvCalculationDone>(stateCalculateHeadIK, stateMoveHeadJoints, transitionMapping);
+    addTransition<EvJointTargetReached>(stateMoveHeadJoints, stateSuccess, transitionMapping);
     addTransitionFromAllStates<EvFailure>(stateFailure);
+    addTransitionFromAllStates<EvTimeout>(stateFailure);
 }
 
 
@@ -789,20 +790,32 @@ void CalculateHeadIK::defineParameters()
 {
     addToInput("target", VariantType::FramedPosition, false);
     addToInput("headIKKinematicChainName", VariantType::String, false);
+
+    addToOutput("jointNames", VariantType::List(VariantType::String), true);
+    addToOutput("targetJointValues", VariantType::List(VariantType::Float), true);
+}
+
+
+void CalculateHeadIK::onEnter()
+{
+    targetPosition = getInput<FramedPosition>("target");
+    kinematicChainName = getInput<std::string>("headIKKinematicChainName");
 }
 
 
 void CalculateHeadIK::run()
 {
-    //RobotStatechartContext* context = getContext<RobotStatechartContext>();
-    //VirtualRobot::RobotPtr robot(new RemoteRobot(context->robotStateComponent->getRobotSnapshot("CalculateTCPPoseTime")));
+    RobotStatechartContext* context = getContext<RobotStatechartContext>();
+    VirtualRobot::RobotPtr robotSnapshot(new RemoteRobot(context->robotStateComponent->getRobotSnapshot("CalculateTCPPoseTime")));
     // TODO: with the following line, the IK doesn't find a solution, so this terrible hack must be used. Fix it!!!
     //VirtualRobot::RobotPtr robot = robotPtr->clone("CalculateTCPPoseClone");
+    VirtualRobot::RobotConfigPtr robotSnapshotConfig = robotSnapshot->getConfig();
     std::string robotModelFile;
     ArmarXDataPath::getAbsolutePath("Armar3/data/robotmodel/ArmarIII.xml", robotModelFile);
-    VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotModelFile.c_str());
+    VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotModelFile.c_str(), VirtualRobot::RobotIO::eStructure);
+    robot->setConfig(robotSnapshotConfig);
 
-    VirtualRobot::RobotNodeSetPtr kinematicChain = robot->getRobotNodeSet(getInput<std::string>("kinematicChainName"));
+    VirtualRobot::RobotNodeSetPtr kinematicChain = robot->getRobotNodeSet(kinematicChainName);
     VirtualRobot::RobotNodePrismaticPtr virtualJoint;
     for (unsigned int i=0; i<kinematicChain->getSize(); i++)
     {
@@ -814,18 +827,18 @@ void CalculateHeadIK::run()
     VirtualRobot::GazeIK ikSolver(kinematicChain, virtualJoint);
     ikSolver.enableJointLimitAvoidance(true);
 
-    FramedPositionPtr targetPosition = getInput<FramedPosition>("target");
-    VirtualRobot::LinkedCoordinate target = ArmarPose::createLinkedCoordinate(robot, targetPosition, FramedOrientationPtr());
-    ARMARX_INFO << "target: " << target.getPose() << armarx::flush;
+    Eigen::Matrix3f m = Eigen::Matrix3f::Identity();
+    FramedOrientationPtr targetOri = new FramedOrientation(m, targetPosition->frame);
+    VirtualRobot::LinkedCoordinate target = ArmarPose::createLinkedCoordinate(robot, targetPosition, targetOri);
+    ARMARX_INFO << "Target: " << target.getPose() << armarx::flush;
     Eigen::Matrix4f goalInRoot = target.getInFrame(robot->getRootNode());
-    Eigen::Matrix4f goalGlobal = robot->getRootNode()->toGlobalCoordinateSystem(goalInRoot);
-    ARMARX_VERBOSE << "GOAL in root: " << goalInRoot << armarx::flush;
-    ARMARX_VERBOSE << "GOAL global: " << goalGlobal << armarx::flush;
+    //Eigen::Matrix4f goalGlobal = robot->getRootNode()->toGlobalCoordinateSystem(goalInRoot);
+    Eigen::Vector3f targetPos = goalInRoot.block<3,1>(0,3);
 
-    ARMARX_INFO << "Calculating IK" << flush;
-    if (!ikSolver.solve(goalGlobal.block<3,1>(0,3)))
+    ARMARX_INFO << "Calculating IK for target position " << targetPos;
+    if (!ikSolver.solve(targetPos))
     {
-        ARMARX_WARNING << "IKSolver found no solution! " << flush;
+        ARMARX_WARNING << "IKSolver found no solution! ";
         sendEvent<EvFailure>();
     }
     else

source/RobotAPI/motioncontrol/MotionControl.h

@@ -326,7 +326,10 @@ namespace MotionControl
     struct CalculateHeadIK : StateTemplate<CalculateHeadIK>
     {
         void defineParameters();
+        void onEnter();
         void run();
+        FramedPositionPtr targetPosition;
+        std::string kinematicChainName;
     };
 
 } // namespace MotionControl