Added ikWithOrientation input to allow position IK requests (without orientation)

Improved target reached checks by considering orientation
Optimized IK calls in order to get better and faster results
Removed robot loading code in IK state, switched to RemoteRobot::createLocalClone which is much faster
removed hard coded check for VirtualCentralGaze RobotNode from ik skill

source/RobotAPI/motioncontrol/MotionControl.cpp

@@ -244,6 +244,8 @@ void MoveTCPPoseIK::defineParameters()
 
     addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
     addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
+    addToInput("ikWithOrientation", VariantType::Bool, false);
+
     addToInput("timeoutInMs", VariantType::Int, false);
 
     addToInput("jointTargetTolerance", VariantType::Float, false);
@@ -300,8 +302,11 @@ void MoveTCPTrajectory::defineParameters()
 
     addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
     addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
+    addToInput("ikWithOrientation", VariantType::Bool, false);
+
     addToInput("timeoutInMs", VariantType::Int, false);
 
+    addToInput("jointTargetTolerance", VariantType::Float, false);
 
     addToOutput("TCPDistanceToTarget", VariantType::Float, true);
     addToOutput("TCPOrientationDistanceToTarget", VariantType::Float, true);
@@ -424,11 +429,14 @@ void MoveTCPTrajectoryNextPoint::defineParameters()
 
     addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
     addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
+    addToInput("ikWithOrientation", VariantType::Bool, false);
+
     addToInput("timeoutInMs", VariantType::Int, false);
 
     addToInput("trajectoryPointCounterID", VariantType::ChannelRef, false);
     addToOutput("trajectoryPointCounterID", VariantType::ChannelRef, false);
 
+    addToInput("jointTargetTolerance", VariantType::Float, false);
 
     addToLocal("targetTCPPosition", VariantType::FramedPosition);
     addToLocal("targetTCPOrientation", VariantType::FramedOrientation);
@@ -476,6 +484,8 @@ void CalculateJointAngleConfiguration::defineParameters()
     addToInput("targetTCPPosition", VariantType::FramedPosition, false);
     addToInput("targetTCPOrientation", VariantType::FramedOrientation, false);
     addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
+    addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
+    addToInput("ikWithOrientation", VariantType::Bool, false);
 
     addToOutput("jointNames", VariantType::List(VariantType::String), true);
     addToOutput("targetJointValues", VariantType::List(VariantType::Float), true);
@@ -490,18 +500,52 @@ void CalculateJointAngleConfiguration::run()
 
     // 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;
+    //std::string robotModelFile;
     //ArmarXDataPath::getAbsolutePath("Armar4/data/Armar4/ArmarIV.xml", robotModelFile);
-    ArmarXDataPath::getAbsolutePath("Armar3/data/robotmodel/ArmarIII.xml", robotModelFile);
-    VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotModelFile.c_str());
+    //ArmarXDataPath::getAbsolutePath("Armar3/data/robotmodel/ArmarIII.xml", robotModelFile);
+    //VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotModelFile.c_str());
+
+    std::string kinChainName = getInput<std::string>("kinematicChainName");
+    float maxError = getInput<float>("targetPositionDistanceTolerance");
+    float maxErrorRot = getInput<float>("targetOrientationDistanceTolerance");
+    bool withOrientation = getInput<bool>("ikWithOrientation");
+    VirtualRobot::IKSolver::CartesianSelection ikType = VirtualRobot::IKSolver::All;
+    if (!withOrientation)
+        ikType = VirtualRobot::IKSolver::Position;
+
+
+    RobotStatechartContext* context = getContext<RobotStatechartContext>();
+    if (!context)
+    {
+        ARMARX_WARNING << "Need a RobotStatechartContext" << flush;
+        sendEvent<EvFailure>();
+    }
 
