void workerPoolUninit() {
assert(M != NULL);
int i;
for (i=0; i < M->threadNum; i++) {
threadRelease(M->threads[i]);
}
zfree(M->threads);
contextQueueRelease(M->queue);
pthread_mutex_destroy(&M->lock);
pthread_cond_destroy(&M->cond);
zfree(M);
}
bool cartControlReachAvoidThread::threadInit()
{
ts.update();
inportTargetCoordinates.open(("/"+name+"/reachingTarget:i").c_str());
inportAvoidanceVectors.open(("/"+name+"/avoidanceVectors:i").c_str());
inportReachingGain.open(("/"+name+"/reachingGain:i").c_str());
inportAvoidanceGain.open(("/"+name+"/avoidanceGain:i").c_str());
//getting values from config file
// general part
Bottle &bGeneral=rf.findGroup("general");
useLeftArm=bGeneral.check("left_arm",Value("on"),"Getting left arm use flag").asString()=="on"?true:false;
useRightArm=bGeneral.check("right_arm",Value("on"),"Getting right arm use flag").asString()=="on"?true:false;
trajTime=bGeneral.check("traj_time",Value(3.0),"Getting trajectory time").asDouble();
reachTol=bGeneral.check("reach_tol",Value(0.05),"Getting reaching tolerance").asDouble();
reachTmo=bGeneral.check("reach_tmo",Value(10.0),"Getting reach timeout").asDouble();
// torso part
torsoSwitch.resize(3,0);
torsoLimits.resize(4,3); torsoLimits.zero();
torsoHomePoss.resize(3,0.0);
torsoHomeVels.resize(3,10.0);
Bottle &bTorso=rf.findGroup("torso");
if(!bTorso.isNull()){
yInfo("Setting torso options from config file:");
bTorso.setMonitor(rf.getMonitor());
getTorsoOptions(bTorso,"pitch",0,torsoSwitch,torsoLimits);
getTorsoOptions(bTorso,"roll",1,torsoSwitch,torsoLimits);
getTorsoOptions(bTorso,"yaw",2,torsoSwitch,torsoLimits);
getTorsoHomeOptions(bTorso,torsoHomePoss,torsoHomeVels);
}
else{
yInfo("Setting default torso options ([torso] in .ini file not found)");
torsoSwitch(0)=0; torsoSwitch(1)=0; torsoSwitch(2)=0; //all off
//torsoSwitch(0)=1; torsoSwitch(1)=0; torsoSwitch(2)=1; //pitch on, roll off, yaw on
//torsoLimits(0,2)=1.0;
//torsoLimits(1,0)=10.0; //pitch max limit
//torsoHomePoss and torsoHomeVels was already intialized fine during definition
}
yInfo("torsoSwitch: %s",torsoSwitch.toString().c_str());
yInfo("torsoLimits: %s",torsoLimits.toString().c_str());
yInfo("torsoHomePoss: %s",torsoHomePoss.toString().c_str());
yInfo("torsoHomeVels: %s",torsoHomeVels.toString().c_str());
// arm home part
armHomePoss.resize(7,0.0); armHomeVels.resize(7,0.0);
Bottle &bHome=rf.findGroup("home_arm");
if(!bHome.isNull()){
bHome.setMonitor(rf.getMonitor());
getArmHomeOptions(bHome,armHomePoss,armHomeVels);
yInfo("Setting arm home from config file:");
}
else{
yInfo("Setting default arm home values (home_arm in .ini file not found)");
armHomePoss[0]=-30.0; armHomePoss[1]=30.0; armHomePoss[2]=0.0; armHomePoss[3]=45.0; armHomePoss[4]=0.0; armHomePoss[5]=0.0; armHomePoss[6]=0.0;
armHomeVels[0]=10.0; armHomeVels[1]=10.0; armHomeVels[2]=10.0; armHomeVels[3]=10.0; armHomeVels[4]=10.0; armHomeVels[5]=10.0; armHomeVels[6]=10.0;
}
yInfo("armHomePoss: %s",armHomePoss.toString().c_str());
yInfo("armHomeVels: %s",armHomeVels.toString().c_str());
newTargetFromPort = false;
newTargetFromRPC = false;
targetPosFromPort.resize(3,0.0);
targetPosFromRPC.resize(3,0.0);
targetPos.resize(3,0.0);
//TODO add to config file
reachableSpace.minX=-0.4; reachableSpace.maxX=-0.2;
reachableSpace.minY=-0.3; reachableSpace.maxY=0.3;
reachableSpace.minZ=-0.1; reachableSpace.maxZ=0.1;
string fwslash="/";
// open remote_controlboard drivers
Property optTorso("(device remote_controlboard)");
Property optLeftArm("(device remote_controlboard)");
Property optRightArm("(device remote_controlboard)");
optTorso.put("remote",(fwslash+robot+"/torso").c_str());
optTorso.put("local",(fwslash+name+"/torso").c_str());
optLeftArm.put("remote",(fwslash+robot+"/left_arm").c_str());
optLeftArm.put("local",(fwslash+name+"/left_arm").c_str());
optRightArm.put("remote",(fwslash+robot+"/right_arm").c_str());
optRightArm.put("local",(fwslash+name+"/right_arm").c_str());
drvTorso=new PolyDriver;
if (!drvTorso->open(optTorso))
{
threadRelease();
return false;
}
if (useLeftArm)
{
drvLeftArm=new PolyDriver;
if (!drvLeftArm->open(optLeftArm))
{
threadRelease();
return false;
}
}
if (useRightArm)
{
drvRightArm=new PolyDriver;
if (!drvRightArm->open(optRightArm))
{
threadRelease();
return false;
}
}
// open cartesiancontrollerclient drivers
Property optCartLeftArm("(device cartesiancontrollerclient)");
Property optCartRightArm("(device cartesiancontrollerclient)");
optCartLeftArm.put("remote",(fwslash+robot+"/cartesianController/left_arm").c_str());
optCartLeftArm.put("local",(fwslash+name+"/left_arm/cartesian").c_str());
optCartRightArm.put("remote",(fwslash+robot+"/cartesianController/right_arm").c_str());
optCartRightArm.put("local",(fwslash+name+"/right_arm/cartesian").c_str());
if (useLeftArm)
{
drvCartLeftArm=new PolyDriver;
if (!drvCartLeftArm->open(optCartLeftArm))
{
threadRelease();
return false;
}
}
if (useRightArm)
{
drvCartRightArm=new PolyDriver;
if (!drvCartRightArm->open(optCartRightArm))
{
threadRelease();
return false;
}
}
// open views
drvTorso->view(encTorso);
drvTorso->view(posTorso);
drvTorso->view(velTorso);
drvTorso->view(modTorso);
if (useLeftArm)
{
drvLeftArm->view(encArm);
drvLeftArm->view(posArm);
drvLeftArm->view(velArm);
drvLeftArm->view(modArm);
drvCartLeftArm->view(cartArm);
armSel=LEFTARM;
}
else if (useRightArm)
{
drvRightArm->view(encArm);
drvRightArm->view(posArm);
drvRightArm->view(velArm);
drvRightArm->view(modArm);
drvCartRightArm->view(cartArm);
armSel=RIGHTARM;
}
else
{
encArm=NULL;
posArm=NULL;
velArm=NULL;
cartArm=NULL;
armSel=NOARM;
}
encArm->getAxes(&armAxes);
arm.resize(armAxes,0.0);
encTorso->getAxes(&torsoAxes);
torso.resize(torsoAxes,0.0);
/* set reference speeds for position controllers (used for homing) */
posTorso->setRefSpeeds(torsoHomeVels.data());
IPositionControl *posArm_;
drvLeftArm->view(posArm_);
posArm_->setRefSpeeds(armHomeVels.data());
drvRightArm->view(posArm_);
posArm_->setRefSpeeds(armHomeVels.data());
/* we need to set reference accelerations for the velocityMove to be used later */
/* profile, 50 degrees/sec^2 */
int k;
Vector tmp_acc_arm;
tmp_acc_arm.resize(armAxes,0.0);
for (k = 0; k < armAxes; k++) {
tmp_acc_arm[k] = REF_ACC;
}
IVelocityControl2 *velArm_;
drvLeftArm->view(velArm_);
velArm_->setRefAccelerations(tmp_acc_arm.data());
drvRightArm->view(velArm_);
velArm_->setRefAccelerations(tmp_acc_arm.data());
for(int j=0;j<7;j++){
armIdx.push_back(j);
}
int l;
Vector tmp_acc_torso;
tmp_acc_torso.resize(torsoAxes,0.0);
for (l = 0; l < torsoAxes; l++) {
tmp_acc_torso[l] = REF_ACC;
}
velTorso->setRefAccelerations(tmp_acc_torso.data());
targetPos.resize(3,0.0);
R=Rx=Ry=Rz=eye(3,3);
initCartesianCtrl(torsoSwitch,torsoLimits,LEFTARM);
initCartesianCtrl(torsoSwitch,torsoLimits,RIGHTARM);
cartArm->getDOF(cartDOFconfig);
cartNrDOF = cartDOFconfig.length();
yInfo("cartArm DOFs [%s]\n",cartDOFconfig.toString().c_str()); // [0 0 0 1 1 1 1 1 1 1] will be printed out if torso is off
// steer the robot to the initial configuration
setControlModeArmsAndTorso(VOCAB_CM_POSITION);
steerTorsoToHome();
steerArmToHome(LEFTARM);
steerArmToHome(RIGHTARM);
wentHome=false;
state=STATE_IDLE;
minJerkVelCtrl = new minJerkVelCtrlForIdealPlant(threadPeriod,cartNrDOF); //3 torso + 7 arm
//TODO can be moved to config file, but will be eventually obtained from allostatic control
reachingGain = 1.0;
avoidanceGain = 0.0;
return true;
}
void eventloopUninit() {
threadRelease(M->thread);
aeDeleteEventLoop(M->eventloop);
zfree(M);
}
