bool SimoxRobotViewer::setObjectPose( const std::string &objectName, const Eigen::Matrix4f &pose, const std::string coordSystem )
{
if (!sceneSep)
return false;
lock();
Eigen::Matrix4f p = pose;
if (objects.find(objectName) == objects.end())
{
VR_INFO << "Do not know object " << objectName << endl;
unlock();
return false;
}
if (!coordSystem.empty() && robot && robot->hasRobotNode(coordSystem))
{
RobotNodePtr rn = robot->getRobotNode(coordSystem);
if (rn)
p = rn->toGlobalCoordinateSystem(p);
}
objects[objectName].object->setGlobalPose(p);
SoNode *n = objects[objectName].visuGrasps->getChild(0);
SoMatrixTransform *mat = dynamic_cast<SoMatrixTransform*>(n);
if (mat)
mat->matrix.setValue(CoinVisualizationFactory::getSbMatrix(p));
n = objects[objectName].visuReachableGrasps->getChild(0);
mat = dynamic_cast<SoMatrixTransform*>(n);
if (mat)
mat->matrix.setValue(CoinVisualizationFactory::getSbMatrix(p));
// delete reachable grasps visu, it may not be valid any more
if (objects[objectName].visuReachableGrasps->getNumChildren()==2)
objects[objectName].visuReachableGrasps->removeChild(1);
if (sceneSep->findChild(objects[objectName].visuReachableGrasps)>=0)
sceneSep->removeChild(objects[objectName].visuReachableGrasps);
unlock();
redraw();
return true;
}
