added head IK functionality

source/RobotAPI/motioncontrol/MotionControl.cpp

 #include "MotionControl.h"
+
+// Core
+#include <Core/robotstate/remote/RemoteRobot.h>
+#include <Core/observers/variant/ChannelRef.h>
+#include <Core/observers/ConditionCheck.h>
+#include <Core/core/system/ArmarXDataPath.h>
+
+// Virtual Robot
+#include <VirtualRobot/IK/GazeIK.h>
 #include <VirtualRobot/IK/GenericIKSolver.h>
 #include <VirtualRobot/IK/DifferentialIK.h>
 #include <VirtualRobot/Nodes/RobotNode.h>
 #include <VirtualRobot/MathTools.h>
 #include <VirtualRobot/XML/RobotIO.h>
-#include <Core/robotstate/remote/RemoteRobot.h>
-#include <Core/observers/variant/ChannelRef.h>
-#include <Core/observers/ConditionCheck.h>
-
-#include <Core/core/system/ArmarXDataPath.h>
 
 #include <Eigen/src/Geometry/Quaternion.h>
 
@@ -483,7 +487,7 @@ void CalculateJointAngleConfiguration::defineParameters()
 
 void CalculateJointAngleConfiguration::run()
 {
-    RobotStatechartContext* context = getContext<RobotStatechartContext>();
+    //RobotStatechartContext* context = getContext<RobotStatechartContext>();
     //VirtualRobot::RobotPtr robot(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!!!
@@ -696,3 +700,155 @@ void MotionControl::StopRobot::onEnter()
 }
 
 
+
+
+void CloseHand::defineParameters()
+{
+    addToInput("useLeftHand", VariantType::Bool, false);
+}
+
+
+void CloseHand::onEnter()
+{
+    RobotStatechartContext* context = getContext<RobotStatechartContext>();
+    bool useLeftHand = getInput<bool>("useLeftHand");
+    NameValueMap handConfig;
+    if (useLeftHand)
+    {
+        handConfig["left_hand_configuration_actual_float"] = 4;
+    }
+    else
+    {
+        handConfig["right_hand_configuration_actual_float"] = 4;
+    }
+
+    context->kinematicUnitPrx->setJointAngles(handConfig);
+
+    sendEvent<EvSuccess>();
+}
+
+
+
+
+void OpenHand::defineParameters()
+{
+    addToInput("useLeftHand", VariantType::Bool, false);
+}
+
+
+void OpenHand::onEnter()
+{
+    RobotStatechartContext* context = getContext<RobotStatechartContext>();
+    bool useLeftHand = getInput<bool>("useLeftHand");
+    NameValueMap handConfig;
+    if (useLeftHand)
+    {
+        handConfig["left_hand_configuration_actual_float"] = 1;
+    }
+    else
+    {
+        handConfig["right_hand_configuration_actual_float"] = 1;
+    }
+
+    context->kinematicUnitPrx->setJointAngles(handConfig);
+
+    sendEvent<EvSuccess>();
+}
+
+
+
+
+
+void HeadLookAtTarget::defineParameters()
+{
+    addToInput("target", VariantType::FramedPosition, false);
+    addToInput("headIKKinematicChainName", VariantType::String, false);
+}
+
+
+void HeadLookAtTarget::defineSubstates()
+{
+    StatePtr stateCalculateHeadIK = addState<CalculateHeadIK>("CalculateHeadIKState");
+    StatePtr stateMoveHeadJoints = addState<MoveJoints>("MoveHeadJointsState");
+    StatePtr stateSuccess = addState<SuccessState>("Success");
+    StatePtr stateFailure = addState<FailureState>("Failure");
+
+    ParameterMappingPtr initialMapping = ParameterMapping::createMapping()->mapFromParent("*", "*");
+    ParameterMappingPtr transitionMapping = ParameterMapping::createMapping()->mapFromOutput("*", "*")->mapFromParent("*", "*");
+
+    setInitState(stateCalculateHeadIK, initialMapping);
+    addTransition<EvSuccess>(stateCalculateHeadIK, stateMoveHeadJoints, transitionMapping);
+    addTransition<EvSuccess>(stateMoveHeadJoints, stateSuccess, transitionMapping);
+    addTransitionFromAllStates<EvFailure>(stateFailure);
+}
+
+
+
+
+void CalculateHeadIK::defineParameters()
+{
+    addToInput("target", VariantType::FramedPosition, false);
+    addToInput("headIKKinematicChainName", VariantType::String, false);
+}
+
+
+void CalculateHeadIK::run()
+{
+    //RobotStatechartContext* context = getContext<RobotStatechartContext>();
+    //VirtualRobot::RobotPtr robot(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");
+    std::string robotModelFile;
+    ArmarXDataPath::getAbsolutePath("Armar3/data/robotmodel/ArmarIII.xml", robotModelFile);
+    VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotModelFile.c_str());
+
+    VirtualRobot::RobotNodeSetPtr kinematicChain = robot->getRobotNodeSet(getInput<std::string>("kinematicChainName"));
+    VirtualRobot::RobotNodePrismaticPtr virtualJoint;
+    for (unsigned int i=0; i<kinematicChain->getSize(); i++)
+    {
+        if (kinematicChain->getNode(i)->getName().compare("VirtualCentralGaze") == 0)
+        {
+            virtualJoint =  boost::dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(kinematicChain->getNode(i));
+        }
+    }
+    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::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;
+
+    ARMARX_INFO << "Calculating IK" << flush;
+    if (!ikSolver.solve(goalGlobal.block<3,1>(0,3)))
+    {
+        ARMARX_WARNING << "IKSolver found no solution! " << flush;
+        sendEvent<EvFailure>();
+    }
+    else
+    {
+
+        SingleTypeVariantList jointNames = SingleTypeVariantList(VariantType::String);
+        SingleTypeVariantList targetJointValues = SingleTypeVariantList(VariantType::Float);
+        for (int i = 0; i < (signed int)kinematicChain->getSize(); i++)
+        {
+            if (kinematicChain->getNode(i)->getName().compare("VirtualCentralGaze") != 0)
+            {
+                jointNames.addVariant(Variant(kinematicChain->getNode(i)->getName()));
+                targetJointValues.addVariant(Variant(kinematicChain->getNode(i)->getJointValue()));
+                ARMARX_LOG << " joint: " << kinematicChain->getNode(i)->getName() << "   value: " << kinematicChain->getNode(i)->getJointValue() << flush;
+            }
+        }
+        ARMARX_LOG << "number of joints to be set: " << jointNames.getSize() << flush;
+        ARMARX_LOG << "setting output: jointNames" << flush;
+        setOutput("jointNames", jointNames);
+        ARMARX_LOG << "setting output: targetJointValues" << flush;
+        setOutput("targetJointValues", targetJointValues);
+        sendEvent<EvCalculationDone>();
+    }
+}
+
+

