示例#1
0
void SimDynamicsWindow::addObject()
{
    ManipulationObjectPtr vitalis;
    std::string vitalisPath = "objects/VitalisWithPrimitives.xml";

    if (VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(vitalisPath))
    {
        vitalis = ObjectIO::loadManipulationObject(vitalisPath);
    }

    if (vitalis)
    {
        vitalis->setMass(0.5f);
        float x, y, z;
        int counter = 0;

        do
        {
            x = float(rand() % 2000 - 1000);
            y = float(rand() % 2000 - 1000);
            z = float(rand() % 2000 + 100);
            Eigen::Matrix4f gp = vitalis->getGlobalPose();
            gp.block(0, 3, 3, 1) = Eigen::Vector3f(x, y, z);
            vitalis->setGlobalPose(gp);
            counter++;

            if (counter > 100)
            {
                cout << "Error, could not find valid pose" << endl;
                return;
            }
        }
        while (robot && CollisionChecker::getGlobalCollisionChecker()->checkCollision(robot, vitalis));

        SimDynamics::DynamicsObjectPtr dynamicsObjectVitalis = dynamicsWorld->CreateDynamicsObject(vitalis);
        dynamicsObjectVitalis->setPosition(Eigen::Vector3f(x, y, z));
        dynamicsObjects.push_back(dynamicsObjectVitalis);
        dynamicsWorld->addObject(dynamicsObjectVitalis);
        buildVisualization();
    }

}
示例#2
0
    bool ObjectIO::saveManipulationObject(ManipulationObjectPtr object, const std::string& xmlFile)
    {
        THROW_VR_EXCEPTION_IF(!object, "NULL object");

        boost::filesystem::path filenameBaseComplete(xmlFile);
        boost::filesystem::path filenameBasePath = filenameBaseComplete.branch_path();
        std::string basePath = filenameBasePath.string();

        std::string xmlString = object->toXML(basePath);

        // save file
        return BaseIO::writeXMLFile(xmlFile, xmlString, true);
    }
