bool RobotNode::initialize(SceneObjectPtr parent, const std::vector<SceneObjectPtr> &children)
{
RobotPtr rob = robot.lock();
THROW_VR_EXCEPTION_IF(!rob, "Could not init RobotNode without robot" );
// robot
if (!rob->hasRobotNode(static_pointer_cast<RobotNode>(shared_from_this())))
rob->registerRobotNode(static_pointer_cast<RobotNode>(shared_from_this()));
// update visualization of coordinate systems
if (visualizationModel && visualizationModel->hasAttachedVisualization("CoordinateSystem"))
{
VisualizationNodePtr v = visualizationModel->getAttachedVisualization("CoordinateSystem");
// not needed any more!
// this is a little hack: The globalPose is used to set the "local" position of the attached Visualization:
// Since the attached visualizations are already positioned at the global pose of the visualizationModel,
// we just need the local postJointTransform
//v->setGlobalPose(postJointTransformation);
}
checkValidRobotNodeType();
for (size_t i=0;i<sensors.size();i++)
{
sensors[i]->initialize(shared_from_this());
}
return SceneObject::initialize(parent,children);
}
RobotNodePtr RobotNode::clone( RobotPtr newRobot, bool cloneChildren, RobotNodePtr initializeWithParent, CollisionCheckerPtr colChecker, float scaling )
{
ReadLockPtr lock = getRobot()->getReadLock();
if (!newRobot)
{
VR_ERROR << "Attempting to clone RobotNode for invalid robot";
return RobotNodePtr();
}
std::vector< std::string > clonedChildrenNames;
VisualizationNodePtr clonedVisualizationNode;
if (visualizationModel)
clonedVisualizationNode = visualizationModel->clone(true,scaling);
CollisionModelPtr clonedCollisionModel;
if (collisionModel)
clonedCollisionModel = collisionModel->clone(colChecker,scaling);
RobotNodePtr result = _clone(newRobot, clonedVisualizationNode, clonedCollisionModel, colChecker, scaling);
if (!result)
{
VR_ERROR << "Cloning failed.." << endl;
return result;
}
if (cloneChildren)
{
std::vector< SceneObjectPtr > children = this->getChildren();
for (size_t i=0;i<children.size();i++)
{
RobotNodePtr n = dynamic_pointer_cast<RobotNode>(children[i]);
if (n)
{
RobotNodePtr c = n->clone(newRobot,true,RobotNodePtr(),colChecker,scaling);
if (c)
result->attachChild(c);
} else
{
SensorPtr s = dynamic_pointer_cast<Sensor>(children[i]);
if (s)
{
// performs registering and initialization
SensorPtr c = s->clone(result,scaling);
} else
{
SceneObjectPtr so = children[i]->clone(children[i]->getName(),colChecker,scaling);
if (so)
result->attachChild(so);
}
}
}
}
result->setMaxVelocity(maxVelocity);
result->setMaxAcceleration(maxAcceleration);
result->setMaxTorque(maxTorque);
std::map< std::string, float>::iterator it = propagatedJointValues.begin();
while (it != propagatedJointValues.end())
{
result->propagateJointValue(it->first,it->second);
it++;
}
newRobot->registerRobotNode(result);
if (initializeWithParent)
result->initialize(initializeWithParent);
return result;
}
