예제 #1
0
    virtual void demothread(int argc, char ** argv) {
        string scenefilename = "data/pa10grasp2.env.xml";
        penv->Load(scenefilename);

        vector<RobotBasePtr> vrobots;
        penv->GetRobots(vrobots);
        RobotBasePtr probot = vrobots.at(0);

        // find a manipulator chain to move
        for(size_t i = 0; i < probot->GetManipulators().size(); ++i) {
            if( probot->GetManipulators()[i]->GetName().find("arm") != string::npos ) {
                probot->SetActiveManipulator(probot->GetManipulators()[i]);
                break;
            }
        }
        RobotBase::ManipulatorPtr pmanip = probot->GetActiveManipulator();

        // load inverse kinematics using ikfast
        ModuleBasePtr pikfast = RaveCreateModule(penv,"ikfast");
        penv->Add(pikfast,true,"");
        stringstream ssin,ssout;
        vector<dReal> vsolution;
        ssin << "LoadIKFastSolver " << probot->GetName() << " " << (int)IKP_Transform6D;
        if( !pikfast->SendCommand(ssout,ssin) ) {
            RAVELOG_ERROR("failed to load iksolver\n");
        }
        if( !pmanip->GetIkSolver()) {
            throw OPENRAVE_EXCEPTION_FORMAT0("need ik solver",ORE_Assert);
        }

        ModuleBasePtr pbasemanip = RaveCreateModule(penv,"basemanipulation"); // create the module
        penv->Add(pbasemanip,true,probot->GetName()); // load the module

        while(IsOk()) {
            {
                EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment

                // find a new manipulator position and feed that into the planner. If valid, robot will move to it safely.
                Transform t = pmanip->GetEndEffectorTransform();
                t.trans += Vector(RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f);
                t.rot = quatMultiply(t.rot,quatFromAxisAngle(Vector(RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f)*0.2f));
                ssin.str("");
                ssin.clear();
                ssin << "MoveToHandPosition pose " << t;
                // start the planner and run the robot
                RAVELOG_INFO("%s\n",ssin.str().c_str());
                if( !pbasemanip->SendCommand(ssout,ssin) ) {
                    continue;
                }
            }

            // unlock the environment and wait for the robot to finish
            while(!probot->GetController()->IsDone() && IsOk()) {
                boost::this_thread::sleep(boost::posix_time::milliseconds(1));
            }
        }
    }
