C++ EnvironmentBasePtr::Destroy方法代码示例

bool GetIKSolutions(EnvironmentBasePtr _penv, Transform Pose, std::vector< std::vector< dReal > > &vsolution){

	ModuleBasePtr _pikfast = RaveCreateModule(_penv,"ikfast");
	RobotBasePtr _probot = _penv->GetRobot(robotname);
	_probot->SetActiveManipulator("1"); 
	RobotBase::ManipulatorPtr _pmanip = _probot->GetActiveManipulator(); 
	_penv->Add(_pikfast,true,"");
	std::vector< std::vector< dReal > > solns;

	solns.resize(6);
	stringstream ssin,ssout;
	string iktype = "Transform6D";
	ssin << "LoadIKFastSolver " << _probot->GetName() << " " << iktype;
	if( !_pikfast->SendCommand(ssout,ssin) ) {
		RAVELOG_ERROR("failed to load iksolver\n");
		_penv->Destroy();
		return false;
	}

	if(_pmanip->FindIKSolutions(IkParameterization(Pose),vsolution,IKFO_CheckEnvCollisions) ) {
		stringstream ss; ss << "solution is: ";
			for(size_t i = 0; i < vsolution.size(); ++i) {
				for(size_t j = 0; j < vsolution[i].size(); ++j){
					ss << vsolution[i][j] << " " ;
				} ss << endl;
			}
		ss << endl;
		////RAVELOG_INFO(ss.str());
	 }
	else {
		// could fail due to collisions, etc
		return false;
	}
	return true ;
}

int main(int argc, char ** argv)
{
    //int num = 1;
    string scenefilename = "data/lab1.env.xml";
    string viewername = RaveGetDefaultViewerType(); //qtcoin

    // parse the command line options
    int i = 1;
    while(i < argc) {
        if((strcmp(argv[i], "-h") == 0)||(strcmp(argv[i], "-?") == 0)||(strcmp(argv[i], "/?") == 0)||(strcmp(argv[i], "--help") == 0)||(strcmp(argv[i], "-help") == 0)) {
            RAVELOG_INFO("orloadviewer [--num n] [--scene filename] viewername\n");
            return 0;
        }
        else if( strcmp(argv[i], "--scene") == 0 ) {
            scenefilename = argv[i+1];
            i += 2;
        }
        else
            break;
    }
    if( i < argc ) {
        viewername = argv[i++];
    }

    RaveInitialize(true); // start openrave core
    EnvironmentBasePtr penv = RaveCreateEnvironment(); // create the main environment
    RaveSetDebugLevel(Level_Debug);

    boost::thread thviewer(boost::bind(SetViewer,penv,viewername));
    penv->Load(scenefilename); // load the scene


    UserDataPtr pregistration;
    while(!pregistration) {
        if( !pregistration && !!penv->GetViewer() ) {
            pregistration = penv->GetViewer()->RegisterViewerThreadCallback(boost::bind(ViewerCallback,penv->GetViewer()));
        }
        boost::this_thread::sleep(boost::posix_time::milliseconds(1));
    }

    thviewer.join(); // wait for the viewer thread to exit
    pregistration.reset();
    penv->Destroy(); // destroy
    return 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;
}

int main()
{
   
    traj.clear();
    unsigned int mainthreadsleft = numThreads;
    
   // boost::shared_ptr<boost::thread> ( new boost::thread(boost::bind( &track_threads )));
    Ta.trans = transA;
    Ta.rot = quatA;
    Tb.trans = transB;
    Tb.rot = quatB;
    std::string scenefilename = "scenes/test6dof.mujin.zae";
    std::string viewername = "qtcoin";
    RaveInitialize(true); // start openrave core
    EnvironmentBasePtr penv = RaveCreateEnvironment(); // create the main environment
    Transform robot_t;
    RaveVector< dReal > transR(c, d, 0);
    robot_t.trans = transR;

    //boost::thread thviewer(boost::bind(SetViewer,penv,viewername));
    penv->Load(scenefilename);
    RobotBasePtr probot = penv->GetRobot("RV-4F");

    //removing floor for collision checking
    EnvironmentBasePtr pclondedenv = penv->CloneSelf(Clone_Bodies);
    pclondedenv->Remove( pclondedenv->GetKinBody("floor"));
    RobotBasePtr probot_clone = pclondedenv->GetRobot("RV-4F");

    unsigned int tot = ((( abs(a) + abs(c) )/discretization_x )+1) * (((( abs(b) + abs(d) )/discretization_y )+1) * (( abs( z )/discretization_z )+1));
    unsigned int tot_o = tot;
   
    for (unsigned int i = 0 ; i <= (( abs(a) + abs(c) )/discretization_x );i++) {
	for (unsigned int j = 0 ; j <= (( abs(b) + abs(d) )/discretization_y ); j++) {
		for (unsigned int k = 0 ; k <= ( abs( z )/discretization_z ) ; k++) {
			////std::cout << transR[0] << ";" << transR[1] << ";" << transR[2] << std::endl;
			
			//boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
			robot_t.trans = transR;
			probot->SetTransform(robot_t);
                        probot_clone->SetTransform(robot_t);
			if( pclondedenv->CheckCollision(RobotBaseConstPtr(probot_clone)) ){
				//std::cout << "Robot in collision with the environment" << std::endl;
			}
			else {	
				
				do_task(Ta, Tb, penv,3);
			}
			tot -= 1;
			std::cout << tot << "/" << tot_o << std::endl;
			transR[2] = transR[2]+ discretization_z;
		}
		transR[2] = 0;
		transR[1] = transR[1] + discretization_y;
		robot_t.trans = transR;
	}
	transR[2] = 0;
	transR[1] = c;
	transR[0] = transR[0] + discretization_x;
	robot_t.trans = transR;
    }

 
    //thviewer.join(); // wait for the viewer thread to exit	
    RaveDestroy(); // make sure to destroy the OpenRAVE runtime
    penv->Destroy(); // destroy
    return 0;
}

