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