C++ EnvironmentBasePtr类代码示例

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

void GetTaskTime(EnvironmentBasePtr _penv, std::vector<dReal> &vsolutionA, std::vector<dReal> &vsolutionB, TrajectoryBasePtr ptraj){

	//thread::id get_id();
	std::cout << "this thread ::" << boost::this_thread::get_id() << std::endl;
	PlannerBasePtr planner = RaveCreatePlanner(_penv,"birrt");
	//std::vector<dReal> vinitialconfig,vgoalconfig;
	ptraj = RaveCreateTrajectory(_penv,"");
	PlannerBase::PlannerParametersPtr params(new PlannerBase::PlannerParameters());
	RobotBasePtr probot = _penv->GetRobot(robotname);
	params->_nMaxIterations = 4000; // max iterations before failure

	GraphHandlePtr pgraph;
	{
		EnvironmentMutex::scoped_lock lock(_penv->GetMutex()); // lock environment
		params->SetRobotActiveJoints(probot); // set planning configuration space to current active dofs

		//initial config
		probot->SetActiveDOFValues(vsolutionA);
		params->vinitialconfig.resize(probot->GetActiveDOF());
		probot->GetActiveDOFValues(params->vinitialconfig);

		//goal config	
		probot->SetActiveDOFValues(vsolutionB);
		params->vgoalconfig.resize(probot->GetActiveDOF());
		probot->GetActiveDOFValues(params->vgoalconfig);

		//setting limits
		vector<dReal> vlower,vupper;
		probot->GetActiveDOFLimits(vlower,vupper);

		//RAVELOG_INFO("starting to plan\n");
		
		if( !planner->InitPlan(probot,params) ) {
			//return ptraj;
		}
		// create a new output trajectory
		
		if( !planner->PlanPath(ptraj) ) {
			RAVELOG_WARN("plan failed \n");
			//return NULL;
			
		}

		
	}
	
	
	////RAVELOG_INFO(ss.str());
	if (!! ptraj){
		//std::cout << ptraj->GetDuration() << std::endl;	
		writeTrajectory(ptraj);
	}
	std::cout << "this thread ::" << boost::this_thread::get_id() << " has completed its work" << std::endl;
	mtx__.lock();
	completed_thread = boost::this_thread::get_id();
	mtx__.unlock();
	//return  ptraj;


}

int ORCEnvironmentGetBodies(void* env, void** bodies)
{
    EnvironmentBasePtr penv = GetEnvironment(env);
    std::vector<KinBodyPtr> vbodies;
    penv->GetBodies(vbodies);
    if( !bodies ) {
        return static_cast<int>(vbodies.size());
    }
    for(size_t i = 0; i < vbodies.size(); ++i) {
        bodies[i] = new InterfaceBasePtr(vbodies[i]);
    }
    return vbodies.size();
}

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

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




}

void SetViewer(EnvironmentBasePtr penv, const string& viewername)
{
  ViewerBasePtr viewer = RaveCreateViewer(penv,viewername);
  BOOST_ASSERT(!!viewer);

  // attach it to the environment:
  penv->AddViewer(viewer);

  // finally call the viewer's infinite loop (this is why a separate thread is needed)
  bool showgui = true;
  viewer->main(showgui);
}

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;
//.........这里部分代码省略.........

InterfaceBase::InterfaceBase(InterfaceType type, EnvironmentBasePtr penv) : __type(type), __penv(penv)
{
    RaveInitializeFromState(penv->GlobalState()); // make sure global state is set
    RegisterCommand("help",boost::bind(&InterfaceBase::_GetCommandHelp,this,_1,_2),
                    "display help commands.");
}

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

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

KinBodyPtr GetCppKinBody(py::object py_kb, EnvironmentBasePtr env) {
  int id = py::extract<int>(py_kb.attr("GetEnvironmentId")());
  return env->GetBodyFromEnvironmentId(id);
}

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

void do_task(Transform Ta, Transform Tb, EnvironmentBasePtr _penv, unsigned int thread_count_left)
{
	std::vector< std::vector< dReal > > vsolutionsA, vsolutionsB;
	dReal time_seconds;
	
	TrajectoryBasePtr ptraj;
	EnvironmentBasePtr pEnv = _penv->CloneSelf(Clone_Bodies);
	
	if (!!(GetIKSolutions(_penv, Tb, vsolutionsB) && GetIKSolutions(_penv, Ta,vsolutionsA))){
		//RAVELOG_INFO("Solution found for both source and goal\n");
		
	} 
	else {

		////RAVELOG_INFO("No solution found either for source or goal");
	}
	k_threads = 1;
	unsigned int _combinations = vsolutionsA.size() * vsolutionsB.size() , combinations = 0;
	std::cout << "Found " << _combinations << " combinations" << std::endl;
	if (!! _combinations){
	vector<boost::shared_ptr<boost::thread> > vthreads(_combinations);
	for(size_t i = 0; i < vsolutionsA.size(); ++i) {
		//RAVELOG_INFO("Planning for every Pose A \n");
			for(size_t j = 0; j < vsolutionsB.size(); ++j) {
				//RAVELOG_INFO("to every Pose B \n");
				std::cout << "i " << i << "j " <<j << std::endl;
				combinations +=1;
				//check if all threads are already running
				 if (k_threads == thread_count_left || combinations == _combinations){
					 for(size_t k = 0; k < k_threads-1; ++k) {
					        std::cout << "Retrieved thread to be complete " << completed_thread << std::endl;
						vthreads[k]->join();
						//k_threads -= 1;
						RAVELOG_INFO("joined ........................\n");
					  }


					 /*for(size_t k = 0; k < k_threads-1; ++k) {
					        std::cout << "Retrieved thread to be complete " << completed_thread << std::endl;
						if (vthreads[k]->get_id() == completed_thread){
								vthreads[k]->join();
								k_threads -= 1;
								RAVELOG_INFO("joined \n");

						}
						
					  }*/
				 }
	
				 else if(k_threads <= thread_count_left){
					//std::cout << k_threads-1 << std::endl;
					
				 	vthreads[k_threads-1].reset(new boost::thread(boost::bind(&GetTaskTime, _penv, vsolutionsA[i], vsolutionsB[j],ptraj)));
					//vthreads[k_threads].get_id();
					k_threads += 1;
					continue;
					
				 }
				  
			          //RAVELOG_INFO("wait for threads to finish\n");
				  //std::cout << vthreads.size() << std::endl;
				  //boost::this_thread::sleep(boost::posix_time::milliseconds(10));
				 
				 
				   
				   ////RAVELOG_INFO("threads finished\n");
				   //vthreads.resize(0);
			}
	}
	
 	
	
	}

}