예제 #2
0
InterfaceBasePtr RegisterSimulationFunction(int environmentid, boost::python::object simulationfn)
{
    ModuleBasePtr module = RaveCreateModule(RaveGetEnvironment(environmentid), "PythonBinding");
    if( !!module ) {
        boost::shared_ptr<FunctionUserData> p = boost::dynamic_pointer_cast<FunctionUserData>(module->GetUserData());
        p->simulationfn = simulationfn;
        module->GetEnv()->AddModule(module,"");
    }
    return InterfaceBasePtr(module);
}
예제 #3
0
int main(int argc, char ** argv)
{
    if( argc < 3 ) {
        RAVELOG_INFO("ikloader robot iktype\n");
        return 1;
    }

    string robotname = argv[1];
    string iktype = argv[2];
    RaveInitialize(true); // start openrave core

    EnvironmentBasePtr penv = RaveCreateEnvironment(); // create the main environment

    {
        // lock the environment to prevent changes
        EnvironmentMutex::scoped_lock lock(penv->GetMutex());
        // load the scene
        RobotBasePtr probot = penv->ReadRobotXMLFile(robotname);
        if( !probot ) {
            penv->Destroy();
            return 2;
        }
        penv->Add(probot);

        ModuleBasePtr pikfast = RaveCreateModule(penv,"ikfast");
        penv->Add(pikfast,true,"");
        stringstream ssin,ssout;
        ssin << "LoadIKFastSolver " << probot->GetName() << " " << iktype;
        // if necessary, add free inc for degrees of freedom
        //ssin << " " << 0.04f;
        // get the active manipulator
        RobotBase::ManipulatorPtr pmanip = probot->GetActiveManipulator();
        if( !pikfast->SendCommand(ssout,ssin) ) {
            RAVELOG_ERROR("failed to load iksolver\n");
            penv->Destroy();
            return 1;
        }

        RAVELOG_INFO("testing random ik\n");
        while(1) {
            Transform trans;
            trans.rot = quatFromAxisAngle(Vector(RaveRandomFloat()-0.5,RaveRandomFloat()-0.5,RaveRandomFloat()-0.5));
            trans.trans = Vector(RaveRandomFloat()-0.5,RaveRandomFloat()-0.5,RaveRandomFloat()-0.5)*2;

            vector<dReal> vsolution;
            if( pmanip->FindIKSolution(IkParameterization(trans),vsolution,IKFO_CheckEnvCollisions) ) {
                stringstream ss; ss << "solution is: ";
                for(size_t i = 0; i < vsolution.size(); ++i) {
                    ss << vsolution[i] << " ";
                }
                ss << endl;
                RAVELOG_INFO(ss.str());
            }
            else {
                // could fail due to collisions, etc
            }
        }
    }

    RaveDestroy(); // destroy
    return 0;
}
예제 #4
0
    virtual void demothread(int argc, char ** argv) {

        string scenefilename = "/home/yo/asibot/main/app/ravebot/models/asibot_kitchen_wheelchair.env.xml";
        penv->Load(scenefilename);

        vector<RobotBasePtr> vrobots;
        penv->GetRobots(vrobots);

        KinBodyPtr objPtr = penv->GetKinBody("asibot");
        if(objPtr) {
            if(vrobots.at(1)->Grab(objPtr)) printf("[success] object asibot exists and grabbed.\n");
            else printf("[warning] object asibot exists but not grabbed.\n");
        } else printf("[fail] object asibot does not exist.\n");

        printf("begin wait\n");
        sleep(5);
        printf("end wait\n");
        RobotBasePtr probot = vrobots.at(1);

        vector<dReal> v(2);

        /* vector<int> vindices(probot->GetDOF());
        for(size_t i = 0; i < vindices.size(); ++i) {
            vindices[i] = i;
        }
        probot->SetActiveDOFs(vindices);*/

        vector<int> vindices;  // send it empty instead
        //probot->SetActiveDOFs(vindices,DOF_X|DOF_Y|DOF_RotationAxis,Vector(0,0,1));
        probot->SetActiveDOFs(vindices,DOF_X|DOF_Y,Vector(0,0,1));

        ModuleBasePtr pbasemanip = RaveCreateModule(penv,"basemanipulation"); // create the module
        penv->Add(pbasemanip,true,probot->GetName()); // load the module

        int iteracion = 0;

        while(IsOk()) {
            {
                EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment

                // find a set of free joint values for the robot
                {
                    RobotBase::RobotStateSaver saver(probot); // save the state
                    while(1) {
                        Transform T = probot->GetTransform();
                        if(iteracion%2) {
                            v[0] = T.trans.x;
                            v[1] = T.trans.y+1;
                            //v[2] = T.trans.z;
                            iteracion++;
                        } else {
                            v[0] = T.trans.x;
                            v[1] = T.trans.y-0.5;
                            //v[2] = T.trans.z;
                            iteracion++;
                        }
                        if(iteracion==5) vrobots.at(1)->Release(objPtr);
                        probot->SetActiveDOFValues(v);
                        if( !penv->CheckCollision(probot) && !probot->CheckSelfCollision() ) {
                            break;
                        }
                    }
                    // robot state is restored
                }

                stringstream cmdin,cmdout;
                cmdin << "MoveActiveJoints maxiter 2000 maxtries 1 goal ";
                for(size_t i = 0; i < v.size(); ++i) {
                    cmdin << v[i] << " ";
                }

                // start the planner and run the robot
                RAVELOG_INFO("%s\n",cmdin.str().c_str());
                if( !pbasemanip->SendCommand(cmdout,cmdin) ) {
                    continue;
                }
            }

            // unlock the environment and wait for the robot to finish
            while(!probot->GetController()->IsDone() && IsOk()) {
                boost::this_thread::sleep(boost::posix_time::milliseconds(1));
            }
        }
    }
 bool SimulationStep(dReal fElapsedTime) {
     return _pmodule->SimulationStep(fElapsedTime);
 }