void KukieControlQueue::cartPtpInternal(geometry_msgs::Pose pos, double maxForce) { cartesianPtpReached = 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; } vector<geometry_msgs::Pose> desiredPlan; desiredPlan.push_back(getCurrentCartesianPose()); desiredPlan.push_back(pos); vector<vec> desiredJointPlan = planner->planCartesianTrajectory(desiredPlan, false, true); komoMutex.unlock(); bool maxForceExceeded = false; if(desiredJointPlan.size() > 0) { for(int i = 0; i < desiredJointPlan.size() && !maxForceExceeded; ++i) { if(getAbsoluteCartForce() > maxForce) maxForceExceeded = true; else addJointsPosToQueue(desiredJointPlan.at(i)); synchronizeToControlQueue(1); } } else { ROS_ERROR("(KukieControlQueue) Cartesian position not reachable"); } }
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; }