bool threadInit()
{
//initialize here variables
printf("ControlThread:starting\n");
Property options;
options.put("device", "remote_controlboard");
options.put("local", "/local/head");
//substitute icubSim with icub for use with the real robot
options.put("remote", "/icubSim/head");
dd.open(options);
dd.view(iencs);
dd.view(ivel);
if ( (!iencs) || (!ivel) )
return false;
int joints;
iencs->getAxes(&joints);
encoders.resize(joints);
commands.resize(joints);
commands=10000;
ivel->setRefAccelerations(commands.data());
count=0;
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;
}
