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