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; } }
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/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) { //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; }
void initialize_robot(RobotBasePtr robot, const Vector2d& start) { robot->SetDOFValues(toDblVec(start)); }