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