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