示例#3
0
bool WorkspaceGrid::fillGridData( WorkspaceRepresentationPtr ws, ManipulationObjectPtr o, GraspPtr g, RobotNodePtr baseRobotNode )
{
	if (!ws || !o || !g)
		return false;

	Eigen::Matrix4f graspGlobal = g->getTcpPoseGlobal(o->getGlobalPose());

	WorkspaceRepresentation::WorkspaceCut2DPtr cutXY = ws->createCut(graspGlobal,discretizeSize);

	std::vector<WorkspaceRepresentation::WorkspaceCut2DTransformationPtr> transformations = ws->createCutTransformations(cutXY,baseRobotNode);
	setEntries(transformations,graspGlobal,g);
	return true;
}
示例#4
0
SimDynamicsWindow::SimDynamicsWindow(std::string& sRobotFilename, Qt::WFlags flags)
    : QMainWindow(NULL)
{
    VR_INFO << " start " << endl;
    //this->setCaption(QString("ShowRobot - KIT - Humanoids Group"));
    //resize(1100, 768);

    useColModel = false;
    VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(sRobotFilename);
    std::string robotFilename = sRobotFilename;
    sceneSep = new SoSeparator;
    sceneSep->ref();

    comSep = new SoSeparator;
    sceneSep->addChild(comSep);

    contactsSep = new SoSeparator;
    sceneSep->addChild(contactsSep);

    forceSep = new SoSeparator;
    sceneSep->addChild(forceSep);

    // optional visualizations (not considered by dynamics)
    SoSeparator* cc = CoinVisualizationFactory::CreateCoordSystemVisualization(10.0f);
    sceneSep->addChild(cc);
    BulletEngineConfigPtr config(new BulletEngineConfig());
    config->bulletSolverIterations = 2000;
    config->bulletObjectRestitution = btScalar(0.1);
    config->bulletObjectFriction = btScalar(1.0);
    config->bulletSolverGlobalContactForceMixing = btScalar(0.00001);
    config->bulletObjectDampingLinear = btScalar(0.3);
    config->bulletObjectDampingAngular = btScalar(0.3);
    dynamicsWorld = SimDynamics::DynamicsWorld::Init(config);
    SIMDYNAMICS_ASSERT(dynamicsWorld);

    dynamicsWorld->createFloorPlane();

    VirtualRobot::ObstaclePtr o = VirtualRobot::Obstacle::createBox(1000.0f, 1000.0f, 1000.0f, VirtualRobot::VisualizationFactory::Color::Blue());
    o->setMass(1.0f); // 1kg

    dynamicsObject = dynamicsWorld->CreateDynamicsObject(o);
    dynamicsObject->setPosition(Eigen::Vector3f(1000, 2000, 1000.0f));
    dynamicsWorld->addObject(dynamicsObject);

    addObject();

#if 0
    std::string f = "/home/niko/coding/armarx/SimulationX/data/environment/KIT_Robot_Kitchen.xml";
    ManipulationObjectPtr mo = ObjectIO::loadManipulationObject(f);
    mo->setSimulationType(VirtualRobot::SceneObject::Physics::eStatic);
    dynamicsObject2 = dynamicsWorld->CreateDynamicsObject(mo);
    //dynObj->setPosition(Eigen::Vector3f(3000,3000,1000.0f));
    dynamicsWorld->addObject(dynamicsObject2);
#endif

#if 0
    std::string f = "/home/SMBAD/vahrenka/.armarx/mongo/.cache/files/lowersink.xml";
    ManipulationObjectPtr mo = ObjectIO::loadManipulationObject(f);
    mo->setSimulationType(SceneObject::Physics::eDynamic);
    SimDynamics::DynamicsObjectPtr dynObj2 = dynamicsWorld->CreateDynamicsObject(mo);
    //dynObj2->setSimType(VirtualRobot::SceneObject::Physics::eDynamic);
    dynObj2->setPosition(Eigen::Vector3f(0, 0, -200.0f));
    dynamicsWorld->addObject(dynObj2);
    dynamicsObjects.push_back(dynObj2);
#endif

    setupUI();
    loadRobot(robotFilename);

    // build visualization
    buildVisualization();

    viewer->viewAll();

    // register callback
    float TIMER_MS = 30.0f;
    SoSensorManager* sensor_mgr = SoDB::getSensorManager();
    timerSensor = new SoTimerSensor(timerCB, this);
    timerSensor->setInterval(SbTime(TIMER_MS / 1000.0f));
    sensor_mgr->insertTimerSensor(timerSensor);
}
示例#5
0
    ManipulationObjectPtr ObjectIO::processManipulationObject(rapidxml::xml_node<char>* objectXMLNode, const std::string& basePath)
    {
        THROW_VR_EXCEPTION_IF(!objectXMLNode, "No <ManipulationObject> tag in XML definition");

        bool visuProcessed = false;
        bool colProcessed = false;
        VisualizationNodePtr visualizationNode;
        CollisionModelPtr collisionModel;
        bool useAsColModel = false;
        SceneObject::Physics physics;
        bool physicsDefined = false;
        std::vector<GraspSetPtr> graspSets;
        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);
            ManipulationObjectPtr result = loadManipulationObject(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(), "ManipulationObject 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 ManipulationObject '" << objName << "'." << endl);
                visualizationNode = processVisualizationTag(node, objName, basePath, useAsColModel);
                visuProcessed = true;

                if (useAsColModel)
                {
                    THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in ManipulationObject '" << 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 ManipulationObject '" << objName << "'." << endl);
                collisionModel = processCollisionTag(node, objName, basePath);
                colProcessed = true;
            }
            else if (nodeName == "physics")
            {
                THROW_VR_EXCEPTION_IF(physicsDefined, "Two physics tags defined in ManipulationObject '" << objName << "'." << endl);
                processPhysicsTag(node, objName, physics);
                physicsDefined = true;
            }
            else if (nodeName == "graspset")
            {
                GraspSetPtr gs = processGraspSet(node, objName);
                THROW_VR_EXCEPTION_IF(!gs, "Invalid grasp set in '" << objName << "'." << endl);
                graspSets.push_back(gs);

            }
            else if (nodeName == "globalpose")
            {
                processTransformNode(node, objName, globalPose);
            }
            else
            {
                THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in ManipulationObject <" << objName << ">." << endl);
            }

            node = node->next_sibling();
        }



        // build object
        ManipulationObjectPtr object(new ManipulationObject(objName, visualizationNode, collisionModel, physics));

        for (size_t i = 0; i < graspSets.size(); i++)
        {
            object->addGraspSet(graspSets[i]);
        }

        object->setGlobalPose(globalPose);

        return object;
    }
