bool Arena::moveRobots() { for (int k = 0; k < m_nRobots; k++) { Robot* rp = m_robots[k]; rp->move(); if (rp->row() == m_player->row() && rp->col() == m_player->col()) m_player->setDead(); } // return true if the player is still alive, false otherwise return ! m_player->isDead(); }
Robot* Model::loadRobot( const QString& fileName, bool verbose) { mutexData.lockForWrite(); DT_ResponseClass newRobotClass = newResponseClass( worldTable ); DT_ResponseClass newBaseClass = newResponseClass( worldTable ); DT_ResponseClass newFieldClass = newResponseClass( worldTable ); DT_ResponseClass newBaseFieldClass = newResponseClass( worldTable ); DT_AddPairResponse( worldTable, newRobotClass, obstacleClass, reflexTrigger, DT_WITNESSED_RESPONSE, (void*) this ); DT_AddPairResponse( worldTable, newRobotClass, obstacleClass, collisionHandler, DT_WITNESSED_RESPONSE, (void*) this ); DT_AddPairResponse( worldTable, newRobotClass, targetClass, collisionHandler, DT_WITNESSED_RESPONSE, (void*) this ); DT_AddPairResponse( worldTable, newFieldClass, obstacleClass, repel, DT_DEPTH_RESPONSE, (void*) this ); DT_AddPairResponse( worldTable, newFieldClass, obstacleClass, collisionHandler, DT_WITNESSED_RESPONSE, (void*) this ); QVector<DT_ResponseClass>::iterator i; for ( i = robotResponseClasses.begin(); i != robotResponseClasses.end(); ++i ) { DT_AddPairResponse( worldTable, newRobotClass, *i, reflexTrigger, DT_WITNESSED_RESPONSE, (void*) this ); DT_AddPairResponse( worldTable, newBaseClass, *i, reflexTrigger, DT_WITNESSED_RESPONSE, (void*) this ); DT_AddPairResponse( worldTable, newFieldClass, *i, repel, DT_DEPTH_RESPONSE, (void*) this ); DT_AddPairResponse( worldTable, newBaseFieldClass, *i, repel, DT_DEPTH_RESPONSE, (void*) this ); } robotResponseClasses.append( newRobotClass ); robotResponseClasses.append( newFieldClass ); //fieldResponseClasses.append( newFieldClass ); //robotBaseClasses.append( newBaseClass ); //printf("Loading non-yarp robot.\n"); DT_RespTableHandle newTable = newRobotTable(); // a table for handling self collisions DT_RespTableHandle newFieldTable = newRobotFieldTable(); // a table for handling self repulsion Robot* robot = new Robot( this, newTable, newFieldTable, newRobotClass, newBaseClass, newFieldClass, newBaseFieldClass ); mutexData.unlock(); robot->open( fileName, verbose ); // open calls appendObject, which locks the mutex by itself mutexData.lockForWrite(); robots.append( robot ); mutexData.unlock(); robot->appendMarkersToModel(); return robot; }
void Code() { string instructions; cin >> instructions; Robot rob; Position high,low; //we go through the instructions twice, once to get the maze dimensions and once to "draw" the maze for(unsigned int i=0; i<instructions.length(); i++) { Position ret=rob.Move(GetMove(instructions.at(i))); if(ret.x>high.x) { high.x=ret.x; } if(ret.y>high.y) { high.y=ret.y; } if(ret.x<low.x) { low.x=ret.x; //note that this can not happen for this specific problem since we start at the west-most edge of the maze } if(ret.y<low.y) { low.y=ret.y; } //cout << ">" << ret.x << "," << ret.y<<endl; } int w=high.x+abs(low.x)+2; //+2 because of outer wall & counting from 0 int h=high.y+abs(low.y)+3; //+3 because of outer wall & counting from 0 vector<vector<char>> maze; maze.resize(w); for(int i=0; i<w; i++) { maze[i].resize(h); for(int j=0; j<h; j++) { maze[i][j]='#'; } } Position start(-low.x,-low.y+1); //+1 because of outer wall rob.pos=start; maze[start.x][start.y]='.'; rob.dir=E; //now go through the instructions again and draw the maze for(unsigned int i=0; i<instructions.length(); i++) { Position ret=rob.Move(GetMove(instructions.at(i))); maze[ret.x][ret.y]='.'; } //output: cout<<h<<" "<<w<<endl; for(int i=h-1; i>=0; i--) { for(int j=0; j<w; j++) { cout << maze[j][i]; } cout<< endl; } }
Config RobotPoseWidget::Pose_Conditioned(const Config& qref) const { Robot* robot = linkPoser.robot; Assert(qref.n == linkPoser.poseConfig.n); Config res = linkPoser.poseConfig; for(int i=0; i<robot->q.n; i++) { if(robot->links[i].type == RobotLink3D::Revolute && robot->qMin(i)+TwoPi < robot->qMax(i)) { if(Abs(res[i]-qref[i]) > Pi) { res[i] = AngleDiff(AngleNormalize(res[i]),AngleNormalize(qref[i])) + qref[i]; } } } return res; }
confPtr_t move3d_robot_shoot(confPtr_t q, bool sample_passive) { Robot* R = q->getRobot(); #ifdef LIGHT_PLANNER if (ENV.getBool(Env::FKShoot)) { R->deactivateCcConstraint(); p3d_shoot(static_cast<p3d_rob*>(R->getP3dRobotStruct()), q->getConfigStruct(), false); R->setAndUpdate(*q); q = R->getCurrentPos(); R->activateCcConstraint(); // cout << "FKShoot" <<endl; return q; } else { p3d_shoot(static_cast<p3d_rob*>(R->getP3dRobotStruct()), q->getConfigStruct(), sample_passive); return q; } #else p3d_shoot(static_cast<p3d_rob*>(R->getP3dRobotStruct()), q->getConfigStruct(), sample_passive); return q; #endif }
int main (int argc, char *argv[]) { std::string hostname = "172.26.1.1"; //std::string hostname = "172.26.1.2"; if (argc > 1) hostname = argv[1]; Robot robot (hostname); robot.run (); return 0; }
bool ConstraintChecker::HasContactVelocity(const Robot& robot,const Stance& stance,Real maxErr) { Vector res; Vector3 dw,dv; for(Stance::const_iterator i=stance.begin();i!=stance.end();i++) { const IKGoal& g=i->second.ikConstraint; res.resize(IKGoal::NumDims(g.posConstraint)+IKGoal::NumDims(g.rotConstraint)); robot.GetWorldAngularVelocity(g.link,robot.dq,dw); robot.GetWorldVelocity(Vector3(Zero),g.link,robot.dq,dv); EvalIKGoalDeriv(g,robot.links[i->first].T_World,dw,dv,res); if(res.maxAbsElement() > maxErr) return false; } return true; }
void JointSliders::SliderCBWorker() { size_t k = 0; for (size_t h = 0; h < m_Robots->size(); h ++) { Robot *robot = m_Robots->at(h); size_t n = robot->GetLinks().size(); for (size_t i = 0; i < n; i++) { robot->ActuateJoint((int) i, m_Sliders.at(k)->value()); k = k + 1; } robot->Update(m_Verbose); } }
/* Main Loop * Open window with initial window size, title bar, * color index display mode, and handle input events. */ int main( int argc, char **argv ) { QApplication::setColorSpec( QApplication::CustomColor ); QApplication a( argc, argv ); if ( !QGLFormat::hasOpenGL() ) { qWarning( "This system has no OpenGL support. Exiting." ); return -1; } Robot w; a.setMainWidget( &w ); w.show(); return a.exec(); }
void Coop::assignRobots(Robot robot) { int id_set_busy; id_set_busy == 0; for (int i = 0; i < robots_.size(); i++) { if (robot.getNamespace() == robots_.at(i).getNamespace()) { id_set_busy = i; break; } } coop_pkg::SetBusy busy_srv; busy_srv.request.busy = true; if (set_busy_clis_.at(id_set_busy).call(busy_srv)) { ROS_INFO("OK: %s!!!", robot.getName().c_str()); } }
// handle MessageTurn message void MessageHandler::handle(MessageTurn *msg) { if (msg->envObjID == 0) { Robot *robot = HubModule::modellingSystem->getRobotByPort(msg->port); unsigned int currentOrientation = robot->getOrientation(); robot->setOrientation(currentOrientation + msg->degrees); } else { EnvObject* env = HubModule::modellingSystem->getEnvObject(msg->envObjID - 1); unsigned int currentOrientation = env->getOrientation(); env->setOrientation(currentOrientation + msg->degrees); } }
void MainWindow::createScene() //Renderer() { RasterMap *dda = new RasterMap( 40, 30, 0.1 ); dda->setPosition( -0.3, -1, -15 ); m_pRenderer->attachObject( dda ); Robot *robot = new Robot(); robot->setMovable( true ); robot->setPosition( 0, 2, -13 ); m_pRenderer->attachObject( robot, true ); WFObject *wf1 = new WFObject( "bowl.obj" ); wf1->setMaterial( "Marble" ); wf1->setTexture( "Marble" ); wf1->setPosition( -1, -2.4, -14.0 ); wf1->setMovable( true ); wf1->setRotatable( true ); wf1->setRotation( 0, 0, 0 ); m_pRenderer->attachObject( wf1 ); /* ParticleBox *ElasticPBox = new ParticleBox( 1.5, false, 0.9 ); ElasticPBox->setPosition( 3, -1, -13 ); m_pRenderer->attachObject( ElasticPBox ); ParticleBox *InelasticPBox = new ParticleBox( 1.5, true, 0.9 ); InelasticPBox->setPosition( 1, -1, -13 ); m_pRenderer->attachObject( InelasticPBox ); */ WFObject *wf2 = new WFObject( "brickwall.obj" ); wf2->setMaterial( "Wall" ); wf2->setTexture( "Wall" ); wf2->setPosition( 3.5, -2.0, -15.0 ); m_pRenderer->attachObject( wf2 ); WFObject *whiteboard = new WFObject( "whiteboard.obj" ); whiteboard->setMaterial( "Material" ); whiteboard->setTexture( "Whiteboard" ); whiteboard->setPosition( 0, -0.5, -15.1 ); m_pRenderer->attachObject( whiteboard ); WFObject *floor = new WFObject( "floor.obj" ); floor->setMaterial( "Floor" ); floor->setTexture( "Floor" ); floor->setPosition( 1.5, -2.5, -14.0 ); m_pRenderer->attachObject( floor ); }
void singlePtCmdCallBack ( const bosch_arm_control::PointCommand& msg ) { vector<double> act=rob->getMotorPosition ( msg.joint_angles ); for ( int i=0;i<4;i++ ) qd[i]=act[i]; }
void update() { rob->update(); //t_now=rob->time_now; clock_gettime ( CLOCK_REALTIME, &ts ); //double dt=rob->convertToWallTime ( t_now,t_last ); //t_last=t_now; double lambda = exp ( -2*3.14*cutoff*loop_time ); for ( int i=0;i<4;i++ ) q[i]=rob->q[i]; for ( int i=0;i<4;i++ ) { dq[i]=qd[i]-q[i]; if ( fabs ( dq[i] ) >e_lim ) safe_exit ( "position error too large" ); float dq_max=1.0*t_lims[i]/Kp[i]; truncate ( dq[i],dq_max ); v[i]= rob->v[i] * ( 1-lambda ) + lambda*v[i]; if ( fabs ( v[i] ) >v_lims[i] ) safe_exit ( "overspeed" ); torque[i]=Kp[i]*dq[i]-Kv[i]*v[i]; if ( fabs ( torque[i] ) >5*t_lim ) safe_exit ( "torque command over 5 times max achievable" ); truncate ( torque[i],t_lims[i] ); } for ( int i=0;i<4;i++ ) rob->torque[i]=torque[i]; logging(); }
Team::Team(Stage* stage, const Team& team) : QObject(stage), color_(team.color()), goals_(team.goals()), yellowCards_(team.yellowCards()), redCards_(team.redCards()) { stage_ = stage; for(int i=0; i<team.size(); i++){ Robot* r = new Robot( *(team.at(i)) ); r->setTeam(this); r->setStage(stage); this->push_back(r); } }
JointSliders::JointSliders(vector<Robot *> *robots, int verbose) { m_Running = 1; m_Robots = robots; m_Verbose = verbose; // How many joints we have? m_TotNumLinks = 0; for (size_t i = 0; i < m_Robots->size(); i++) { Robot *robot = m_Robots->at(i); m_TotNumLinks = m_TotNumLinks + robot->GetLinks().size(); } pthread_t thread1, thread2; pthread_create(&thread1, NULL, UDPReceiver, (void *) this); pthread_create(&thread2, NULL, Run, (void *) this); }
GameState::GameState(int numberOfRobots) { for (int i = 0; i < numberOfRobots; i++) { Robot* robot = new Robot(); //Randomly teleport the robot until it finds an empty location while (!isEmpty(*robot)) robot->teleport(); robots.push_back(robot); } robotsAlive = numberOfRobots; teleportHero(); }
int main() { //shared_ptr<Robot> robot2 = make_sharred<Robot>(); Robot Robot; try { Robot.NastavRychlost(7); Robot.NastavRychlost(18); } catch (std::length_error& error) { cout << error.what() << endl; } Robot.NastavRychlost(5); return 0; }
void GeneralizedRobot::GetMegaRobot(Robot& robot) const { vector<string> prefix; vector<Robot*> robotsAndObjects; list<Robot> convertedObjects; for(map<int,Element>::const_iterator i=elements.begin();i!=elements.end();i++) { if(i->second.robot) robotsAndObjects.push_back(i->second.robot); else { convertedObjects.push_back(Robot()); ObjectToRobot(*i->second.object,convertedObjects.back()); robotsAndObjects.push_back(&convertedObjects.back()); } if(i->second.name.empty()) { stringstream ss; ss<<"element_"<<i->first; prefix.push_back(ss.str()); } else prefix.push_back(i->second.name); } robot.Merge(robotsAndObjects); //fix up the names size_t nl = 0; for(size_t i=0;i<robotsAndObjects.size();i++) { for(size_t j=0;j<robotsAndObjects[i]->links.size();j++) robot.linkNames[nl+j] = prefix[i]+"["+robot.linkNames[nl+j]+"]"; nl += robotsAndObjects[i]->links.size(); } }
void EvaluateMultiPath(Robot& robot,const MultiPath& path,Real t,Config& q,Real xtol,Real contactol,int numIKIters) { RobotCSpace space(robot);; RobotGeodesicManifold manifold(robot); GeneralizedCubicBezierCurve curve(&space,&manifold); Real duration,param; int seg=path.Evaluate(t,curve,duration,param,MultiPath::InterpLinear); if(seg < 0) seg = 0; if(seg >= path.sections.size()) seg = (int)path.sections.size()-1; curve.Eval(param,q); //solve for constraints bool solveIK = false; if(!path.settings.contains("resolution")) solveIK = true; else { Real res = path.settings.as<Real>("resolution"); if(res > xtol) solveIK=true; } if(solveIK) { vector<IKGoal> ik; path.GetIKProblem(ik,seg); if(!ik.empty()) { swap(q,robot.q); robot.UpdateFrames(); int iters=numIKIters; bool res=SolveIK(robot,ik,contactol,iters,0); if(!res) printf("Warning, couldn't solve IK problem at sec %d, time %g\n",seg,t); swap(q,robot.q); } } }
void ObjectToRobot(const RigidObject& object,Robot& robot) { robot.InitializeRigidObject(); robot.torqueMax.setZero(); robot.powerMax.setZero(); robot.geometry.resize(6); robot.geometry[5] = object.geometry; robot.geomFiles.resize(6); robot.geomFiles[5] = object.geomFile; robot.selfCollisions.resize(6,6,NULL); robot.envCollisions.resize(6,NULL); robot.links[5].mass = object.mass; robot.links[5].com = object.com; robot.links[5].inertia = object.inertia; TransformToConfig(object.T,robot.q); robot.accMax.resize(6,Inf); robot.joints.resize(1); robot.joints[0].type = RobotJoint::Floating; robot.joints[0].linkIndex = 5; robot.joints[0].baseIndex = -1; robot.drivers.resize(0); robot.linkNames.resize(6); robot.linkNames[0] = "x"; robot.linkNames[1] = "y"; robot.linkNames[2] = "z"; robot.linkNames[3] = "rz"; robot.linkNames[4] = "ry"; robot.linkNames[5] = "rx"; robot.driverNames.resize(0); }
namespace Dungeon { Robot oRobot; bool Handle() // This is called by the 'MainLoop' when text input is selected { if (oRobot.IsAlive()) oRobot.Handle(); return oRobot.IsAlive(); // Returns if the program should continue or not } bool OnKeyPress(int a_iKey) // THis is called by the 'MainLoop' when standard input is selected { if (oRobot.IsAlive()) oRobot.OnKeyPress(a_iKey); return oRobot.IsAlive(); // Returns if the program should continue or not } void Init() { oRobot = Robot(1, 1, DIRECTION::DOWN); // Create a player character at (1, 1) facing downwards } void Quit() { } }
int main(int argc, char *argv[]) { // Initialize GLUT and open a window. glutInit(&argc, argv); glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_DEPTH); glutInitWindowSize(800, 600); glutCreateWindow(argv[0]); // Register a bunch of callbacks for GLUT events. glutDisplayFunc(display); glutReshapeFunc(reshape); glutMotionFunc(mouseMove); glutMouseFunc(mouseClick); glutKeyboardFunc(keyboard); // Setup OpenGL initOpenGL(); // Initialize our robot. if (argc > 1) { std::vector<Pose> poses; readPoseFile(argv[1], poses); robot.setPoses(poses); } // Schedule the first animation callback ASAP. glutTimerFunc(0, animate, 0); // Pass control to GLUT. glutMainLoop(); // Will never be reached. return 0; }
int RobotWorld::LoadRobot(const string& fn) { Robot* robot = new Robot; if(!robot->Load(fn.c_str())) { delete robot; return -1; } const char* justfn = GetFileName(fn.c_str()); char* buf = new char[strlen(justfn)+1]; strcpy(buf,justfn); StripExtension(buf); string name=buf; delete [] buf; int i = AddRobot(name,robot); return i; }
void logging() { std::string log; std::stringstream out; for ( int i=0;i<4;i++ ) out<<q[i]<<","; for ( int i=0;i<4;i++ ) out<<qd[i]<<","; for ( int i=0;i<4;i++ ) out<<v[i]<<","; for ( int i=0;i<4;i++ ) out<<torque[i]<<","; out << ts.tv_sec << ','<< ts.tv_nsec; log = out.str(); diag.data=log; diag.header.stamp.sec=ts.tv_sec; diag.header.stamp.nsec=ts.tv_nsec; diagnostic_pub.publish ( diag ); vector<double> cur_pos_act = rob->getJointPosition(); js.header.stamp.sec=ts.tv_sec; js.header.stamp.nsec=ts.tv_nsec; js.position=cur_pos_act; joint_state_pub.publish ( js ); }
void Simulation::check_scan(Robot &robot) { std::list<std::shared_ptr<Robot>> scanTargets; for (auto const &player2 : players) { if (&robot == &player2.second) { continue; } const Vector_d pose1 = robot.getPosition(); const double rotation = wrapRadians(robot.getRotation() + robot.getTurretAngle()); const Vector_d pose2 = player2.second.getPosition(); if (inScanArea(pose1, rotation, pose2)) { scanTargets.push_back(std::make_shared<Robot>(player2.second)); } } robot.setScanTargets(std::move(scanTargets)); }
/** * @brief Main thread function for Proxy166. * Runs forever, until MyTaskInitialized is false. * * @todo Update DS switch array */ int Proxy::Main( int a2, int a3, int a4, int a5, int a6, int a7, int a8, int a9, int a10) { Robot *lHandle = NULL; WaitForGoAhead(); lHandle = Robot::getInstance(); Timer matchTimer; while(MyTaskInitialized) { setNewpress(); if(lHandle->IsOperatorControl() && lHandle->IsEnabled()) { if(manualDSIO) { SetEnhancedIO(); } if(manualJoystick[0]) { SetJoystick(1, stick1); } if(manualJoystick[1]) { SetJoystick(2, stick2); } if(manualJoystick[2]) { SetJoystick(3, stick3); } if(manualJoystick[3]) { SetJoystick(4, stick4); } } if(!lHandle->IsEnabled()) { matchTimer.Reset(); // It became disabled matchTimer.Stop(); set("matchtimer",0); } else { // It became enabled matchTimer.Start(); if(lHandle->IsAutonomous()) { set("matchtimer",max( 15 - matchTimer.Get(),0)); } else { set("matchtimer",max(120 - matchTimer.Get(),0)); } } // The task ends if it's not initialized WaitForNextLoop(); } return 0; }
int main() { Robot bob; Field fld(20); char c; while(cin >> c) { switch(c) { case 'w': bob.up();break; case 's': bob.down();break; case 'a': bob.left();break; case 'd': bob.right();break; } fld.print(cout, bob); } return 0; }
void Canvas::setup(int bots, int lights, QVector<QPointF> botLoc, QVector<QPointF> lightLoc, int **matrix) { this->addLine(0,0,400,400); if (bots == 0) { bots = 1; qDebug() << "ERROR: Invalid number of robots. Defaulting to 1."; } if (lights == 0) { lights = 1; qDebug() << "ERROR: Invalid number of lights. Defaulting to 1."; } if (botLoc.size() < bots) { botLoc.clear(); botLoc = defaultBotLoc(bots); qDebug() << "ERROR: Not enough robot locations. Using the default locations."; } if (lightLoc.size() < bots) { lightLoc.clear(); lightLoc = defaultLightLoc(lights); qDebug() << "ERROR: Not enough light locations. Using the default locations."; } m_robotManager->setKMatrix(matrix); Robot* robot; for(int i = 0; i < bots; i++) { robot = new Robot(); robot->setTransformOriginPoint(ROBOT_HEIGHT, ROBOT_HEIGHT); robot->setPos(botLoc[i]); createRobot(robot); } LightSource *light; for (int i = 0; i < lights; i++) { light = new LightSource(); light->setPos(lightLoc[i]); // TODO: take intensity from a file light->setIntensity(100); createLight(light); } }
void Environment::handleRadioTx(bool blue, const Packet::RadioTx& tx) { for (int i = 0; i < tx.robots_size(); ++i) { const Packet::RadioTx::Robot& cmd = tx.robots(i); Robot* r = robot(blue, cmd.robot_id()); if (r) { // run controls update r->radioTx(&cmd); // trigger signals to update visualization Geometry2d::Point pos2 = r->getPosition(); QVector3D pos3(pos2.x, pos2.y, 0.0); QVector3D axis(0.0, 0.0, 1.0); } else { printf("Commanding nonexistent robot %s:%d\n", blue ? "Blue" : "Yellow", cmd.robot_id()); } Packet::RadioRx rx = r->radioRx(); rx.set_robot_id(r->shell); // Send the RX packet std::string out; rx.SerializeToString(&out); if (blue) _radioSocketBlue.writeDatagram(&out[0], out.size(), LocalAddress, RadioRxPort + 1); else _radioSocketYellow.writeDatagram(&out[0], out.size(), LocalAddress, RadioRxPort); } // FIXME: the interface changed for this part // Robot *rev = robot(blue, tx.robot_id()); // if (rev) // { // Packet::RadioRx rx = rev->radioRx(); // rx.set_robot_id(tx.robot_id()); // // // Send the RX packet // std::string out; // rx.SerializeToString(&out); // _radioSocket[ch].writeDatagram(&out[0], out.size(), LocalAddress, // RadioRxPort + ch); // } }