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