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

bool TaskSpaceRegion::Initialize(EnvironmentBasePtr penv_in)
{
    _volume = 0;
    _sumbounds = 0;
    _dimensionality = 0;

    for(int i = 0; i < 6; i++)
    { 
        //compute volume in whatever dimensionality this defines                
        //when Bw values are backwards, it signifies an axis flip (not an error)
        if(Bw[i][1] != Bw[i][0])
        {
            _volume = _volume * fabs(Bw[i][1] - Bw[i][0]);
            _sumbounds = _sumbounds + fabs(Bw[i][1] - Bw[i][0]);
            _dimensionality++;
        }
    }

    if( stricmp(relativebodyname.c_str(), "NULL") == 0 )
    {
        prelativetolink.reset();
    }
    else
    {
        KinBodyPtr pobject;
        pobject = penv_in->GetKinBody(relativebodyname.c_str());
        if(pobject.get() == NULL)
        {
            RAVELOG_INFO("Error: could not find the specified object to attach frame\n");
            return false;
        }
       
        //find the link
        vector<KinBody::LinkPtr> vlinks = pobject->GetLinks();
        bool bGotLink = false;
        for(int j =0; j < vlinks.size(); j++)
        {
            if(strcmp(relativelinkname.c_str(), vlinks[j]->GetName().c_str()) == 0 )
            {
                RAVELOG_INFO("frame link: %s:%s\n",vlinks[j]->GetParent()->GetName().c_str(),vlinks[j]->GetName().c_str() );
                prelativetolink = vlinks[j];
                bGotLink = true;
                break;
            }
        }
        if(!bGotLink)
        {
            RAVELOG_INFO("Error: could not find the specified link of the object to attach frame\n");
            return false;
        }       
    }  


    //Print();
    return true;
}

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