void SetUp() { RAVELOG_DEBUG("SetUp\n"); RobotBasePtr robot = GetRobot(*env); robot->SetDOFValues(DblVec(robot->GetDOF(), 0)); Transform I; I.identity(); robot->SetTransform(I); }
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; }