diff --git a/source/RobotAPI/components/units/HeadIKUnit.cpp b/source/RobotAPI/components/units/HeadIKUnit.cpp index 953fc2bd2e690658c6e8ce11dd7cb7e79f684200..67fbafdb0324c25daa4cc8c49883f329e7c0cf71 100644 --- a/source/RobotAPI/components/units/HeadIKUnit.cpp +++ b/source/RobotAPI/components/units/HeadIKUnit.cpp @@ -52,7 +52,6 @@ namespace armarx headIKUnitListener = getTopic<armarx::HeadIKUnitListenerPrx>(getProperty<std::string>("HeadIKUnitTopicName").getValue()); - //std::string robotModelFile; //ArmarXDataPath::getAbsolutePath("Armar3/robotmodel/ArmarIII.xml", robotModelFile); //localRobot = VirtualRobot::RobotIO::loadRobot(robotModelFile.c_str(), VirtualRobot::RobotIO::eStructure); @@ -123,6 +122,7 @@ namespace armarx this->targetPosition->y = targetPosition->y; this->targetPosition->z = targetPosition->z; this->targetPosition->frame = targetPosition->frame; + FramedPositionPtr globalTarget = FramedPositionPtr::dynamicCast(targetPosition)->toGlobal(robotStateComponentPrx->getSynchronizedRobot()); if (drawer && getProperty<bool>("VisualizeIKTarget").getValue()) @@ -138,8 +138,6 @@ namespace armarx ARMARX_DEBUG << "new Head target set: " << *globalTarget; newTargetSet = true; - - headIKUnitListener->reportHeadTargetChanged(); } @@ -269,6 +267,7 @@ namespace armarx // controlModes[kinematicChain->getNode(i)->getName()] = ePositionControl; } } + FramedPositionPtr globalPos; float error = -1; //VirtualRobot::RobotPtr robotSnapshot(new RemoteRobot(remoteRobotPrx)); //localRobot->setConfig(robotSnapshot->getConfig()); @@ -304,7 +303,8 @@ namespace armarx ikSolver.enableJointLimitAvoidance(true); ikSolver.setup(10, 30, 20); // ikSolver.setVerbose(true); - auto globalPos = targetPosition->toGlobal(localRobot); + + globalPos = targetPosition->toGlobal(localRobot); ARMARX_DEBUG << "Calculating IK for target position " << globalPos->output(); auto start = IceUtil::Time::now(); bool solutionFound = ikSolver.solve(globalPos->toEigen()); @@ -329,6 +329,8 @@ namespace armarx } else { + + foundSolution = true; selectedRobotNodeSetName = robotNodeSetName; break; @@ -361,6 +363,13 @@ namespace armarx ARMARX_DEBUG << kinematicChain->getNode(i)->getName() << ": " << kinematicChain->getNode(i)->getJointValue(); } + + + if (headIKUnitListener) + { + headIKUnitListener->reportHeadTargetChanged(targetJointAngles, globalPos); + } + try { kinematicUnitPrx->switchControlMode(controlModes); diff --git a/source/RobotAPI/interface/units/HeadIKUnit.ice b/source/RobotAPI/interface/units/HeadIKUnit.ice index ebfb4224f0aaa3e6485a8ea13a15edd17a554eb2..24c44037eff8914cf413c3536602559380e9f3f4 100644 --- a/source/RobotAPI/interface/units/HeadIKUnit.ice +++ b/source/RobotAPI/interface/units/HeadIKUnit.ice @@ -59,7 +59,7 @@ module armarx interface HeadIKUnitListener { - void reportHeadTargetChanged(); + void reportHeadTargetChanged(NameValueMap targetJointAngles, FramedPositionBase targetPosition); }; };