From 51691fb1dc35b794570a453cdbf207dc1f799a35 Mon Sep 17 00:00:00 2001
From: David Sippel <sippel@i61pc014.itec.uni-karlsruhe.de>
Date: Tue, 5 Jan 2016 10:53:55 +0100
Subject: [PATCH] Added target position and target joint values to callback.
---
source/RobotAPI/components/units/HeadIKUnit.cpp | 17 +++++++++++++----
source/RobotAPI/interface/units/HeadIKUnit.ice | 2 +-
2 files changed, 14 insertions(+), 5 deletions(-)
diff --git a/source/RobotAPI/components/units/HeadIKUnit.cpp b/source/RobotAPI/components/units/HeadIKUnit.cpp
index 953fc2bd2..67fbafdb0 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 ebfb4224f..24c44037e 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);
};
};
--
GitLab