void ControlQueue::switchMode(int mode) {
if(ros::ok) {
if(!isShutUp()) {
ROS_INFO("(ControlQueue) switching control mode");
}
setCurrentControlTypeInternal(mode);
currentControlType = mode;
} else {
if(!isShutUp())
ROS_INFO("(ControlQueue) ros error");
}
}
void KukieControlQueue::setStiffness(float cpstiffnessxyz, float cpstiffnessabc, float cpdamping, float cpmaxdelta, float maxforce, float axismaxdeltatrq) {
if(isRealRobot) {
iis_robot_dep::CartesianImpedance imp;
imp.stiffness.linear.x = imp.stiffness.linear.y = imp.stiffness.linear.z = cpstiffnessxyz;
imp.damping.linear.x = imp.damping.linear.y = imp.damping.linear.z = cpdamping;
imp.stiffness.angular.x = imp.stiffness.angular.y = imp.stiffness.angular.z = cpstiffnessabc;
imp.damping.angular.x = imp.damping.angular.y = imp.damping.angular.z = cpdamping;
imp.cpmaxdelta = cpmaxdelta;
imp.axismaxdeltatrq = axismaxdeltatrq;
iis_robot_dep::FriJointImpedance newImpedance;
for (int j = 0; j < 7; j++){
newImpedance.stiffness[j] = cpstiffnessxyz;
newImpedance.damping[j] = cpdamping;
}
pub_set_cart_stiffness.publish(imp);
pub_set_joint_stiffness.publish(newImpedance);
ros::spinOnce();
} else {
if(!isShutUp())
ROS_INFO("(setStiffness) this functionality is not available in simulator - ignored");
}
}
void KukieControlQueue::setAdditionalLoad(float loadMass, float loadPos) {
if(isRealRobot) {
std_msgs::Float32MultiArray msg;
msg.layout.dim.push_back(std_msgs::MultiArrayDimension());
msg.layout.dim[0].size = 2;
msg.layout.dim[0].stride = 0;
msg.layout.dim[0].label = "Load";
msg.data.push_back(loadMass);
msg.data.push_back(loadPos);
pubAddLoad.publish(msg);
int tmpMode = getCurrentControlType();
stopCurrentMode();
switchMode(tmpMode);
} else {
if(!isShutUp())
ROS_INFO("(setAdditionalLoad) this functionality is not available in simulator - ignored");
}
}
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;
}