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