-    VirtualRobot::GenericIKSolverPtr ikSolver(new VirtualRobot::GenericIKSolver(robot->getRobotNodeSet(getInput<std::string>("kinematicChainName"))));
+    if (!context->robotStateComponent)
+    {
+        ARMARX_WARNING << "No RobotStatechartContext->robotStateComponent" << flush;
+        sendEvent<EvFailure>();
+    }
+
+    VirtualRobot::RobotPtr robot = RemoteRobot::createLocalClone(context->robotStateComponent);
+    if (!robot)
+    {
+        ARMARX_WARNING << "Could not create a local clone of RemoteRobot" << flush;
+        sendEvent<EvFailure>();
+    }
+    if (!robot->hasRobotNodeSet(kinChainName))
+    {
+        ARMARX_WARNING << "Robot does not know kinematic chain with name " << kinChainName << flush;
+        sendEvent<EvFailure>();
+    }
+    ARMARX_INFO << "Setting up ik solver for kin chain: " << kinChainName << ", max position error:" << maxError << ", max orientation erorr " << maxErrorRot << ", useOrientation:" << withOrientation << armarx::flush;
+    VirtualRobot::GenericIKSolverPtr ikSolver(new VirtualRobot::GenericIKSolver(robot->getRobotNodeSet(kinChainName), VirtualRobot::JacobiProvider::eSVDDamped));
     ikSolver->setVerbose(true);
-    ikSolver->setMaximumError(getInput<float>("targetPositionDistanceTolerance"));
-    ikSolver->setupJacobian(0.1, 40);
+    ikSolver->setMaximumError(maxError,maxErrorRot);
+    ikSolver->setupJacobian(0.6f, 10);
 
     VirtualRobot::LinkedCoordinate target = ArmarPose::createLinkedCoordinate(robot, getInput<FramedPosition>("targetTCPPosition"), getInput<FramedOrientation>("targetTCPOrientation"));
-    ARMARX_INFO << "target: " << target.getPose() << armarx::flush;
+    ARMARX_INFO << "IK target: " << target.getPose() << armarx::flush;
     Eigen::Matrix4f goalInRoot = target.getInFrame(robot->getRootNode());
 
 //    // test
@@ -514,7 +558,7 @@ void CalculateJointAngleConfiguration::run()
 
     ARMARX_INFO << "Calculating IK" << flush;
     //if (!ikSolver->solve(goalGlobal, VirtualRobot::IKSolver::Position, 50))
-    if (!ikSolver->solve(goalGlobal, VirtualRobot::IKSolver::All, 50))
+    if (!ikSolver->solve(goalGlobal, ikType, 5))
     {
         ARMARX_WARNING << "IKSolver found no solution! " << flush;
         sendEvent<EvFailure>();
@@ -524,15 +568,12 @@ void CalculateJointAngleConfiguration::run()
 
         SingleTypeVariantList jointNames = SingleTypeVariantList(VariantType::String);
         SingleTypeVariantList targetJointValues = SingleTypeVariantList(VariantType::Float);
-        VirtualRobot::RobotNodeSetPtr kinematicChain = robot->getRobotNodeSet(getInput<std::string>("kinematicChainName") );
+        VirtualRobot::RobotNodeSetPtr kinematicChain = robot->getRobotNodeSet(kinChainName);
         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;
-            }
+            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;
@@ -551,6 +592,7 @@ void ValidateJointAngleConfiguration::defineParameters()
     addToInput("targetTCPOrientation", VariantType::FramedOrientation, false);
     addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
     addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
+    addToInput("ikWithOrientation", VariantType::Bool, false);
     addToInput("kinematicChainName", VariantType::String, false);
 
     addToOutput("TCPDistanceToTarget", VariantType::Float, true);
@@ -582,7 +624,8 @@ void ValidateJointAngleConfiguration::onEnter()
 
     setOutput("TCPDistanceToTarget", cartesianDistance);
     setOutput("TCPOrientationDistanceToTarget", orientationDistance);
-    if (cartesianDistance <= getInput<float>("targetPositionDistanceTolerance") && orientationDistance <= getInput<float>("targetOrientationDistanceTolerance"))
+    bool withOri = getInput<bool>("ikWithOrientation");
+    if (cartesianDistance <= getInput<float>("targetPositionDistanceTolerance") && (!withOri || orientationDistance <= getInput<float>("targetOrientationDistanceTolerance")))
     {
         sendEvent<EvSuccess>();
     }
@@ -643,6 +686,8 @@ void MotionControlTestStateIK::defineParameters()
 
     addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
     addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
+    addToInput("ikWithOrientation", VariantType::Bool, false);
+
     addToInput("timeoutInMs", VariantType::Int, false);
 
 
@@ -993,6 +1038,7 @@ void CalculateHeadIK::onEnter()
 {
     targetPosition = getInput<FramedPosition>("target");
     kinematicChainName = getInput<std::string>("headIKKinematicChainName");
+    virtualPrismaticJointName = getInput<std::string>("headIKVirtualPrismaticJointName");
 }
 
 