//.........这里部分代码省略.........
    ControllerBasePtr wheelcontroller, armcontroller;
    // create the controllers, make sure to lock environment!
    {
        EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment

        MultiControllerPtr multi(new MultiController(penv));
        vector<int> dofindices(probot->GetDOF());
        for(int i = 0; i < probot->GetDOF(); ++i) {
            dofindices[i] = i;
        }
        probot->SetController(multi,dofindices,1); // control everything
        // set the velocity controller on all joints that have 'wheel' in their description
        for(std::vector<KinBody::JointPtr>::const_iterator itjoint = probot->GetJoints().begin(); itjoint != probot->GetJoints().end(); ++itjoint) {
            if( (*itjoint)->GetName().find("wheel") != string::npos ) {
                for(int i = 0; i < (*itjoint)->GetDOF(); ++i) {
                    wheelindices.push_back((*itjoint)->GetDOFIndex()+i);
                }
            }
            else {
                for(int i = 0; i < (*itjoint)->GetDOF(); ++i) {
                    restindices.push_back((*itjoint)->GetDOFIndex()+i);
                }
            }
        }

        if(wheelindices.size() > 0 ) {
            wheelcontroller = RaveCreateController(penv,"odevelocity");
            multi->AttachController(wheelcontroller,wheelindices,0);
        }

        if( restindices.size() > 0 ) {
            armcontroller = RaveCreateController(penv,"idealcontroller");
            multi->AttachController(armcontroller,restindices,0);
        }
        else {
            RAVELOG_WARN("robot needs to have wheels and arm for demo to work\n");
        }
    }

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

            if( !!armcontroller ) {
                // set a trajectory on the arm and velocity on the wheels
                TrajectoryBasePtr traj = RaveCreateTrajectory(penv,"");
                probot->SetActiveDOFs(restindices);
                ConfigurationSpecification spec = probot->GetActiveConfigurationSpecification();
                int timeoffset = spec.AddDeltaTime();
                traj->Init(spec);
                probot->GetActiveDOFValues(q); // get current values
                vector<dReal> vdata(spec.GetDOF(),0);
                std::copy(q.begin(),q.end(),vdata.begin());
                traj->Insert(0,vdata);
                for(int i = 0; i < 4; ++i) {
                    q.at(RaveRandomInt()%restindices.size()) += RaveRandomFloat()-0.5; // move a random axis
                }

                // check for collisions
                {
                    RobotBase::RobotStateSaver saver(probot); // add a state saver so robot is not moved permenantly
                    probot->SetActiveDOFValues(q);
                    if( probot->CheckSelfCollision() ) { // don't check env collisions since we have physics enabled
                        continue; // robot in collision at final point, so reject
                    }
                }

                std::copy(q.begin(),q.end(),vdata.begin());
                vdata.at(timeoffset) = 2; // trajectory takes 2s
                traj->Insert(1,vdata);
                planningutils::RetimeActiveDOFTrajectory(traj,probot,true);
                armcontroller->SetPath(traj);
            }

            if( !!wheelcontroller ) {
                stringstream sout,ss; ss << "setvelocity ";
                for(size_t i = 0; i < wheelindices.size(); ++i) {
                    ss << 2*(RaveRandomFloat()-0.5) << " ";
                }
                if( !wheelcontroller->SendCommand(sout,ss) ) {
                    RAVELOG_WARN("failed to send velocity command\n");
                }
            }
        }

        // unlock the environment and wait for the arm controller to finish (wheel controller will never finish)
        if( !armcontroller ) {
            usleep(2000000);
        }
        else {
            while(!armcontroller->IsDone()) {
                usleep(1000);
            }
        }
    }

    thviewer.join(); // wait for the viewer thread to exit
    penv->Destroy(); // destroy
    return 0;
}