Esempio n. 1
0
RobotNodeSet::RobotNodeSet(const std::string &name, 
						   RobotWeakPtr robot, 
						   const std::vector< RobotNodePtr > &robotNodes, 
						   const RobotNodePtr kinematicRoot /*= RobotNodePtr()*/,
						   const RobotNodePtr tcp /*= RobotNodePtr()*/)
						   :SceneObjectSet(name,robotNodes.size()>0?robotNodes[0]->getCollisionChecker():CollisionCheckerPtr())
{
	this->robotNodes = robotNodes;
	this->kinematicRoot = kinematicRoot;
	this->tcp = tcp;
	RobotPtr rob = robot.lock();
	if (!kinematicRoot && robotNodes.size()>0)
		this->kinematicRoot = rob->getRootNode();
	if (!tcp && robotNodes.size()>0)
		this->tcp = robotNodes[robotNodes.size()-1];
	this->robot = robot;

	// now, the objects are stored in the parent's (SceneObjectSet) data structure, so that the methods of SceneObjectSet do work
	for(size_t i = 0; i < robotNodes.size(); i++)
	{
		SceneObjectPtr cm = boost::dynamic_pointer_cast<SceneObject>(robotNodes[i]);
		if (cm)
		{
			if (colChecker != cm->getCollisionChecker())
			{
				VR_WARNING << "col model of " << robotNodes[i]->getName() << " belongs to different instance of collision checker, in: " << getName().c_str() << endl;
			} else
			{
				sceneObjects.push_back(cm);
			}
		}
	}
}
/**
 * This method creates a VirtualRobot::RobotNodeRevolute from DH parameters.
 *
 * \return instance of VirtualRobot::RobotNodeRevolute.
 */
RobotNodePtr RobotNodeRevoluteFactory::createRobotNodeDH(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const DHParameter& dhParameters, const SceneObject::Physics &p, RobotNode::RobotNodeType rntype) const
{
	RobotNodePtr robotNode(new RobotNodeRevolute(robot, nodeName, limitLow, limitHigh, dhParameters.aMM(), dhParameters.dMM(), dhParameters.alphaRadian(), dhParameters.thetaRadian(), visualizationModel, collisionModel, jointValueOffset, p,  CollisionCheckerPtr(), rntype));

	return robotNode;
}
Esempio n. 3
0
    ObstaclePtr ObjectIO::processObstacle(rapidxml::xml_node<char>* objectXMLNode, const std::string& basePath)
    {
        THROW_VR_EXCEPTION_IF(!objectXMLNode, "No <Obstacle> tag in XML definition");

        bool visuProcessed = false;
        bool colProcessed = false;
        VisualizationNodePtr visualizationNode;
        CollisionModelPtr collisionModel;
        bool useAsColModel = false;
        SceneObject::Physics physics;
        bool physicsDefined = false;
        Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity();

        // get name
        std::string objName = processNameAttribute(objectXMLNode);

        // first check if there is an xml file to load
        rapidxml::xml_node<>* xmlFileNode = objectXMLNode->first_node("file", 0, false);

        if (xmlFileNode)
        {
            std::string xmlFile = processFileNode(xmlFileNode, basePath);
            ObstaclePtr result = loadObstacle(xmlFile);

            if (!result)
            {
                return result;
            }

            if (!objName.empty())
            {
                result->setName(objName);
            }

            // update global pose
            rapidxml::xml_node<>* poseNode = objectXMLNode->first_node("globalpose", 0, false);

            if (poseNode)
            {
                processTransformNode(poseNode, objName, globalPose);
                result->setGlobalPose(globalPose);
            }

            return result;
        }

        THROW_VR_EXCEPTION_IF(objName.empty(), "Obstacle definition expects attribute 'name'");


        rapidxml::xml_node<>* node = objectXMLNode->first_node();

        while (node)
        {
            std::string nodeName = getLowerCase(node->name());

            if (nodeName == "visualization")
            {
                THROW_VR_EXCEPTION_IF(visuProcessed, "Two visualization tags defined in Obstacle '" << objName << "'." << endl);
                visualizationNode = processVisualizationTag(node, objName, basePath, useAsColModel);
                visuProcessed = true;

                if (useAsColModel)
                {
                    THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in Obstacle '" << objName << "'." << endl);
                    std::string colModelName = objName;
                    colModelName += "_VISU_ColModel";
                    // todo: ID?
                    collisionModel.reset(new CollisionModel(visualizationNode, colModelName, CollisionCheckerPtr()));
                    colProcessed = true;
                }
            }
            else if (nodeName == "collisionmodel")
            {
                THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in Obstacle '" << objName << "'." << endl);
                collisionModel = processCollisionTag(node, objName, basePath);
                colProcessed = true;
            }
            else if (nodeName == "physics")
            {
                THROW_VR_EXCEPTION_IF(physicsDefined, "Two physics tags defined in Obstacle '" << objName << "'." << endl);
                processPhysicsTag(node, objName, physics);
                physicsDefined = true;
            }
            else if (nodeName == "globalpose")
            {
                processTransformNode(node, objName, globalPose);
            }
            else
            {
                THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in Obstacle <" << objName << ">." << endl);
            }

            node = node->next_sibling();
        }

        // build object
        ObstaclePtr object(new Obstacle(objName, visualizationNode, collisionModel, physics));
        object->setGlobalPose(globalPose);
        return object;
    }
/**
 * This method creates a VirtualRobot::RobotNodeRevolute.
 *
 * \return instance of VirtualRobot::RobotNodeRevolute.
 */
RobotNodePtr RobotNodeRevoluteFactory::createRobotNode(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const Eigen::Matrix4f& preJointTransform, const Eigen::Vector3f& axis, const Eigen::Vector3f& translationDirection, const SceneObject::Physics &p, RobotNode::RobotNodeType rntype) const
{
	RobotNodePtr robotNode(new RobotNodeRevolute(robot, nodeName, limitLow, limitHigh, preJointTransform, axis, visualizationModel, collisionModel, jointValueOffset, p,  CollisionCheckerPtr(), rntype));

	return robotNode;
}