void threadRelease() { printf("ControlThread:stopping the robot\n"); ivel->stop(); dd.close(); printf("Done, goodbye from ControlThread\n"); }
bool close() { handlerPort.close(); objectsPort.close(); igaze->stopControl(); igaze->restoreContext(startupGazeContextID); igaze->deleteContext(startupGazeContextID); igaze->deleteContext(currentGazeContextID); if (clientGazeCtrl.isValid()) clientGazeCtrl.close(); icartLeft->stopControl(); icartLeft->restoreContext(startupArmLeftContextID); icartLeft->deleteContext(startupArmLeftContextID); icartLeft->deleteContext(currentArmLeftContextID); if (clientArmLeft.isValid()) clientArmLeft.close(); icartRight->stopControl(); icartRight->restoreContext(startupArmRightContextID); icartRight->deleteContext(startupArmRightContextID); icartRight->deleteContext(currentArmRightContextID); if (clientArmLeft.isValid()) clientArmLeft.close(); itorsoVelocity->stop(); if (clientTorso.isValid()) clientTorso.close(); return true; }
bool exploreTorso(Vector target) { Vector torsoInitialJoints; Vector torsoActualJoints; Vector torsoVelocityCommand; Vector torsoAccCommand; Vector error,integral,derivative,preError; int jointsNumber=0; int i; double time= 0.0; itorsoVelocity->getAxes(&jointsNumber); torsoAccCommand.resize(jointsNumber); torsoVelocityCommand.resize(jointsNumber); torsoInitialJoints.resize(jointsNumber); torsoActualJoints.resize(jointsNumber); error.resize(jointsNumber); integral.resize(jointsNumber); derivative.resize(jointsNumber); preError.resize(jointsNumber); preError[0] = 0.0; preError[1] = 0.0; preError[2] = 0.0; integral[0] = 0.0; integral[1] = 0.0; integral[2] = 0.0; derivative[0] = 0.0; derivative[1] = 0.0; derivative[2] = 0.0; for(i =0; i< jointsNumber;i++); torsoAccCommand[i] = torsoAcceleration[i]; itorsoVelocity->setRefAccelerations(torsoAccCommand.data()); if (!iTorsoEncoder->getEncoders(torsoInitialJoints.data())){ cout<<"Error in reading encoders."<<endl; return false; } VectorOf<int> modes(3); modes[0]=modes[1]=modes[2]=VOCAB_CM_VELOCITY; itorsoMode->setControlModes(modes.getFirst()); error = target-torsoInitialJoints; integral = integral + (error * DT); derivative = (error - preError) / DT; preError = error; torsoVelocityCommand = kp * error + KI * integral + KD * derivative; time = Time::now(); while (norm(error)>0.2){ if(Time::now()-time>maxTorsoTrajTime){ cout<<"Max time reached."<<endl; itorsoVelocity->stop(); return true; } itorsoVelocity->velocityMove(torsoVelocityCommand.data()); if (!iTorsoEncoder->getEncoders(torsoActualJoints.data())){ cout<<"Error in reading encoders."<<endl; itorsoVelocity->stop(); return false; } error = target-torsoActualJoints; integral = integral + (error * DT); derivative = (error - preError) / DT; preError = error; torsoVelocityCommand = kp * error + KI * integral + KD * derivative; Time::delay(DT); } itorsoVelocity->stop(); return true; }