diff --git a/source/RobotAPI/components/units/HeadIKUnit.cpp b/source/RobotAPI/components/units/HeadIKUnit.cpp index c7ccaca529f6c7569d56d6e5880078c99b6fbaaf..953fc2bd2e690658c6e8ce11dd7cb7e79f684200 100644 --- a/source/RobotAPI/components/units/HeadIKUnit.cpp +++ b/source/RobotAPI/components/units/HeadIKUnit.cpp @@ -114,7 +114,11 @@ namespace armarx { ScopedLock lock(accessMutex); - this->robotNodeSetName = robotNodeSetName; + this->robotNodeSetNames = armarx::Split(robotNodeSetName, ","); + for (auto& setName : robotNodeSetNames) + { + boost::trim(setName); + } this->targetPosition->x = targetPosition->x; this->targetPosition->y = targetPosition->y; this->targetPosition->z = targetPosition->z; @@ -247,53 +251,95 @@ namespace armarx if (doCalculation) { ScopedLock lock(accessMutex); - RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponentPrx); - //VirtualRobot::RobotPtr robotSnapshot(new RemoteRobot(remoteRobotPrx)); - //localRobot->setConfig(robotSnapshot->getConfig()); - VirtualRobot::RobotNodeSetPtr kinematicChain = localRobot->getRobotNodeSet(robotNodeSetName); - VirtualRobot::RobotNodePrismaticPtr virtualJoint; - - for (unsigned int i = 0; i < kinematicChain->getSize(); i++) + VirtualRobot::RobotNodeSetPtr kinematicChain; + bool foundSolution = false; + NameValueMap targetJointAngles; + NameControlModeMap controlModes; + std::set<std::string> possiblyInvolvedJointNames; + // // set all involved joints initially to zero + for (auto robotNodeSetName : robotNodeSetNames) { - if (kinematicChain->getNode(i)->getName().compare("VirtualCentralGaze") == 0) + kinematicChain = localRobot->getRobotNodeSet(robotNodeSetName); + for (unsigned int i = 0; i < kinematicChain->getSize(); i++) { - virtualJoint = boost::dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(kinematicChain->getNode(i)); + possiblyInvolvedJointNames.insert(kinematicChain->getNode(i)->getName()); + // kinematicChain->getNode(i)->setJointValue(0.0f); + // targetJointAngles[kinematicChain->getNode(i)->getName()] = 0.0f; + // controlModes[kinematicChain->getNode(i)->getName()] = ePositionControl; } } - - ARMARX_DEBUG << deactivateSpam(5) << VAROUT(virtualJoint->getName()) << " " << VAROUT(kinematicChain->getName()); - VirtualRobot::GazeIK ikSolver(kinematicChain, virtualJoint); - ikSolver.enableJointLimitAvoidance(true); - ikSolver.setup(10, 30, 20); - // ikSolver.setVerbose(true); - auto globalPos = targetPosition->toGlobal(localRobot); - ARMARX_DEBUG << "Calculating IK for target position " << globalPos->output(); - auto start = IceUtil::Time::now(); - bool solutionFound = ikSolver.solve(globalPos->toEigen()); - auto duration = (IceUtil::Time::now() - start); - - if (duration.toMilliSeconds() > 500) + float error = -1; + //VirtualRobot::RobotPtr robotSnapshot(new RemoteRobot(remoteRobotPrx)); + //localRobot->setConfig(robotSnapshot->getConfig()); + std::string selectedRobotNodeSetName; + for (auto robotNodeSetName : robotNodeSetNames) { - ARMARX_INFO << "Calculating Gaze IK took " << duration.toMilliSeconds() << " ms"; - } + RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponentPrx); + kinematicChain = localRobot->getRobotNodeSet(robotNodeSetName); - if (!solutionFound) - { + 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)); + } + + } + // set other not-used joints to 0 + for (auto& nodeName : possiblyInvolvedJointNames) + { + if (!kinematicChain->hasRobotNode(nodeName)) + { + localRobot->getRobotNode(nodeName)->setJointValue(0.0f); + targetJointAngles[nodeName] = 0.0f; + controlModes[nodeName] = ePositionControl; + } + } + + ARMARX_DEBUG << deactivateSpam(5) << VAROUT(virtualJoint->getName()) << " " << VAROUT(kinematicChain->getName()); + VirtualRobot::GazeIK ikSolver(kinematicChain, virtualJoint); + ikSolver.enableJointLimitAvoidance(true); + ikSolver.setup(10, 30, 20); + // ikSolver.setVerbose(true); + auto globalPos = targetPosition->toGlobal(localRobot); + ARMARX_DEBUG << "Calculating IK for target position " << globalPos->output(); + auto start = IceUtil::Time::now(); + bool solutionFound = ikSolver.solve(globalPos->toEigen()); + auto duration = (IceUtil::Time::now() - start); + + if (duration.toMilliSeconds() > 500) + { + ARMARX_INFO << "Calculating Gaze IK took " << duration.toMilliSeconds() << " ms"; + } Eigen::Vector3f position = globalPos->toEigen() - kinematicChain->getTCP()->getGlobalPose().block(0, 3, 3, 1); - float error = position.norm(); - if (error < 200) + error = position.norm(); + if (!solutionFound) { - ARMARX_INFO << "IKSolver found no solution! applying best solution with " << error << "mm error"; + + if (error < 150) + { + foundSolution = true; + selectedRobotNodeSetName = robotNodeSetName; + ARMARX_INFO << "IKSolver found no solution! applying best solution with " << error << "mm error"; + break; + } } else { - ARMARX_WARNING << "IKSolver found no solution! " << error << "mm error"; - return; + foundSolution = true; + selectedRobotNodeSetName = robotNodeSetName; + break; } } - - ARMARX_DEBUG << "Solution found"; + if (!foundSolution) + { + ARMARX_WARNING << "IKSolver found no solution! " << error << "mm error"; + return; + } + ARMARX_DEBUG << "Solution found with " << selectedRobotNodeSetName << " - remaining error: " << error << " mm"; if (drawer && localRobot->hasRobotNode("Cameras") && getProperty<bool>("VisualizeIKTarget").getValue()) { @@ -304,10 +350,6 @@ namespace armarx 17); } - - NameValueMap targetJointAngles; - NameControlModeMap controlModes; - for (int i = 0; i < (signed int)kinematicChain->getSize(); i++) { if (kinematicChain->getNode(i)->getName().compare("VirtualCentralGaze") != 0) diff --git a/source/RobotAPI/components/units/HeadIKUnit.h b/source/RobotAPI/components/units/HeadIKUnit.h index f76b57c7db2cfe8f1980eda8a124b8b741b6fb90..a1732449bed4bbf05c3bf4884d4534b0b742ec44 100644 --- a/source/RobotAPI/components/units/HeadIKUnit.h +++ b/source/RobotAPI/components/units/HeadIKUnit.h @@ -110,7 +110,7 @@ namespace armarx VirtualRobot::RobotPtr localRobot; DebugDrawerInterfacePrx drawer; - std::string robotNodeSetName; + Ice::StringSeq robotNodeSetNames; FramedPositionPtr targetPosition; bool newTargetSet;