示例#6
0
void showSceneWindow::loadScene()
{
    sceneVisuSep->removeAllChildren();
    currentEEF.reset();
    currentGrasp.reset();
    currentGraspSet.reset();
    currentObject.reset();
    currentRobot.reset();
    currentTrajectory.reset();
    cout << "Loading Scene from " << sceneFile << endl;

    scene.reset();

    try
    {
        scene = SceneIO::loadScene(sceneFile);
    }
    catch (VirtualRobotException& e)
    {
        cout << "Could not find valid scene in file " << sceneFile << endl;
    }

    if (!scene)
    {
        // try manip object
        try
        {

            ManipulationObjectPtr mo = ObjectIO::loadManipulationObject(sceneFile);
            //mo = mo->clone(mo->getName());

            if (mo)
            {
                VR_INFO << "Loaded Manipulation object:" << endl;
                mo->print();
                scene.reset(new Scene(mo->getName()));
                scene->registerManipulationObject(mo);
            }
        }
        catch (VirtualRobotException& e)
        {
            cout << "Could not find valid manipulation object in file " << sceneFile << endl;
        }
    }

    if (!scene)
    {
        // try object
        try
        {

            ObstaclePtr mo = ObjectIO::loadObstacle(sceneFile);

            if (mo)
            {
                VR_INFO << "Loaded obstacle:" << endl;
                mo->print();
                scene.reset(new Scene(mo->getName()));
                scene->registerObstacle(mo);
            }
        }
        catch (VirtualRobotException& e)
        {
            cout << "Could not find valid obstacle in file " << sceneFile << endl;
        }
    }

    if (!scene)
    {
        cout << " ERROR while creating scene" << endl;
        return;
    }


    /*std::vector<VirtualRobot::ManipulationObjectPtr> mo;
    mo = scene->getManipulationObjects();
    cout << "Printing " << mo.size() << " objects" << endl;
    for (size_t i=0;i<mo.size();i++)
    {
        mo[i]->print();
        mo[i]->showCoordinateSystem(true);
        Eigen::Vector3f c = mo[i]->getCoMGlobal();
        cout << "com global: \n" << c << endl;
        c = mo[i]->getCoMLocal();
        cout << "com local: \n" << c << endl;
        //mo[i]->showBoundingBox(true);
    }*/
    /*std::vector<VirtualRobot::ObstaclePtr> o;
    o = scene->getObstacles();
    cout << "Printing " << o.size() << " obstacles" << endl;
    for (size_t i=0;i<o.size();i++)
    {
        o[i]->print();
        o[i]->showCoordinateSystem(true);
        Eigen::Vector3f c = o[i]->getCoMGlobal();
        cout << "com global: \n" << c << endl;
        c = o[i]->getCoMLocal();
        cout << "com local: \n" << c << endl;
        //mo[i]->showBoundingBox(true);
    }*/

    // get nodes
    /*m_pRobot->getRobotNodes(allRobotNodes);
    m_pRobot->getRobotNodeSets(robotNodeSets);
    m_pRobot->getEndEffectors(eefs);
    updateEEFBox();
    updateRNSBox();
    selectRNS(0);
    if (allRobotNodes.size()==0)
        selectJoint(-1);
    else
        selectJoint(0);



    displayTriangles();

    // build visualization
    collisionModel();
    robotStructure();*/
    updateGui();
    buildVisu();
    viewer->viewAll();
}