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

 static void SetUpTestCase() {
   RAVELOG_DEBUG("SetupTestCase\n");
   RaveInitialize(false, verbose ? Level_Debug : Level_Info);
   env = RaveCreateEnvironment();
   env->StopSimulation();
   env->Load("robots/pr2-beta-static.zae") ;
   env->Load(string(DATA_DIR) + "/table.xml");
   if (plotting) {
     viewer.reset(new OSGViewer(env));
     viewer->UpdateSceneData();
     env->AddViewer(viewer);
   }
 }

TEST(cast, boxes) {
  EnvironmentBasePtr env = RaveCreateEnvironment();
  ASSERT_TRUE(env->Load(data_dir() + "/box.xml"));
  ASSERT_TRUE(env->Load(data_dir() + "/boxbot.xml"));
  KinBodyPtr box = env->GetKinBody("box");
  RobotBasePtr boxbot = env->GetRobot("boxbot");
  RobotAndDOFPtr rad(new RobotAndDOF(boxbot, IntVec(), DOF_X | DOF_Y, Vector()));
  rad->GetRobot()->SetActiveDOFs(rad->GetJointIndices(), DOF_X | DOF_Y, Vector());
  Json::Value root = readJsonFile(string(DATA_DIR) + "/box_cast_test.json");
  DblVec start_dofs; start_dofs += -1.9, 0;
  rad->SetDOFValues(start_dofs);
  TrajOptProbPtr prob = ConstructProblem(root, env);
  TrajArray traj = prob->GetInitTraj();


  //shouldn't be necessary:
#if 0
  ASSERT_TRUE(!!prob);
  double dist_pen = .02, coeff = 10;
  prob->addCost(CostPtr(new CollisionCost(dist_pen, coeff, rad, prob->GetVarRow(0), prob->GetVarRow(1))));
  prob->addCost(CostPtr(new CollisionCost(dist_pen, coeff, rad, prob->GetVarRow(1), prob->GetVarRow(2))));
  CollisionCheckerPtr checker = CollisionChecker::GetOrCreate(*prob->GetEnv());
  {
    vector<Collision> collisions;
    checker->ContinuousCheckTrajectory(traj, *rad, collisions);
  }
  vector<Collision> collisions;
  cout << "traj: " << endl << traj << endl;
  checker->CastVsAll(*rad, rad->GetRobot()->GetLinks(), toDblVec(traj.row(0)), toDblVec(traj.row(1)), collisions);
  cout << collisions.size() << endl;
#endif

  BasicTrustRegionSQP opt(prob);
  if (plotting) opt.addCallback(PlotCallback(*prob));
  opt.initialize(trajToDblVec(prob->GetInitTraj()));
  opt.optimize();
  if (plotting) PlotCallback(*prob)(NULL, opt.x());




}

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();

}

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)
{
    std::string environment_uri;
    std::string robot_name;
    std::string plugin_name;
    size_t num_trials;
    bool self = false;
    bool profile = false;

    // Parse arguments.
    po::options_description desc("Profile OpenRAVE's memory usage.");
    desc.add_options()
        ("num-samples", po::value<size_t>(&num_trials)->default_value(10000),
            "number of samples to run")
        ("self", po::value<bool>(&self)->zero_tokens(),
            "run self-collision checks")
        ("profile", po::value<bool>(&profile)->zero_tokens(),
            "remove objects from environment")
        ("environment_uri",
            po::value<std::string>(&environment_uri)->required(),
            "number of samples to run")
        ("robot", po::value<std::string>(&robot_name)->required(),
            "robot_name")
        ("collision_checker",
            po::value<std::string>(&plugin_name)->required(),
            "collision checker name")
        ("help",
            "print usage information")
        ;

    po::positional_options_description pd;
    pd.add("environment_uri", 1);
    pd.add("robot", 1);
    pd.add("collision_checker", 1);

    po::variables_map vm;
    po::store(
        po::command_line_parser(argc, argv)
            .options(desc)
            .positional(pd).run(),
        vm
    );
    po::notify(vm);    

    if (vm.count("help")) {
        std::cout << desc << std::endl;
        return 1;
    }

    // Create the OpenRAVE environment.
    RaveInitialize(true);

    EnvironmentBasePtr const env = RaveCreateEnvironment();
    CollisionCheckerBasePtr const collision_checker
            = RaveCreateCollisionChecker(env, plugin_name);
    env->SetCollisionChecker(collision_checker);

    env->StopSimulation();

    // "/usr/share/openrave-0.9/data/wamtest1.env.xml"
    env->Load(environment_uri);
    KinBodyPtr const body = env->GetKinBody(robot_name);

    // Generate random configuations.
    std::vector<OpenRAVE::dReal> lower;
    std::vector<OpenRAVE::dReal> upper;
    body->GetDOFLimits(lower, upper);
	std::vector<std::vector<double> > data;
    data = benchmarks::DataUtils::GenerateRandomConfigurations(
            num_trials, lower, upper);

    //
    RAVELOG_INFO("Running %d collision checks.\n", num_trials);

    boost::timer const timer;
    if (profile) {
        std::string const prof_name = str(
                format("CheckCollision.%s.prof") %  plugin_name);
        RAVELOG_INFO("Writing gperftools information to '%s'.\n",
            prof_name.c_str()
        );

        ProfilerStart(prof_name.c_str());
    }

    size_t num_collision = 0;
    for (size_t i = 0; i < num_trials; ++i) {
        body->SetDOFValues(data[i]);

        bool is_collision;
        if (self) {
            is_collision = body->CheckSelfCollision();
        } else {
            is_collision = env->CheckCollision(body);
        }
        num_collision += !!is_collision;
    }

    if (profile) {
        ProfilerStop();
//.........这里部分代码省略.........

int main(int argc, char ** argv)
{

  //int interval = 1000000000; // 1hz (1.0 sec)
  //int interval = 500000000; // 2hz (0.5 sec)
  //int interval = 10000000; // 100 hz (0.01 sec)
  int interval =   40000000; // 25 hz (0.04 sec)

  // At this point an explanation is necessary for the following arrays
  //
  // jointNames is the order makeTraj python code saves the joint values in plain text format
  string jointNames[] = {"RHY", "RHR", "RHP", "RKP", "RAP", "RAR", "LHY", "LHR", "LHP", "LKP", "LAP", "LAR", "RSP", "RSR", "RSY", "REP", "RWY", "RWR", "RWP", "LSP", "LSR", "LSY", "LEP", "LWY", "LWR", "LWP", "NKY", "NK1", "NK2", "HPY", "rightThumbKnuckle1", "rightIndexKnuckle1", "rightMiddleKnuckle1", "rightRingKnuckle1", "rightPinkyKnuckle1", "leftThumbKnuckle1", "leftIndexKnuckle1", "leftMiddleKnuckle1", "leftRingKnuckle1", "leftPinkyKnuckle1"};
  
  // openRAVE Joint Indices keeps the mapping between the order given above and hubo's joint index in openRAVE. (E.g. RHY's index in openRAVE is 1, RHR's is 3 and so on). A -1 means that the joint name does not exist in openRAVE but it exists on the real robot. (E.g. RWR)
  int openRAVEJointIndices[] = {1, 3, 5, 7, 9, 11, 2, 4, 6, 8, 10, 12, 13, 15, 17, 19, 21, -1, 23, 14, 16, 18, 20, 22, -1, 24, -1, -1, -1, 0, 41, 29, 32, 35, 38, 56, 44, 47, 50, 53};

  // hubo_ref Joint Indices keeps the mapping between openRAVE joint Indices and the order hubo-read-trajectory writes in ach channel. (E.g. RHY is the 26th element of H.ref[] array).
  int hubo_refJointIndices[] = {26, 27, 28, 29, 30, 31, 19, 20, 21, 22, 23, 24, 11, 12, 13, 14, 15, 16, 17, 4, 5, 6, 7, 8, 9, 10, 1, 2, 3, 0, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41};

  /* Joint Numbers/Index values */	
  string viewername = "qtcoin";
  string scenefilename = "../../../openHubo/huboplus/";
  RaveInitialize(true); // start openrave core
  EnvironmentBasePtr penv = RaveCreateEnvironment(); // create the main environment
	
  int i = 1;
  bool vflag = false; // verbose mode flag
  bool Vflag = false; // extra verbose mode flag
  bool cflag = false; // self colision flag
  bool fflag = false; // ref vs. ref-filter channel flag
  /* For Viewer */
  if(argc <= i ){
    printf("Loading Headless\n");	
  }
  while(argc > i) {
    if(strcmp(argv[i], "-g") == 0) {
      //RaveSetDebugLevel(Level_Debug);
      boost::thread thviewer(boost::bind(SetViewer,penv,viewername));
    }
    else {
      printf("Loading Headless\n");
    }

		
    if(strcmp(argv[i], "-v") == 0) {
      vflag = true;
      printf("Entering Verbose Mode");
    }
    else {
      vflag = false;
    }
		
    if(strcmp(argv[i], "-V") == 0) {
      Vflag = true;
      printf("Entering Extra Verbose Mode");
    }

    if(strcmp(argv[i], "-f") == 0) {
      fflag = true;
      printf("listening to filtered channel.");
    }
		
    if(strcmp(argv[i], "-m") == 0) {
      i++;
      string hubomodel;
      hubomodel = argv[i];
      scenefilename.append(hubomodel);
      scenefilename.append(".robot.xml");
      cout<<"Loading model "<<scenefilename<<endl;
    }
    i++;
  }

  vector<dReal> vsetvalues;
  // parse the command line options
    
  // load the scene
  if( !penv->Load(scenefilename) ) {
    return 2;
  }

  // Wait until the robot model appears
  usleep(1000000);
	
  /* 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;
//.........这里部分代码省略.........

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;
}

int main(int argc, char *argv[]) {
  int T = 4;
  bool sim_plotting = false;
  bool stage_plotting = false;
  bool first_step_only = false;

  {
    Config config;
    config.add(new Parameter<bool>("sim_plotting", &sim_plotting, "sim_plotting"));
    config.add(new Parameter<bool>("stage_plotting", &stage_plotting, "stage_plotting"));
    config.add(new Parameter<bool>("first_step_only", &first_step_only, "first_step_only"));
    CommandParser parser(config);
    parser.read(argc, argv, true);
  }

  string manip_name("base_point");
  string link_name("Base");

  RaveInitialize();
  EnvironmentBasePtr env = RaveCreateEnvironment();
  env->StopSimulation();
  env->Load(string(DATA_DIR) + "/point.env.xml");
  OSGViewerPtr viewer;
  RobotBasePtr robot = GetRobot(*env);


  Vector2d start = Vector2d::Zero();
  Matrix2d start_sigma = Matrix2d::Identity() * 0.2 * 0.2;

  deque<Vector2d> initial_controls;
  for (int i = 0; i < T; ++i) {
    //initial_controls.push_back((Vector2d(0, 5) - start) / T);//Vector2d::Zero());
    initial_controls.push_back(Vector2d::Zero());
  }

  Matrix4d goal_trans;
  goal_trans <<  1, 0, 0, 0,
                 0, 1, 0, 5,
                 0, 0, 1, 0,
                 0, 0, 0, 1;

  initialize_robot(robot, start);

  PointRobotBSPPlannerPtr planner(new PointRobotBSPPlanner());

  planner->start = start;
  planner->start_sigma = start_sigma;
  planner->goal_trans = goal_trans;
  planner->T = T;
  planner->controls = initial_controls;
  planner->robot = robot;
  planner->rad = RADFromName(manip_name, robot);
  planner->link = planner->rad->GetRobot()->GetLink(link_name);
  planner->method = BSP::DiscontinuousBeliefSpace;
  planner->initialize();

  cout << "dof: " << planner->rad->GetDOF() << endl;

  boost::function<void(OptProb*, DblVec&)> opt_callback;
  if (stage_plotting || sim_plotting) {
    viewer = OSGViewer::GetOrCreate(env);
    initialize_viewer(viewer);
  }

  if (stage_plotting) {
    opt_callback = boost::bind(&stage_plot_callback, 
                               planner, viewer, _1, _2);
  }

  while (!planner->finished()) {
    planner->solve(opt_callback, 1, 1);
    planner->simulate_execution();
    if (first_step_only) break;
    if (sim_plotting) {
      OpenRAVEPlotterMixin<PointRobotBSPPlanner>::sim_plot_callback(planner, planner->rad, viewer);
    }
  }

  RaveDestroy();
  return 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) {
//.........这里部分代码省略.........