source/RobotAPI/motioncontrol/MotionControl.h

@@ -225,6 +225,50 @@ namespace MotionControl
 
 
 
+
+    /**
+    *  \ingroup MotionControl
+    * Closes the given hand of the robot
+    * \param useLeftHand True if left hand should be closed, false for the right hand
+    */
+    struct CloseHand : StateTemplate<CloseHand>
+    {
+        void defineParameters();
+        void onEnter();
+    };
+
+
+
+
+    /**
+    *  \ingroup MotionControl
+    * Opens the given hand of the robot
+    * \param useLeftHand True if left hand should be opened, false for the right hand
+    */
+    struct OpenHand : StateTemplate<OpenHand>
+    {
+        void defineParameters();
+        void onEnter();
+    };
+
+
+
+
+    /**
+    *  \ingroup MotionControl
+    * Moves the head so that it looks at the specified target position
+    * \param target Position at which the robot will try to look
+    * \param headIKKinematicChainName Name of the kinematic chain for the head that will be sued in the IK
+    */
+    struct HeadLookAtTarget : StateTemplate<HeadLookAtTarget>
+    {
+        void defineParameters();
+        void defineSubstates();
+    };
+
+
+
+
     /////////////////////////////////////////////////////////////////////
     //// intermediate states
     /////////////////////////////////////////////////////////////////////
@@ -276,6 +320,15 @@ namespace MotionControl
         void onEnter();
     };
 
+
+
+
+    struct CalculateHeadIK : StateTemplate<CalculateHeadIK>
+    {
+        void defineParameters();
+        void run();
+    };
+
 } // namespace MotionControl
 
 } // namespace armarx