void SetUp() { RAVELOG_DEBUG("SetUp\n"); RobotBasePtr robot = GetRobot(*env); robot->SetDOFValues(DblVec(robot->GetDOF(), 0)); Transform I; I.identity(); robot->SetTransform(I); }
virtual void demothread(int argc, char ** argv) { string scenefilename = "data/pa10grasp2.env.xml"; penv->Load(scenefilename); vector<RobotBasePtr> vrobots; penv->GetRobots(vrobots); RobotBasePtr probot = vrobots.at(0); // find a manipulator chain to move for(size_t i = 0; i < probot->GetManipulators().size(); ++i) { if( probot->GetManipulators()[i]->GetName().find("arm") != string::npos ) { probot->SetActiveManipulator(probot->GetManipulators()[i]); break; } } RobotBase::ManipulatorPtr pmanip = probot->GetActiveManipulator(); // load inverse kinematics using ikfast ModuleBasePtr pikfast = RaveCreateModule(penv,"ikfast"); penv->Add(pikfast,true,""); stringstream ssin,ssout; vector<dReal> vsolution; ssin << "LoadIKFastSolver " << probot->GetName() << " " << (int)IKP_Transform6D; if( !pikfast->SendCommand(ssout,ssin) ) { RAVELOG_ERROR("failed to load iksolver\n"); } if( !pmanip->GetIkSolver()) { throw OPENRAVE_EXCEPTION_FORMAT0("need ik solver",ORE_Assert); } ModuleBasePtr pbasemanip = RaveCreateModule(penv,"basemanipulation"); // create the module penv->Add(pbasemanip,true,probot->GetName()); // load the module while(IsOk()) { { EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment // find a new manipulator position and feed that into the planner. If valid, robot will move to it safely. Transform t = pmanip->GetEndEffectorTransform(); t.trans += Vector(RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f); t.rot = quatMultiply(t.rot,quatFromAxisAngle(Vector(RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f)*0.2f)); ssin.str(""); ssin.clear(); ssin << "MoveToHandPosition pose " << t; // start the planner and run the robot RAVELOG_INFO("%s\n",ssin.str().c_str()); if( !pbasemanip->SendCommand(ssout,ssin) ) { continue; } } // unlock the environment and wait for the robot to finish while(!probot->GetController()->IsDone() && IsOk()) { boost::this_thread::sleep(boost::posix_time::milliseconds(1)); } } }
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; }
virtual bool isValid(const ob::State *state) const { if (si_->satisfiesBounds(state) == false) return false; RobotBasePtr probot; probot = si_->getStateSpace()->as<SuperBotStateSpace>()->_probot; const ob::CompoundStateSpace::StateType *s = state->as<ob::CompoundStateSpace::StateType>(); const double* q = s->as<ob::RealVectorStateSpace::StateType>(0)->values; const double* qd = s->as<ob::RealVectorStateSpace::StateType>(1)->values; std::vector<double> jointvalues; for (int i = 0; i < int(probot->GetActiveDOF()); i++) jointvalues.push_back(q[i]); for (int i = int(probot->GetActiveDOF()); i < int(probot->GetDOF()); i++) jointvalues.push_back(0.0); { EnvironmentMutex::scoped_lock lock(probot->GetEnv()->GetMutex()); probot->SetDOFValues(jointvalues, CLA_NOTHING); if (probot->CheckSelfCollision()) return false; if (probot->GetEnv()->CheckCollision(probot)) return false; else return true; } }
DensoControlSpace::DensoControlSpace(RobotBasePtr probot) : oc::RealVectorControlSpace(ob::StateSpacePtr(new DensoStateSpace(probot)), probot->GetActiveDOF()), _probot(probot) { unsigned int ndof = _probot->GetActiveDOF(); if (_controlViaAcceleration) { std::vector<double> qddBounds; _probot->GetDOFAccelerationLimits(qddBounds); for (unsigned int i = 0; i < ndof; i++) { bounds_.setLow(i, -qddBounds[i]); bounds_.setHigh(i, qddBounds[i]); } } else { std::vector<double> qdBounds; _probot->GetDOFVelocityLimits(qdBounds); for (unsigned int i = 0; i < ndof; i++) { bounds_.setLow(i, -qdBounds[i]); bounds_.setHigh(i, qdBounds[i]); } } }
void ReadFile(RobotBasePtr& probot, const std::string& filename, const AttributesList& atts) { std::ifstream f(filename.c_str()); if( !f ) { throw OPENRAVE_EXCEPTION_FORMAT("failed to read %s filename",filename,ORE_InvalidArguments); } f.seekg(0,ios::end); std::vector<char> filedata(static_cast<size_t>(f.tellg())+1, 0); // need a null-terminator f.seekg(0,ios::beg); f.read(&filedata[0], filedata.size()); Read(probot,filedata,atts); probot->__struri = filename; #if defined(HAVE_BOOST_FILESYSTEM) && BOOST_VERSION >= 103600 // stem() was introduced in 1.36 boost::filesystem::path bfpath(filename); #if defined(BOOST_FILESYSTEM_VERSION) && BOOST_FILESYSTEM_VERSION >= 3 probot->SetName(utils::ConvertToOpenRAVEName(bfpath.stem().string())); #else probot->SetName(utils::ConvertToOpenRAVEName(bfpath.stem())); #endif #endif }
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(); }
void Read(RobotBasePtr& probot, const std::vector<char>& data,const AttributesList& atts) { _ProcessAtts(atts); if( !probot ) { probot = RaveCreateRobot(_penv,_bodytype); } probot->SetName(_bodyname); Assimp::XFileParserOpenRAVE parser(data); _Read(probot,parser.GetImportedData()); if( probot->GetName().size() == 0 ) { probot->SetName("robot"); } // add manipulators FOREACH(itmanip,_listendeffectors) { RobotBase::ManipulatorInfo manipinfo; manipinfo._name = itmanip->first->_info._name; manipinfo._sEffectorLinkName = itmanip->first->GetName(); manipinfo._sBaseLinkName = probot->GetLinks().at(0)->GetName(); manipinfo._tLocalTool = itmanip->second; manipinfo._vdirection=Vector(1,0,0); probot->_vecManipulators.push_back(RobotBase::ManipulatorPtr(new RobotBase::Manipulator(probot,manipinfo))); }
virtual void demothread(int argc, char ** argv) { string scenefilename = "data/lab1.env.xml"; penv->Load(scenefilename); vector<RobotBasePtr> vrobots; penv->GetRobots(vrobots); RobotBasePtr probot = vrobots.at(0); std::vector<dReal> q; while(IsOk()) { { EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment TrajectoryBasePtr traj = RaveCreateTrajectory(penv,""); traj->Init(probot->GetActiveConfigurationSpecification()); probot->GetActiveDOFValues(q); // get current values traj->Insert(0,q); q[RaveRandomInt()%probot->GetDOF()] += 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->SetDOFValues(q); if( penv->CheckCollision(RobotBaseConstPtr(probot)) ) { continue; // robot in collision at final point, so reject } } traj->Insert(1,q); planningutils::SmoothActiveDOFTrajectory(traj,probot); probot->GetController()->SetPath(traj); // setting through the robot is also possible: probot->SetMotion(traj); } // unlock the environment and wait for the robot to finish while(!probot->GetController()->IsDone() && IsOk()) { boost::this_thread::sleep(boost::posix_time::milliseconds(1)); } } }
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; }
btTransform getKinectToWorld(RobotBasePtr robot) { return util::toBtTransform(robot->GetLink("camera_rgb_optical_frame")->GetTransform()); }
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; /* 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])); /* set all joints from ach */ for( int ii = 0; ii < (int)values.size() ; ii++ ) { values[ii] = 0.0; } for( int ii = 0; ii < len; ii++){ if(openRAVEJointIndices[ii] != -1){ values[openRAVEJointIndices[ii]] = H.ref[hubo_refJointIndices[ii]]; } } // values[RSY] = -1.0; // values[REB] = 1.0; pbody->SetDOFValues(values,true); // penv->GetCollisionChecker()->SetCollisionOptions(CO_Contacts); // if( pbody->CheckSelfCollision(report) ) { // cflag = true; // if(vflag | Vflag){ // RAVELOG_INFO("body not in collision\n"); // } // if(Vflag) { // contactpoints = (int)report->contacts.size(); // stringstream ss; // ss << "body in self-collision " // << (!!report->plink1 ? report->plink1->GetName() : "") << ":" // << (!!report->plink2 ? report->plink2->GetName() : "") << " at " // << contactpoints << "contacts" << endl; // for(int i = 0; i < contactpoints; ++i) { // CollisionReport::CONTACT& c = report->contacts[i]; // ss << "contact" << i << ": pos=(" // << c.pos.x << ", " << c.pos.y << ", " << c.pos.z << "), norm=(" // << c.norm.x << ", " << c.norm.y << ", " << c.norm.z << ")" << endl; // } // RAVELOG_INFOA(ss.str()); // } // } // else { // cflag = false; // if(vflag | Vflag) { // RAVELOG_INFO("body not in collision\n"); // } // } // get the transformations of all the links vector<Transform> vlinktransforms; pbody->GetLinkTransformations(vlinktransforms); //boost::this_thread::sleep(boost::posix_time::milliseconds(2000)); // boost::this_thread::sleep(boost::posix_time::milliseconds(1)); t.tv_nsec+=interval; tsnorm(&t); // runflag = false; //pbody->Enable(true); //pbody->SetVisible(true); usleep(10000); pbody->SimulationStep(0.01); penv->StepSimulation(0.01); } } pause(); RaveDestroy(); // destroy return contactpoints; }
TEST(continuous_collisions, 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"); CollisionCheckerPtr checker = CreateCollisionChecker(env); { RobotAndDOFPtr rad(new RobotAndDOF(boxbot, IntVec(), DOF_X | DOF_Y, Vector())); TrajArray traj(2,2); traj << -1.9,0, 0,1.9; vector<Collision> collisions; checker->ContinuousCheckTrajectory(traj, rad, collisions); ASSERT_EQ(collisions.size(), 1); Collision col = collisions[0]; Vector robot_normal = (float)(col.linkA == boxbot->GetLinks()[0].get() ? 1. : -1.) * col.normalB2A; EXPECT_VECTOR_NEAR(robot_normal, Vector(-1, 0, 0), 1e-4); } #define TRAJ_TEST_BOILERPLATE\ RobotAndDOFPtr rad(new RobotAndDOF(boxbot, IntVec(), DOF_X | DOF_Y, Vector()));\ vector<Collision> collisions;\ checker->CastVsAll(*rad, rad->GetRobot()->GetLinks(), toDblVec(traj.row(0)), toDblVec(traj.row(1)), collisions);\ ASSERT_EQ(collisions.size(), 1);\ Collision col = collisions[0];\ Vector robot_normal = (float)(col.linkA == boxbot->GetLinks()[0].get() ? 1. : -1.) * col.normalB2A; { TrajArray traj(2,2); traj << -1.9,0, 0,1.9; TRAJ_TEST_BOILERPLATE EXPECT_VECTOR_NEAR(robot_normal, Vector(-1/sqrtf(2), 1/sqrtf(2), 0), 1e-4); EXPECT_NEAR(col.time, .5, 1e-1); } { TrajArray traj(2,2); traj << 0, .9, 0,2; TRAJ_TEST_BOILERPLATE EXPECT_VECTOR_NEAR(robot_normal, Vector(0,1,0), 1e-4); EXPECT_NEAR(col.time, 0, 1e-6); } { TrajArray traj(2,2); traj << 0,2, 0,.9; TRAJ_TEST_BOILERPLATE EXPECT_VECTOR_NEAR(robot_normal, Vector(0,1,0), 1e-4); EXPECT_NEAR(col.time, 1, 1e-6); } { RobotAndDOFPtr rad(new RobotAndDOF(boxbot, IntVec(), DOF_X | DOF_Y | DOF_Z, Vector())); vector<Collision> collisions; vector<DblVec> multi_joints; multi_joints.push_back(toDblVec(Eigen::Vector3d(0,1.9,5))); multi_joints.push_back(toDblVec(Eigen::Vector3d(-1.9,0,5))); multi_joints.push_back(toDblVec(Eigen::Vector3d(0,-2,5))); multi_joints.push_back(toDblVec(Eigen::Vector3d(2,0,5))); multi_joints.push_back(toDblVec(Eigen::Vector3d(0,1.8,-5))); multi_joints.push_back(toDblVec(Eigen::Vector3d(-1.9,0,-5))); multi_joints.push_back(toDblVec(Eigen::Vector3d(0,-2,-5))); multi_joints.push_back(toDblVec(Eigen::Vector3d(2,0,-5))); checker->MultiCastVsAll(*rad, rad->GetRobot()->GetLinks(), multi_joints, collisions); ASSERT_EQ(collisions.size(), 1); Collision col = collisions[0]; Vector robot_normal = (float)(col.linkA == boxbot->GetLinks()[0].get() ? 1. : -1.) * col.normalB2A; EXPECT_VECTOR_NEAR(robot_normal, Vector(1/sqrtf(2), -1/sqrtf(2), 0), 1e-4); } }
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; }
virtual void demothread(int argc, char ** argv) { string scenefilename = "/home/yo/asibot/main/app/ravebot/models/asibot_kitchen_wheelchair.env.xml"; penv->Load(scenefilename); vector<RobotBasePtr> vrobots; penv->GetRobots(vrobots); KinBodyPtr objPtr = penv->GetKinBody("asibot"); if(objPtr) { if(vrobots.at(1)->Grab(objPtr)) printf("[success] object asibot exists and grabbed.\n"); else printf("[warning] object asibot exists but not grabbed.\n"); } else printf("[fail] object asibot does not exist.\n"); printf("begin wait\n"); sleep(5); printf("end wait\n"); RobotBasePtr probot = vrobots.at(1); vector<dReal> v(2); /* vector<int> vindices(probot->GetDOF()); for(size_t i = 0; i < vindices.size(); ++i) { vindices[i] = i; } probot->SetActiveDOFs(vindices);*/ vector<int> vindices; // send it empty instead //probot->SetActiveDOFs(vindices,DOF_X|DOF_Y|DOF_RotationAxis,Vector(0,0,1)); probot->SetActiveDOFs(vindices,DOF_X|DOF_Y,Vector(0,0,1)); ModuleBasePtr pbasemanip = RaveCreateModule(penv,"basemanipulation"); // create the module penv->Add(pbasemanip,true,probot->GetName()); // load the module int iteracion = 0; while(IsOk()) { { EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment // find a set of free joint values for the robot { RobotBase::RobotStateSaver saver(probot); // save the state while(1) { Transform T = probot->GetTransform(); if(iteracion%2) { v[0] = T.trans.x; v[1] = T.trans.y+1; //v[2] = T.trans.z; iteracion++; } else { v[0] = T.trans.x; v[1] = T.trans.y-0.5; //v[2] = T.trans.z; iteracion++; } if(iteracion==5) vrobots.at(1)->Release(objPtr); probot->SetActiveDOFValues(v); if( !penv->CheckCollision(probot) && !probot->CheckSelfCollision() ) { break; } } // robot state is restored } stringstream cmdin,cmdout; cmdin << "MoveActiveJoints maxiter 2000 maxtries 1 goal "; for(size_t i = 0; i < v.size(); ++i) { cmdin << v[i] << " "; } // start the planner and run the robot RAVELOG_INFO("%s\n",cmdin.str().c_str()); if( !pbasemanip->SendCommand(cmdout,cmdin) ) { continue; } } // unlock the environment and wait for the robot to finish while(!probot->GetController()->IsDone() && IsOk()) { boost::this_thread::sleep(boost::posix_time::milliseconds(1)); } } }
void initialize_robot(RobotBasePtr robot, const Vector2d& start) { robot->SetDOFValues(toDblVec(start)); }
SuperBotSetup::SuperBotSetup(RobotBasePtr probot) : oc::SimpleSetup(oc::ControlSpacePtr(new SuperBotControlSpace(probot))) { std::cout << "Planning for the first " << probot->GetActiveDOF() << " DOFs\n"; initialize(probot); }
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) { ss << 2*(RaveRandomFloat()-0.5) << " "; } if( !wheelcontroller->SendCommand(sout,ss) ) { RAVELOG_WARN("failed to send velocity command\n"); } } } // unlock the environment and wait for the arm controller to finish (wheel controller will never finish) if( !armcontroller ) { usleep(2000000); } else { while(!armcontroller->IsDone()) { usleep(1000); } } } thviewer.join(); // wait for the viewer thread to exit penv->Destroy(); // destroy return 0; }