@@ -1008,15 +1054,33 @@ void CalculateHeadIK::run()
     VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotModelFile.c_str(), VirtualRobot::RobotIO::eStructure);
     robot->setConfig(robotSnapshotConfig);
 
+    if (!robot)
+    {
+        ARMARX_WARNING << "No robot!" << flush;
+        sendEvent<EvFailure>();
+    }
+
     VirtualRobot::RobotNodeSetPtr kinematicChain = robot->getRobotNodeSet(kinematicChainName);
-    VirtualRobot::RobotNodePrismaticPtr virtualJoint;
-    for (unsigned int i=0; i<kinematicChain->getSize(); i++)
+    if (!kinematicChain)
+    {
+        ARMARX_WARNING << "No kinematicChain with name " << kinematicChainName << flush;
+        sendEvent<EvFailure>();
+    }
+
+    VirtualRobot::RobotNodePrismaticPtr virtualJoint = boost::dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(robot->getRobotNode(virtualPrismaticJointName));
+    if (!virtualJoint)
+    {
+        ARMARX_WARNING << "No virtualJoint with name " << virtualPrismaticJointName << flush;
+        sendEvent<EvFailure>();
+    }
+
+    /*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);
 
@@ -1041,7 +1105,7 @@ void CalculateHeadIK::run()
         SingleTypeVariantList targetJointValues = SingleTypeVariantList(VariantType::Float);
         for (int i = 0; i < (signed int)kinematicChain->getSize(); i++)
         {
-            if (kinematicChain->getNode(i)->getName().compare("VirtualCentralGaze") != 0)
+            if (kinematicChain->getNode(i)->getName().compare(virtualPrismaticJointName) != 0)
             {
                 jointNames.addVariant(Variant(kinematicChain->getNode(i)->getName()));
                 targetJointValues.addVariant(Variant(kinematicChain->getNode(i)->getJointValue()));

source/RobotAPI/motioncontrol/MotionControl.h

@@ -59,6 +59,7 @@ namespace MotionControl
     - targetTCPOrientation: the target orientation
     - targetPositionDistanceTolerance: tolerance for the position to decide if the motion was successfull
     - targetOrientationDistanceTolerance: tolerance for the orientation to decide if the motion was successfull
+    - ikWithOrientation: Consider orientation for IK computation. if false, the rotation of the TCP is not considered.
     - timeoutInMs: a timeout after which the motion is aborted
     Output parameters:
     - TCPDistanceToTarget: kartesian distance between TCP and target position
@@ -72,6 +73,7 @@ namespace MotionControl
     - targetTCPOrientations: the list of target orientations
     - targetPositionDistanceTolerance: tolerance for the position to decide if the motion was successfull
     - targetOrientationDistanceTolerance: tolerance for the orientation to decide if the motion was successfull
+    - ikWithOrientation: Consider orientation for IK computation. if false, the rotation of the TCP is not considered.
     - timeoutInMs: a timeout after which the motion is aborted
     Output parameters:
     - TCPDistanceToTarget: kartesian distance between TCP and target position
@@ -169,6 +171,7 @@ namespace MotionControl
     *   \param targetTCPPosition the target position for the TCP
     *   \param targetTCPOrientation the target orientation
     *   \param targetPositionDistanceTolerance tolerance for the position to decide if the motion was successfull
+    *   \param ikWithOrientation: Consider orientation for IK computation. if false, the rotation of the TCP is not considered.
     *   \param targetOrientationDistanceTolerance tolerance for the orientation to decide if the motion was successfull
     *   \param timeoutInMs a timeout after which the motion is aborted
     */
@@ -195,6 +198,7 @@ namespace MotionControl
     *   \param targetTCPOrientations the list of target orientations
     *   \param targetPositionDistanceTolerance tolerance for the position to decide if the motion was successfull
     *   \param targetOrientationDistanceTolerance tolerance for the orientation to decide if the motion was successfull
+    *   \param ikWithOrientation: Consider orientation for IK computation. if false, the rotation of the TCP is not considered.
     *   \param timeoutInMs a timeout after which the motion is aborted
     */
     struct MoveTCPTrajectory : StateTemplate<MoveTCPTrajectory>
@@ -377,6 +381,7 @@ namespace MotionControl
         void run();
         FramedPositionPtr targetPosition;
         std::string kinematicChainName;
+        std::string virtualPrismaticJointName;
     };
 
 } // namespace MotionControl