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