Skip to content
Snippets Groups Projects
Commit 0a9efd7f authored by Markus Grotz's avatar Markus Grotz
Browse files

fixed HeadIKUnit

robot model was not fully initialized via a
`RemoteRobot::createLocalClone()` call and the GazeIK solver was not
able to find a solution.
parent 0b8dcc8f
No related branches found
No related tags found
No related merge requests found
......@@ -15,9 +15,9 @@
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package
* @author
* @date
* @package
* @author
* @date
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
......@@ -70,7 +70,8 @@ namespace armarx
//remoteRobotPrx = robotStateComponentPrx->getSynchronizedRobot();
localRobot = RemoteRobot::createLocalClone(robotStateComponentPrx);
localRobot = RemoteRobot::createLocalCloneFromFile(robotStateComponentPrx, VirtualRobot::RobotIO::RobotDescription::eFull);
// localRobot = RemoteRobot::createLocalClone(robotStateComponentPrx);
headIKUnitListener = getTopic<armarx::HeadIKUnitListenerPrx>(getProperty<std::string>("HeadIKUnitTopicName").getValue());
......@@ -137,7 +138,7 @@ namespace armarx
ScopedLock lock(accessMutex);
this->robotNodeSetNames = armarx::Split(robotNodeSetName, ",");
for (auto& setName : robotNodeSetNames)
for (auto & setName : robotNodeSetNames)
{
boost::trim(setName);
}
......@@ -298,6 +299,7 @@ namespace armarx
for (auto robotNodeSetName : robotNodeSetNames)
{
RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponentPrx);
kinematicChain = localRobot->getRobotNodeSet(robotNodeSetName);
VirtualRobot::RobotNodePrismaticPtr virtualJoint;
......@@ -311,7 +313,7 @@ namespace armarx
}
// set other not-used joints to 0
for (auto& nodeName : possiblyInvolvedJointNames)
for (auto & nodeName : possiblyInvolvedJointNames)
{
if (!kinematicChain->hasRobotNode(nodeName))
{
......@@ -325,7 +327,7 @@ namespace armarx
VirtualRobot::GazeIK ikSolver(kinematicChain, virtualJoint);
ikSolver.enableJointLimitAvoidance(true);
ikSolver.setup(10, 30, 20);
// ikSolver.setVerbose(true);
//ikSolver.setVerbose(true);
globalPos = targetPosition->toGlobal(localRobot);
ARMARX_DEBUG << "Calculating IK for target position " << globalPos->output();
......@@ -352,8 +354,6 @@ namespace armarx
}
else
{
foundSolution = true;
selectedRobotNodeSetName = robotNodeSetName;
break;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment