void KukieControlQueue::jointPtpInternal(arma::vec joints) { ptpReached = false; komoMutex.lock(); if(!plannerInitialized) { planner = KUKADU_SHARED_PTR<PathPlanner>(new KomoPlanner(shared_from_this(), resolvePath("$KUKADU_HOME/external/komo/share/data/kuka/data/iis_robot.kvg"), resolvePath("$KUKADU_HOME/external/komo/share/data/kuka/config/MT.cfg"), getRobotSidePrefix(), acceptCollisions)); plannerInitialized = true; } bool performPtp = false; vec currentState = getCurrentJoints().joints; for(int i = 0; i < joints.n_elem; ++i) if(abs(currentState(i) - joints(i)) > 0.01) { performPtp = true; break; } vector<vec> desiredJointPlan; if(performPtp) { vector<arma::vec> desiredPlan; desiredPlan.push_back(getCurrentJoints().joints); desiredPlan.push_back(joints); desiredJointPlan = planner->planJointTrajectory(desiredPlan); } komoMutex.unlock(); if(performPtp) { if(desiredJointPlan.size() > 0) { for(int i = 0; i < desiredJointPlan.size(); ++i) addJointsPosToQueue(desiredJointPlan.at(i)); synchronizeToControlQueue(1); } else { ROS_ERROR("(KukieControlQueue) Joint plan not reachable"); } } }
void ControlQueue::jointsCollector() { double lastTime = DBL_MIN; collectedJoints.clear(); ros::Rate r((int) (1.0 / getCycleTime() + 10.0)); while(continueCollecting) { mes_result lastResult = getCurrentJoints(); if(lastResult.time != lastTime) collectedJoints.push_back(lastResult); r.sleep(); } }
void KukieControlQueue::computeCurrentCartPose() { if(!kinematicsInitialized) { kin = KUKADU_SHARED_PTR<Kinematics>(new KomoPlanner(shared_from_this(), resolvePath("$KUKADU_HOME/external/komo/share/data/kuka/data/iis_robot.kvg"), resolvePath("$KUKADU_HOME/external/komo/share/data/kuka/config/MT.cfg"), getRobotSidePrefix(), acceptCollisions)); kinematicsInitialized = true; } while(getQueueRunning()) { forwadKinMutex.lock(); currCarts = kin->computeFk(armadilloToStdVec(getCurrentJoints().joints)); forwadKinMutex.unlock(); } }
void ControlQueue::run() { setInitValuesInternal(); ros::Rate sleepRate(1.0 / sleepTime); arma::vec movement = arma::vec(1); geometry_msgs::Pose movementPose; if(getStartingJoints().n_elem > 1) { if(!isShutUp()) ROS_INFO("(ControlQueue) start moving to start position"); jointPtp(getStartingJoints()); if(!isShutUp()) ROS_INFO("(ControlQueue) finished moving to start position"); } isInit = true; lastDuration = 0.0; double toleratedMaxDuration = 1.1 * getCycleTime(); double toleratedMinDuration = 0.9 * getCycleTime(); movement = getCurrentJoints().joints; t.tic("r"); while(!finish && ros::ok) { t.tic("d"); /* // switched off adaptive cycle time behaviour for now - probably requires a PID controller for that if(toleratedMinDuration < lastDuration || lastDuration > toleratedMaxDuration) { sleepTime = max(0.000000001, sleepTime + 0.3 * (desiredCycleTime - lastDuration)); sleepRate = ros::Rate(1.0 / sleepTime); } */ currentTime = t.toc("r"); currentJoints = getCurrentJoints().joints; currentCartPose = getCurrentCartesianPose(); if(rollbackMode) { rollBackQueue.push_front(currentJoints); // if queue is full --> go back while(rollBackQueue.size() > rollBackQueueSize) rollBackQueue.pop_back(); } if(!stopQueueWhilePtp() || !jointPtpRunning && !cartesianPtpRunning) { if(currentControlType == CONTROLQUEUE_JNT_IMP_MODE || currentControlType == CONTROLQUEUE_JNT_POS_MODE) { if(movementQueue.size() > 0) { // move to position in queue movement = movementQueue.front(); movementQueue.pop(); } else { if(stopQueueWhilePtp()) movement = getCurrentJoints().joints; } submitNextJointMove(movement); } else if(currentControlType == CONTROLQUEUE_CART_IMP_MODE) { if(cartesianMovementQueue.size() > 0) { // move to position in queue movementPose = cartesianMovementQueue.front(); cartesianMovementQueue.pop(); } else { movementPose = getCurrentCartesianPose(); } submitNextCartMove(movementPose); } } sleepRate.sleep(); ros::spinOnce(); lastDuration = t.toc("d"); } if(!isShutUp()) cout << "thread finished" << endl; }