// todo: Using QTs signal/slot mechanism to send the position data to the main thread is
// inefficient. We might need to roll our own here!
void CTrackerMainWindow::onReceive(const FOBData &TrackerPos) {
const double PI = 3.1415926535897932384626433832795;
if (!_muted) {
ui.eTime->setText(QString::number(TrackerPos.time));
ui.ePosX->setText(QString::number(TrackerPos.pos.v[0]));
ui.ePosY->setText(QString::number(TrackerPos.pos.v[1]));
ui.ePosZ->setText(QString::number(TrackerPos.pos.v[2]));
double x = atan(TrackerPos.rot.m[1][2] / TrackerPos.rot.m[2][2]);
double y = asin(-TrackerPos.rot.m[0][2]);
double z = atan(TrackerPos.rot.m[0][1] / TrackerPos.rot.m[0][0]);
ui.eAng1->setText(QString::number(x/PI * 180.0));
ui.eAng2->setText(QString::number(y/PI * 180.0));
ui.eAng3->setText(QString::number(z/PI * 180.0));
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
ui.tRotation->item(i, j)->setText(QString::number(TrackerPos.rot.m[i][j]));
_RotationPreview->setRotation(TrackerPos.rot);
}
sendPose(TrackerPos.pos, TrackerPos.rot);
}
void CTrackerMainWindow::on_btnSend_clicked() {
Vector3 pos;
Matrix3 rot;
pos.v[0] = ui.ePosX->text().toDouble();
pos.v[1] = ui.ePosY->text().toDouble();
pos.v[2] = ui.ePosZ->text().toDouble();
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
rot.m[i][j] = ui.tRotation->item(i, j)->text().toDouble();
sendPose(pos, rot);
}
// This will block until the desired force is achieved!
void sendAndWaitForNormalForce(double fz) {
if(bWaitForForces) {
ROS_INFO_STREAM("Waiting for force "<<fz<<" N.");
sendPose(ee_pose);
ros::Rate wait(500);
while(ros::ok()) {
sendNormalForce(fz);
ROS_INFO_STREAM("Sending Normal force: " << fz << " Fz diff: " << fabs(ee_ft[2]-fz));
if(fabs(ee_ft[2]-fz) < FORCE_WAIT_TOL) {
break;
}
ros::spinOnce();
wait.sleep();
}
}
}
// Go to this pose
bool go_home(tf::Pose& pose_) {
tf::Vector3 start_p(pose_.getOrigin());
tf::Quaternion start_o(pose_.getRotation());
msg_pose.pose.position.x = start_p.x();
msg_pose.pose.position.y = start_p.y();
msg_pose.pose.position.z = start_p.z();
msg_pose.pose.orientation.w = start_o.w();
msg_pose.pose.orientation.x = start_o.x();
msg_pose.pose.orientation.y = start_o.y();
msg_pose.pose.orientation.z = start_o.z();
pub_.publish(msg_pose);
sendNormalForce(0);
ros::Rate thread_rate(2);
ROS_INFO("Homing...");
while(ros::ok()) {
double oerr =(ee_pose.getRotation() - start_o).length() ;
double perr = (ee_pose.getOrigin() - start_p).length();
feedback_.progress = 0.5*(perr+oerr);
as_.publishFeedback(feedback_);
ROS_INFO_STREAM("Error: "<<perr<<", "<<oerr);
if(perr< 0.02 && oerr < 0.02) {
break;
}
if (as_.isPreemptRequested() || !ros::ok() )
{
sendNormalForce(0);
sendPose(ee_pose);
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
return false;
}
thread_rate.sleep();
}
return ros::ok();
}
void MainWindow::createActions()
{
connect(m_ui->actionAbout, SIGNAL(triggered()), this, SLOT(showAbout()));
connect(m_ui->actionDocumentation, SIGNAL(triggered()), this, SLOT(showDocumentation()));
connect(m_ui->actionExport, SIGNAL(triggered()), this, SLOT(exportToKME()));
connect(m_ui->actionGraph, SIGNAL(triggered()), this, SLOT(viewGraph()));
connect(m_ui->actionJoints, SIGNAL(triggered()), this, SLOT(viewJoints()));
connect(m_ui->actionNao_Academics, SIGNAL(triggered()), this, SLOT(setNaoAcademics()));
connect(m_ui->actionNao_RC_Edition, SIGNAL(triggered()), this, SLOT(setNaoRC()));
connect(m_ui->actionNew, SIGNAL(triggered()), this, SLOT(newFile()));
connect(m_ui->actionOpen, SIGNAL(triggered()), this, SLOT(openFile()));
connect(m_ui->actionOpen_Robot, SIGNAL(triggered()), this, SLOT(loadNewRobot()));
connect(m_ui->actionQuit, SIGNAL(triggered()), this, SLOT(close()));
connect(dlg, SIGNAL(slidersClosed(bool)),m_ui->actionJoints, SLOT(setChecked(bool)));
connect(m_ui->poseEditor, SIGNAL(cellChanged(int, int)), this, SLOT(somethingChanged()));
//Pose Editor
connect(m_ui->moveUpPoseButton, SIGNAL(clicked()), this, SLOT(moveUpAction()));
connect(m_ui->moveDownPoseButton, SIGNAL(clicked()), this, SLOT(moveDownAction()));
connect(m_ui->swapPoseButton, SIGNAL(clicked()), this, SLOT(swapAction()));
connect(m_ui->removePoseButton, SIGNAL(clicked()), this, SLOT(removeAction()));
connect(m_ui->storePoseButton, SIGNAL(clicked()), this, SLOT(storePoseAction()));
connect(m_ui->insertPoseButton, SIGNAL(clicked()), this, SLOT(insertPoseAction()));
//Play Motion
connect(m_ui->gotoPoseButton, SIGNAL(clicked()), this, SLOT(gotoPoseAction()));
connect(m_ui->stepPoseButton, SIGNAL(clicked()), this, SLOT(stepPoseAction()));
connect(m_ui->playMotionButton, SIGNAL(clicked()), this, SLOT(playMotionAction()));
connect(m_ui->stiffOn, SIGNAL(clicked()), this, SLOT(stiffOnAction()));
connect(m_ui->stiffOff, SIGNAL(clicked()), this, SLOT(stiffOffAction()));
//Networking
connect(m_ui->connectButton, SIGNAL(clicked()), this, SLOT(connectAction()));
//Sliders
connect(dlg->jointsUI, SIGNAL(poseChanged()), this, SLOT(sendPose()));
}
// Do stuff with learned models
// Phase - Reach, Roll or Back?
// Dynamics type - to select the type of master/slave dynamics (linear/model etc.)
// reachingThreshold - you know
// model_dt - you know
bool learned_model_execution(DoughTaskPhase phase, CDSController::DynamicsType masterType, CDSController::DynamicsType slaveType, double reachingThreshold, double model_dt, tf::Transform trans_obj, tf::Transform trans_att, std::string model_base_path, std::string force_gmm_id) {
ROS_INFO_STREAM(" Model Path "<<model_base_path);
ROS_INFO_STREAM(" Learned model execution with phase "<<phase);
ROS_INFO_STREAM(" Reaching threshold "<<reachingThreshold);
ROS_INFO_STREAM(" Model DT "<<model_dt);
if (force_gmm_id!="")
ROS_INFO_STREAM(" Force GMM ID: "<< force_gmm_id);
ros::Rate wait(1);
tf::Transform trans_final_target, ee_pos_att;
// To Visualize EE Frames
static tf::TransformBroadcaster br;
br.sendTransform(tf::StampedTransform(trans_final_target, ros::Time::now(), world_frame, "/attractor"));
br.sendTransform(tf::StampedTransform(trans_obj, ros::Time::now(), world_frame, "/object_frame"));
// convert attractor information to world frame
trans_final_target.mult(trans_obj, trans_att);
ROS_INFO_STREAM("Final target origin "<<trans_final_target.getOrigin().getX()<<","<<trans_final_target.getOrigin().getY()<<","<<trans_final_target.getOrigin().getZ());
ROS_INFO_STREAM("Final target orient "<<trans_final_target.getRotation().getX()<<","<<trans_final_target.getRotation().getY()<<","<<trans_final_target.getRotation().getZ()<<","<<trans_final_target.getRotation().getW());
// Publish attractors if running in simulation or with fixed values
if ((action_mode == ACTION_LASA_FIXED) || (action_mode == ACTION_BOXY_FIXED)) {
br.sendTransform(tf::StampedTransform(trans_final_target, ros::Time::now(), world_frame, "/attractor"));
}
// Initialize CDS
CDSExecution *cdsRun = new CDSExecution;
cdsRun->initSimple(model_base_path, phase, force_gmm_id);
cdsRun->setObjectFrame(toMatrix4(trans_obj));
cdsRun->setAttractorFrame(toMatrix4(trans_att));
cdsRun->setCurrentEEPose(toMatrix4(ee_pose));
cdsRun->setDT(model_dt);
if (phase==PHASEBACK || phase==PHASEROLL)
masterType = CDSController::LINEAR_DYNAMICS;
// Roll slow but move fast for reaching and back phases.
// If models have proper speed, this whole block can go!
if(phase == PHASEROLL) {
//cdsRun->setMotionParameters(1,1,0.5,reachingThreshold, masterType, slaveType);
cdsRun->setMotionParameters(1,1,2,reachingThreshold, masterType, slaveType);
// large threshold to avoid blocking forever
// TODO: should rely on preempt in action client.
// reachingThreshold = 0.02;
reachingThreshold = 0.025;
} else {
cdsRun->setMotionParameters(1,1,2,reachingThreshold, masterType, slaveType);
}
cdsRun->postInit();
// If phase is rolling, need force model as well
GMR* gmr_perr_force = NULL;
if(phase == PHASEROLL) {
gmr_perr_force = getNewGMRMappingModel(model_base_path, force_gmm_id);
if(!gmr_perr_force) {
ROS_ERROR("Cannot initialize GMR model");
return false;
}
}
ros::Duration loop_rate(model_dt);
tf::Pose mNextRobotEEPose = ee_pose;
std::vector<double> gmr_in, gmr_out;
gmr_in.resize(1);gmr_out.resize(1);
double prog_curr, full_err, pos_err, ori_err, new_err, gmr_err;
tf::Vector3 att_t, curr_t;
ROS_INFO("Execution started");
tf::Pose p;
bool bfirst = true;
std_msgs::String string_msg, action_name_msg, model_fname_msg;
std::stringstream ss, ss_model;
if(phase == PHASEREACH) {
ss << "reach ";
}
else if (phase == PHASEROLL){
ss << "roll";
}
else if (phase == PHASEBACK){
ss << "back";
}
if (!homing){
string_msg.data = ss.str();
pub_action_state_.publish(string_msg);
// ss_model << path_matlab_plot << "/Phase" << phase << "/masterGMM.fig";
if (force_gmm_id=="")
ss_model << path_matlab_plot << "/Phase" << phase << "/masterGMM.fig";
else
ss_model << path_matlab_plot << "/Phase" << phase << "/ForceProfile_" << force_gmm_id << ".fig";
//ss_model << "/Phase" << phase << "/masterGMM.fig";
model_fname_msg.data = ss_model.str();
pub_model_fname_.publish(model_fname_msg);
action_name_msg.data = ss.str();
pub_action_name_.publish(action_name_msg);
plot_dyn = 1;
plot_dyn_msg.data = plot_dyn;
pub_plot_dyn_.publish(plot_dyn_msg);
}
while(ros::ok()) {
if (!homing)
plot_dyn = 1;
else
plot_dyn = 0;
plot_dyn_msg.data = plot_dyn;
pub_plot_dyn_.publish(plot_dyn_msg);
// Nadia's stuff
// Current progress variable (position/orientation error).
// TODO: send this back to action client as current progress
pos_err = (trans_final_target.getOrigin() - ee_pose.getOrigin()).length();
//Real Orientation Error qdiff = acos(dot(q1_norm,q2_norm))*180/pi
ori_err = acos(abs(trans_final_target.getRotation().dot(ee_pose.getRotation())));
ROS_INFO_STREAM_THROTTLE(0.5,"Position Threshold : " << reachingThreshold << " ... Current Error: "<<pos_err);
ROS_INFO_STREAM_THROTTLE(0.5,"Orientation Threshold : " << 0.01 << " ... Current Error: "<<ori_err);
/*double att_pos_err = (trans_final_target.getOrigin() - p.getOrigin()).length();
double att_ori_err = acos(abs(trans_final_target.getRotation().dot(p.getRotation())));
ROS_INFO_STREAM_THROTTLE(0.5,"Des-Att Position Error: " << att_pos_err);
ROS_INFO_STREAM_THROTTLE(0.5,"Des-Att Orientation Error: " << att_ori_err);*/
switch (phase) {
// Home, reach and back are the same control-wise
case PHASEREACH:
case PHASEBACK:
// Current progress variable (position/orientation error).
// TODO: send this back to action client as current progress
prog_curr = 0.5*((trans_final_target.getOrigin() - ee_pose.getOrigin()).length() + (trans_final_target.getRotation()-ee_pose.getRotation()).length());
// Compute Next Desired EE Pose
cdsRun->setCurrentEEPose(toMatrix4(mNextRobotEEPose));
toPose(cdsRun->getNextEEPose(), mNextRobotEEPose);
p = mNextRobotEEPose;
// Aswhini's Hack! Dont rely on model's orientation interpolation. Set it equal to target orientation to avoid
// going the wrong way around
p.setRotation(trans_final_target.getRotation());
//Publish desired force
gmr_msg.data = 0.0;
pub_gmr_out_.publish(gmr_msg);
break;
case PHASEROLL:
// Current progress in rolling phase is simply the position error
prog_curr = (trans_final_target.getOrigin() - ee_pose.getOrigin()).length();
// New position error being fed to GMR Force Model
att_t = tf::Vector3(trans_final_target.getOrigin().getX(),trans_final_target.getOrigin().getY(),0.0);
curr_t = tf::Vector3(ee_pose.getOrigin().getX(),ee_pose.getOrigin().getY(),0.0);
new_err = (att_t - curr_t).length();
gmr_err = new_err;
//Hack! Truncate errors to corresponding models
if ((strncmp(force_gmm_id.c_str(),"first",5)==0) && (new_err > 0.03)){
gmr_err = 0.03;
}
if ((strncmp(force_gmm_id.c_str(),"mid",3)==0) && (new_err > 0.07)){
gmr_err = 0.07;
}
if ((strncmp(force_gmm_id.c_str(),"last",4)==0) && (new_err > 0.13)){
gmr_err = 0.13;
}
//pos_err = prog_curr;
ori_err = 0;
gmr_err = gmr_err;
gmr_in[0] = gmr_err; // distance between EE and attractor
// Query the model for desired force
getGMRResult(gmr_perr_force, gmr_in, gmr_out);
/* double fz_plot;
getGMRResult(gmr_perr_force, -gmr_in, fz_plot);*/
//-> INSTEAD OF SENDING GMR OUTPUT / SEND EST_EE_FT(Z)
// Send fz and distance to attractor for plotting
msg_ft.wrench.force.x = gmr_err;
msg_ft.wrench.force.y = gmr_out[0]; //ee_ft[2]
msg_ft.wrench.force.z = 0;
msg_ft.wrench.torque.x = 0;
msg_ft.wrench.torque.y = 0;
msg_ft.wrench.torque.z = 0;
pub_ee_ft_att_.publish(msg_ft);
// Hack! Scale the force to be in reasonable values
gmr_out[0] = FORCE_SCALING*fabs(gmr_out[0]);
ROS_INFO_STREAM_THROTTLE(0.5,"Distance to Attractor: " << new_err << " GMR output (N): " << gmr_out[0]);
gmr_msg.data = gmr_out[0];
pub_gmr_out_.publish(gmr_msg);
// Hack! Safety first!
if(gmr_out[0] > MAX_ROLLING_FORCE) {
gmr_out[0] = MAX_ROLLING_FORCE;
}
// Give some time for the force to catch up the first time. Then roll with constant force thereafter.
if(bfirst) {
sendAndWaitForNormalForce(-gmr_out[0]);
bfirst = false;
} else {
sendNormalForce(-gmr_out[0]);
}
ROS_INFO_STREAM_THROTTLE(0.5, "Force applied: "<<gmr_out[0]);
cdsRun->setCurrentEEPose(toMatrix4(mNextRobotEEPose));
toPose(cdsRun->getNextEEPose(), mNextRobotEEPose);
p = mNextRobotEEPose;
// Hack! Dont rely on model orientation. Use target orientation instead.
p.setRotation(trans_final_target.getRotation());
// Hack! Dont rely on the Z component of the model. It might go below the table!
p.getOrigin().setZ(trans_final_target.getOrigin().getZ());
homing=false;
break;
default:
ROS_ERROR_STREAM("No such phase defined "<<phase);
return false;
}
// Add rotation of Tool wrt. flange_link for BOXY
/*if (use_boxy_tool){
tf::Matrix3x3 R = p.getBasis();
Eigen::Matrix3d R_ee;
tf::matrixTFToEigen(R,R_ee);
Eigen::Matrix3d R_tool;
R_tool << -0.7071, -0.7071, 0.0,
0.7071,-0.7071, 0.0,
0.0, 0.0, 1.0;
//multiply tool rot
R_ee = R_ee*R_tool;
tf::matrixEigenToTF(R_ee,R);
p.setBasis(R);
}*/
// Send the computed pose from one of the above phases
sendPose(p);
// convert and send ee pose to attractor frame to plots
ee_pos_att.mult(trans_final_target.inverse(), p);
geometry_msgs::PoseStamped msg;
msg.pose.position.x = ee_pos_att.getOrigin().x();
msg.pose.position.y = ee_pos_att.getOrigin().y();
msg.pose.position.z = ee_pos_att.getOrigin().z();
msg.pose.orientation.x = ee_pos_att.getRotation().x();
msg.pose.orientation.y = ee_pos_att.getRotation().y();
msg.pose.orientation.z = ee_pos_att.getRotation().z();
msg.pose.orientation.w = ee_pos_att.getRotation().w();
pub_ee_pos_att_.publish(msg);
//ROS_INFO_STREAM_THROTTLE(0.5,"Error "<<prog_curr);
// check that preempt has not been requested by the client
if (as_.isPreemptRequested() || !ros::ok())
{
sendPose(ee_pose);
sendNormalForce(0);
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
return false;
}
feedback_.progress = prog_curr;
as_.publishFeedback(feedback_);
/* if(prog_curr < reachingThreshold) {
break;
}*/
//Orientation error 0.05
if(pos_err < reachingThreshold && (ori_err < 0.05 || isnan(ori_err))) {
break;
}
loop_rate.sleep();
}
delete cdsRun;
if(phase == PHASEREACH) {
// Hack! If phase is "reach", find the table right after reaching
if (bWaitForForces && !homing) {
bool x = find_table_for_rolling(0.35, 0.05, 5);
// bool x = find_table_for_rolling(0.35, 0.1, 5);
//-> Send command to dynamics plotter to stop logging
return x;
}
if (!homing){
//-> Send command to dynamics plotter to stop logging
}
} else if (phase == PHASEROLL){
// Hack! wait for zero force before getting ready to recieve further commands.
// This is to avoid dragging the dough.
sendAndWaitForNormalForce(0);
//-> Send command to dynamics plotter to start plotting
return ros::ok();
} else {
//->Send command to dynamics plotter to start plotting
return ros::ok();
}
}
bool Kuka_grav_as::execute_CB(alib_server& as_,alib_feedback& feedback_,const cptrGoal& goal){
if (action_name == (goal->action_name))
{
ROS_INFO("Kuka_grav_as::execute_CB");
ROS_INFO("action_name : [%s]",goal->action_name.c_str());
ROS_INFO("action_type : [%s]",goal->action_type.c_str());
ROS_INFO("desired_stifness: [%f,%f,%f,%f,%f,%f,%f]", goal->JointStateImpedance.stiffness[0],
goal->JointStateImpedance.stiffness[1],goal->JointStateImpedance.stiffness[2],
goal->JointStateImpedance.stiffness[3],goal->JointStateImpedance.stiffness[4],
goal->JointStateImpedance.stiffness[5],goal->JointStateImpedance.stiffness[6]);
/*if (bBaseRun){
std::cout<< "bRun: TRUE " << std::endl;
}else{
std::cout<< "bRun: FALSE " << std::endl;
}*/
set_control_type(asrv::GRAV_COMP);
///--- Desired Joint Impedance Command--///
des_j_pose.resize(7);
des_j_velocity.resize(7);
des_j_stiffness.resize(7);
for(std::size_t i = 0; i < 7;++i){
if (action_type == "position")
des_j_pose(i) = goal->JointStateImpedance.position[i];
else
des_j_velocity(i) = goal->JointStateImpedance.velocity[i];
des_j_stiffness(i) = goal->JointStateImpedance.stiffness[i];
}
///--- Action Type (Control interface: Velocity or Position) ---///
action_type = goal->action_type;
///--- Desired Loop Rate Command--///
ros::Duration loop_rate(goal->dt);
bool bBaseRun = true;
while(ros::ok()&& bBaseRun) {
//--- Update Cartesian Pose for (cart_to_joint) everytime you go into interactive grav_comp
//--- Send /cart_to_joint/des_ee_pos as the current ee_pose
tf::TransformListener listener;
tf::StampedTransform curr_ee_tf_;
try{
listener.waitForTransform("/world_frame", "/curr_ee_tf", ros::Time(0), ros::Duration(10.0) );
listener.lookupTransform("/world_frame", "/curr_ee_tf", ros::Time(0), curr_ee_tf_);
} catch (tf::TransformException ex) {
ROS_ERROR("%s",ex.what());
}
ROS_INFO_STREAM("Curr_ee_tf x:" <<curr_ee_tf_.getOrigin().x() << " y:" << curr_ee_tf_.getOrigin().y() << " z:" << curr_ee_tf_.getOrigin().z() );
des_ee_pose_from_curr.setOrigin(curr_ee_tf_.getOrigin());
des_ee_pose_from_curr.setRotation(curr_ee_tf_.getRotation());
sendPose(des_ee_pose_from_curr);
///--- Send Joint Impedance Command to KUKA FRI Bridge ---///
if (action_type == "position")
setJointPos(des_j_pose);
else{
// Send Velocity command ( will be 0 for grav comp)
setJointVel(des_j_velocity);
}
sendJointImpedance(des_j_stiffness);
///--- Stop sending when the desired stiffness is reached ---///
Eigen::VectorXd stiff_diff = j_stiffness - des_j_stiffness;
double stiff_err = abs(stiff_diff.sum());
ROS_INFO_STREAM("Current error to desired stiffness: " << stiff_err);
if(stiff_err < 0.5){
ROS_INFO_STREAM('Desired Stiffness REACHED, you can manipulate the robot now...');
///-- Publish joint action message (for cart_to_joint) --//
//std_msgs::Bool j_action_msg;
// j_action_msg.data = false;
// pub_ja.publish(j_action_msg);
bBaseRun=false;
}
if (as_.isPreemptRequested() || !ros::ok())
{
ROS_INFO("Preempted");
as_.setPreempted();
// bBaseRun = false;
bBaseRun=false;
}
loop_rate.sleep();
}
return true;
/* if(!bBaseRun){
return false;
}else{
return true;
}*/
}else{
std::string msg;
msg = "Kuka_goto_cart_as::execute_CB: wrong action call, requested: " + goal->action_name + " actual: " + action_name;
ROS_ERROR("[%s]",msg.c_str());
return false;
}
}
// Main callback controlling the output rate
void jointStateCallback(const sensor_msgs::JointStateConstPtr& msg) {
const sensor_msgs::JointState* data = msg.get();
int es = data->effort.size();
int ps = data->position.size();
int name = data->name.size();
//15 is not fixed (la,ra,torso)
int joint_state_size;
if (simulation)
joint_state_size = 21;
else
joint_state_size = 14;
//Check joint state message size
if(name != joint_state_size)
return;
if(es != ps) {
ROS_WARN_STREAM_THROTTLE(1, "Effort and position sized unequal! EffortSize: "<<es<<" PosSize: "<<numdof);
return;
}
// Collect the right joint angles by name
int counter=0;
for(unsigned int i=0;i<es; ++i) {
sprintf(buf, "right_arm_%d_joint", counter);
if(!strcmp(buf, data->name[i].c_str())) {
// DLR gravity is negative!!
if(negate_torque) {
read_torque[counter] = - data->effort[i];
} else {
read_torque[counter] = data->effort[i];
}
read_jpos[counter] = data->position[i];
++counter;
}
// If cannot find all the joints
if(counter == numdof) { break; }
}
// If found more or less joints than expected!
if(counter != numdof) {
ROS_WARN_STREAM_THROTTLE(1, "Only found "<<counter<<" DOF in message. Expected "<<numdof);
} else {
// All OK. Compute and send.
mRobot->setJoints(read_jpos);
mRobot->getEEPose(ee_pose);
//Use estimated FT of ee
if (simulation){
computeFT(ee_ft);
sendFT(ee_ft);
}
else {
pub_ft.publish(msg_ft);
}
//Send estimated ee pose
sendPose(ee_pose);
}
}
