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;
}
