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

int ORCEnvironmentGetRobots(void* env, void** robots)
{
    EnvironmentBasePtr penv = GetEnvironment(env);
    std::vector<RobotBasePtr> vrobots;
    penv->GetRobots(vrobots);
    if( !robots ) {
        return static_cast<int>(vrobots.size());
    }
    for(size_t i = 0; i < vrobots.size(); ++i) {
        robots[i] = new InterfaceBasePtr(vrobots[i]);
    }
    return vrobots.size();
}

int main() {
  RaveInitialize(false, OpenRAVE::Level_Debug);
  env = RaveCreateEnvironment();
  env->StopSimulation();
//  bool success = env->Load("data/pr2test2.env.xml");
  {
    bool success = env->Load("/home/joschu/Proj/drc/gfe.xml");
    FAIL_IF_FALSE(success);
  }
  {
    bool success = env->Load("/home/joschu/Proj/trajopt/data/test2.env.xml");
    FAIL_IF_FALSE(success);
  }
  vector<RobotBasePtr> robots;
  env->GetRobots(robots);
  RobotBasePtr robot = robots[0];
  vector<RobotBase::ManipulatorPtr> manips = robot->GetManipulators();


  cc = CollisionChecker::GetOrCreate(*env);
  viewer.reset(new OSGViewer(env));
  env->AddViewer(viewer);



  ManipulatorControl mc(manips[manips.size()-1], viewer);
  DriveControl dc(robot, viewer);
  StatePrinter sp(robot);
  viewer->AddKeyCallback('a', boost::bind(&StatePrinter::PrintAll, &sp));
  viewer->AddKeyCallback('q', &PlotCollisionGeometry);
  viewer->AddKeyCallback('=', boost::bind(&AdjustTransparency, .05));
  viewer->AddKeyCallback('-', boost::bind(&AdjustTransparency, -.05));

  viewer->Idle();

  env.reset();
  viewer.reset();
  RaveDestroy();

}

//.........这里部分代码省略.........
	
  /* timing */
  struct timespec t;
	
  /* hubo ACH Channel */
  struct hubo_ref H;
  memset( &H,   0, sizeof(H));
  int r;
	
  if(fflag){
    r = ach_open(&chan_num, HUBO_CHAN_REF_FILTER_NAME, NULL);
  }
  else{
    r = ach_open(&chan_num, HUBO_CHAN_REF_NAME, NULL);
  }

  size_t fs;


  /* read first set of data */
  r = ach_get( &chan_num, &H, sizeof(H), &fs, NULL, ACH_O_LAST );
  assert( sizeof(H) == fs );

  int contactpoints = 0;
  bool runflag = true;
  while(runflag) {
  {
    // lock the environment to prevent data from changing
    EnvironmentMutex::scoped_lock lock(penv->GetMutex());
    
    //vector<KinBodyPtr> vbodies;
    vector<RobotBasePtr> vbodies;
    //penv->GetBodies(vbodies);
    penv->GetRobots(vbodies);
    // get the first body
    if( vbodies.size() == 0 ) {
      RAVELOG_ERROR("no bodies loaded\n");
      return -3;
    }
    
    //KinBodyPtr pbody = vbodies.at(0);
    RobotBasePtr pbody = vbodies.at(0);
    vector<dReal> values;
    pbody->GetDOFValues(values);
    
    // set new values
    for(int i = 0; i < (int)vsetvalues.size() && i < (int)values.size(); ++i) {
      values[i] = vsetvalues[i];
    }
    
    pbody->Enable(true);
    //pbody->SetVisible(true);
    //CollisionReportPtr report(new CollisionReport());
    //bool runflag = true;
    //while(runflag) {
	
      /* Wait until next shot */
      clock_nanosleep(0,TIMER_ABSTIME,&t, NULL);

      /* Get updated joint info here */
      r = ach_get( &chan_num, &H, sizeof(H), &fs, NULL, ACH_O_LAST );
      assert( sizeof(H) == fs );

      
      /* set all joints from ach */
      int len = (int)(sizeof(openRAVEJointIndices)/sizeof(openRAVEJointIndices[0]));

int main(int argc, char ** argv)
{
    string scenefilename = "data/diffdrive_arm.env.xml";
    string viewername = "qtcoin";
    RaveInitialize(true);
    EnvironmentBasePtr penv = RaveCreateEnvironment();
    penv->SetDebugLevel(Level_Debug);

    boost::thread thviewer(boost::bind(SetViewer,penv,viewername)); // create the viewer
    usleep(400000); // wait for the viewer to init

    penv->Load(scenefilename);

    // attach a physics engine
    penv->SetPhysicsEngine(RaveCreatePhysicsEngine(penv,"ode"));
    penv->GetPhysicsEngine()->SetGravity(Vector(0,0,-9.8));

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

    vector<int> wheelindices, restindices;
    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) {
//.........这里部分代码省略.........