void ControlQueue::rollBack(double time) {
rollbackMode = false;
int rollBackCount = (int) (time / getCycleTime());
int stretchFactor = ceil((double) rollBackCount / (double) rollBackQueue.size());
stretchFactor = max((double) stretchFactor, 1.0);
vec lastCommand(getMovementDegreesOfFreedom());
if(rollBackQueue.size())
lastCommand = rollBackQueue.front();
int newRollBackCount = ceil((double) rollBackCount / (double) stretchFactor);
// fill command queue with last commands (backwards)
for(int i = 0; i < newRollBackCount && rollBackQueue.size(); ++i) {
vec nextCommand = rollBackQueue.front();
// interpolate to stretch the trajectory in case there are not enough measured packets (happens in usage with simulator)
vec diffUnit = (nextCommand - lastCommand) / (double) stretchFactor;
for(int j = 0; j < stretchFactor; ++j) {
move(lastCommand + j * diffUnit);
}
lastCommand = nextCommand;
rollBackQueue.pop_front();
}
// wait until everything has been executed
synchronizeToQueue(1);
rollBackQueue.clear();
}
void KukieControlQueue::submitNextJointMove(arma::vec joints) {
std_msgs::Float64MultiArray nextCommand;
for(int i = 0; i < getMovementDegreesOfFreedom(); ++i)
nextCommand.data.push_back(joints[i]);
pubCommand.publish(nextCommand);
}
void KukieControlQueue::jntFrcTrqCallback(const std_msgs::Float64MultiArray& msg) {
if(isRealRobot)
currentJntFrqTrq = stdToArmadilloVec(msg.data);
else {
currentJntFrqTrq = vec(getMovementDegreesOfFreedom());
currentJntFrqTrq.fill(0.0);
}
}
mes_result PlottingControlQueue::getCurrentJntFrcTrq() {
mes_result ret;
vec frcTrq(getMovementDegreesOfFreedom());
frcTrq.fill(0.0);
ret.joints = frcTrq;
ret.time = getCurrentTime();
return ret;
}
void KukieControlQueue::constructQueue(double sleepTime, std::string commandTopic, std::string retPosTopic, std::string switchModeTopic, std::string retCartPosTopic,
std::string cartStiffnessTopic, std::string jntStiffnessTopic, std::string ptpTopic,
std::string commandStateTopic, std::string ptpReachedTopic, std::string addLoadTopic, std::string jntFrcTrqTopic, std::string cartFrcTrqTopic,
std::string cartPtpTopic, std::string cartPtpReachedTopic, std::string cartMoveRfQueueTopic, std::string cartMoveWfQueueTopic, std::string cartPoseRfTopic, std::string jntSetPtpThreshTopic, bool acceptCollisions, ros::NodeHandle node
) {
set_ctrlc_exit_handler();
this->acceptCollisions = acceptCollisions;
this->ptpTopic = ptpTopic;
this->addLoadTopic = addLoadTopic;
this->commandTopic = commandTopic;
this->cartPtpTopic = cartPtpTopic;
this->retJointPosTopic = retPosTopic;
this->jntFrcTrqTopic = jntFrcTrqTopic;
this->switchModeTopic = switchModeTopic;
this->retCartPosTopic = retCartPosTopic;
this->ptpReachedTopic = ptpReachedTopic;
this->cartFrcTrqTopic = cartFrcTrqTopic;
this->stiffnessTopic = cartStiffnessTopic;
this->jntStiffnessTopic = jntStiffnessTopic;
this->commandStateTopic = commandStateTopic;
this->cartPtpReachedTopic = cartPtpReachedTopic;
this->cartMoveRfQueueTopic = cartMoveRfQueueTopic;
this->cartMoveWfQueueTopic = cartMoveWfQueueTopic;
this->jntSetPtpThreshTopic = jntSetPtpThreshTopic;
loop_rate = new ros::Rate(1.0 / sleepTime);
cartesianPtpReached = 0;
setInitValues();
this->node = node;
if(sleepTime == 0.0) {
ROS_ERROR("(KukieControlQueue) the sleep time you provided is 0. note that it is required in seconds");
throw KukaduException("(KukieControlQueue) the sleep time you provided is 0. note that it is required in seconds");
}
subJntPos = node.subscribe(retPosTopic, 2, &KukieControlQueue::robotJointPosCallback, this);
subComState = node.subscribe(commandStateTopic, 2, &KukieControlQueue::commandStateCallback, this);
subPtpReached = node.subscribe(ptpReachedTopic, 2, &KukieControlQueue::ptpReachedCallback, this);
subjntFrcTrq = node.subscribe(jntFrcTrqTopic, 2, &KukieControlQueue::jntFrcTrqCallback, this);
subCartFrqTrq = node.subscribe(cartFrcTrqTopic, 2, &KukieControlQueue::cartFrcTrqCallback, this);
subCartPtpReached = node.subscribe(cartPtpReachedTopic, 2, &KukieControlQueue::cartPtpReachedCallback, this);
subCartPoseRf = node.subscribe(cartPoseRfTopic, 2, &KukieControlQueue::cartPosRfCallback, this);
pub_set_cart_stiffness = node.advertise<iis_robot_dep::CartesianImpedance>(stiffnessTopic, 1);
pub_set_joint_stiffness = node.advertise<iis_robot_dep::FriJointImpedance>(jntStiffnessTopic, 1);
pubCartPtp = node.advertise<geometry_msgs::Pose>(cartPtpTopic, 1);
pubCartMoveRfQueue = node.advertise<geometry_msgs::Pose>(cartMoveRfQueueTopic, 1);
pubCartMoveWfQueue = node.advertise<geometry_msgs::Pose>(cartMoveWfQueueTopic, 1);
pub_set_ptp_thresh = node.advertise<std_msgs::Float64>(jntSetPtpThreshTopic, 1);
pubCommand = node.advertise<std_msgs::Float64MultiArray>(commandTopic, 10);
pubSwitchMode = node.advertise<std_msgs::Int32>(switchModeTopic, 1);
pubPtp = node.advertise<std_msgs::Float64MultiArray>(ptpTopic, 10);
isRealRobot = (getRobotDeviceType().compare("real")) ? false : true;
// this is required because shared_from_this can't be called in constructor (initializiation happens by lazy loading)
plannerInitialized = false;
kinematicsInitialized = false;
ros::Rate r(10);
while(!firstJointsReceived || !firstModeReceived)
r.sleep();
currentControlType = impMode;
currentCartFrqTrq = vec(6);
currentCartFrqTrq.fill(0.0);
currentJntFrqTrq = vec(getMovementDegreesOfFreedom());
currentJntFrqTrq.fill(0.0);
usleep(1e6);
}
