void cableLengthGuard::run()
{
//ACE_OS::printf("Entering the main thread\n");
for (int i = 0; i < numberOfJoints; i++)
enc->getEncoder(thetaMap[i], theta+i);
Vector thetaDes(numberOfJoints);
//get the last commanded position
//if non commanded position is available
//get current position
for(int i = 0; i < numberOfJoints; i++)
{
if (!lastCommandPos->get(thetaDes(i), thetaMap[i]))
thetaDes(i) = theta[i];
}
Vector ThetaCqp(4);
ThetaCqp(0) = thetaDes(0);
ThetaCqp(1) = thetaDes(1);
ThetaCqp(2) = thetaDes(2);
ThetaCqp(3) = 0; //fake variable for CQP
Vector jointLimits(11);
jointLimits = C*ThetaCqp - d;
//fprintf(stderr, "jointLim is %s\n", jointLimits.toString().c_str());
bool stopMovement = false;
for(int i = 0; i < 5; i++)
if (jointLimits(i) - CABLE_WARNING < 0)
{
fprintf(stderr, "%d ", i);
stopMovement = true;
}
if(stopMovement)
{
fprintf(stderr, "Stopping movement in the current configuration \n");
//for (int i = 0; i < numberOfJoints; i++)
// pos->positionMove(thetaMap[i], theta[i]);
Time::delay(0.1);
lastCommandPos->reset();
}
}
static void jointLimits_proxy(const ModelHandler & model,
DataHandler & data,
const VectorXd_fx & q)
{
jointLimits(*model,*data,q);
}
