tibi_dabo_arm_kinematics_alg_node.cpp

Go to the documentation of this file.

#include "tibi_dabo_arm_kinematics_alg_node.h"


TibiDaboArmKinematicsAlgNode::TibiDaboArmKinematicsAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<TibiDaboArmKinematicsAlgorithm>()
{
  
  this->public_node_handle_.getParam("arm_id", arm_id);
  
  //init class attributes if necessary
  if(arm_id=="Left")   arm = new CSegwayArmKinematics(LEFT_ARM);
  else if(arm_id=="Right")  arm = new CSegwayArmKinematics(RIGHT_ARM);

  angles.resize(4);
  pose.resize(3);
  arm_length=1;
  
  //this->loop_rate_ = 2;//in [Hz]
  // [init publishers]
  // [init subscribers]
  // [init services]
  this->GetPositionIK_server_ = this->public_node_handle_.advertiseService("GetPositionIK", &TibiDaboArmKinematicsAlgNode::GetPositionIKCallback, this);
  this->GetKinematicSolverInfo_server_ = this->public_node_handle_.advertiseService("GetKinematicSolverInfo", &TibiDaboArmKinematicsAlgNode::GetKinematicSolverInfoCallback, this);
  this->GetPositionFK_server_ = this->public_node_handle_.advertiseService("GetPositionFK", &TibiDaboArmKinematicsAlgNode::GetPositionFKCallback, this);
  
  // [init clients]
  // [init action servers]
  // [init action clients]
}

TibiDaboArmKinematicsAlgNode::~TibiDaboArmKinematicsAlgNode(void)
{
  // [free dynamic memory]
}

void TibiDaboArmKinematicsAlgNode::mainNodeThread(void)
{
  // [fill msg structures]
  
  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]

  // [publish messages]
}

/*  [subscriber callbacks] */

/*  [service callbacks] */
bool TibiDaboArmKinematicsAlgNode::GetPositionIKCallback(kinematics_msgs::GetPositionIK::Request &req, kinematics_msgs::GetPositionIK::Response &res) 
{ 
  static int calibration_countdown=5;
  //ROS_INFO("TibiDaboArmKinematicsAlgNode::GetPositionIKCallback: New Request Received!"); 

  //Check if theta3 is given
  if(req.ik_request.ik_seed_state.joint_state.name[0].empty()) {
    ROS_ERROR("ik_request.ik_seed_state.joint_state.name[0] should be 'shoulder_roll', and ik_request.ik_seed_state.joint_state.position[0] must be a valid theta3 seed");
    return false;
  }
  
  //Reset arm lenght if user leaves
  if(req.ik_request.ik_seed_state.joint_state.velocity[0]==1){
    calibration_countdown=5;
    arm_length=1;
  }
 
  //Obtain pose
  pose[0]=req.ik_request.pose_stamped.pose.position.x;
  pose[1]=req.ik_request.pose_stamped.pose.position.y;
  pose[2]=req.ik_request.pose_stamped.pose.position.z;
  
  //Transform pose from neck to shoulder
  pose=(*arm).neck_to_shoulder_transform(pose);
  
  if(!req.ik_request.ik_seed_state.joint_state.velocity[0]==2){ //Not debug mode
  //Try to get the arm length (get the maximum length) (only at the beginning)
  if(sqrt(pose[0]*pose[0]+pose[1]*pose[1]+pose[2]*pose[2])>arm_length && calibration_countdown >= 0)
  {
    arm_length=sqrt(pose[0]*pose[0]+pose[1]*pose[1]+pose[2]*pose[2]);
    calibration_countdown--;
    ROS_WARN_STREAM("CALIBRATING: PLEASE EXTEND ARM  " << calibration_countdown);
    sleep(1);
  }
  
  //Escalate to robot's arm 
  pose[0]=pose[0]/arm_length*((*arm).D+(*arm).A);
  pose[1]=pose[1]/arm_length*((*arm).D+(*arm).A);
  pose[2]=pose[2]/arm_length*((*arm).D+(*arm).A);
  
  }
  
  ROS_WARN_STREAM("Pose for IK: " << pose[0] << " " << pose[1] << " " << pose[2]);
  
  //Calculate IK with q3 seed
  if(!(*arm).best_ikine(pose, req.ik_request.ik_seed_state.joint_state.position[0], angles)){
    ROS_ERROR("IK could not be calculated");
    return false;
  }
  
  ROS_WARN_STREAM("IK angles in kinematics_base: " << angles[0]*180/3.14159 << " " << angles[1]*180/3.14159 << " " << angles[2]*180/3.14159 << " " << angles[3]*180/3.14159);
  
  
  //Potentiometer in left and right arms are not same oriented
  if(arm_id=="Left"){
    angles[0]=-angles[0];
  }else if(arm_id=="Right"){
    angles[3]=-angles[3];
  }
  
  //Transform kinematics angles to potentiometer angles   
  angles[0]=angles[0]+(*arm).theta1_pote_offset;
  angles[1]=angles[1]+(*arm).theta2_pote_offset;
  angles[2]=angles[2]+(*arm).theta3_pote_offset;
  angles[3]=angles[3]+(*arm).theta4_pote_offset;
    
  //Fill srv
  res.solution.joint_state.name.resize(4);
  res.solution.joint_state.position.resize(4);
  int i;
  for(i=0;i<4;i++){
    res.solution.joint_state.name[i]=(*arm).joint_name[i];
    res.solution.joint_state.position[i]=angles[i];
  }
  
  //use appropiate mutex to shared variables if necessary 
  //this->alg_.lock(); 
  //this->GetPositionIK_mutex_.enter(); 

  //if(this->alg_.isRunning()) 
  //{ 
    //ROS_INFO("TibiDaboArmKinematicsAlgNode::GetPositionIKCallback: Processin New Request!"); 
    //do operations with req and output on res 
    //res.data2 = req.data1 + my_var; 
  //} 
  //else 
  //{ 
    //ROS_INFO("TibiDaboArmKinematicsAlgNode::GetPositionIKCallback: ERROR: alg is not on run mode yet."); 
  //} 

  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  //this->GetPositionIK_mutex_.exit(); 

  return true; 
}
bool TibiDaboArmKinematicsAlgNode::GetKinematicSolverInfoCallback(kinematics_msgs::GetKinematicSolverInfo::Request &req, kinematics_msgs::GetKinematicSolverInfo::Response &res) 
{ 
  ROS_INFO("TibiDaboArmKinematicsAlgNode::GetKinematicSolverInfoCallback: New Request Received!"); 
    
  //Fill srv
  
  //Joint names
  int i;
  res.kinematic_solver_info.joint_names.resize(4);
  
  for(i=0;i<4;i++){  
    res.kinematic_solver_info.joint_names[i] = (*arm).joint_name[i];
    
    res.kinematic_solver_info.limits.resize(4);
    res.kinematic_solver_info.limits[i].joint_name=(*arm).joint_name[i];
  }
  
  //Limits
  res.kinematic_solver_info.limits.resize(4);
  
  res.kinematic_solver_info.limits[0].has_position_limits=true;
  res.kinematic_solver_info.limits[0].min_position=(*arm).theta1_lower;
  res.kinematic_solver_info.limits[0].max_position=(*arm).theta1_upper;
  res.kinematic_solver_info.limits[0].has_velocity_limits=false;
  res.kinematic_solver_info.limits[0].has_acceleration_limits=false;
  
  res.kinematic_solver_info.limits[1].has_position_limits=true;
  res.kinematic_solver_info.limits[1].min_position=(*arm).theta2_lower;
  res.kinematic_solver_info.limits[1].max_position=(*arm).theta2_upper;
  res.kinematic_solver_info.limits[1].has_velocity_limits=false;
  res.kinematic_solver_info.limits[1].has_acceleration_limits=false;
  
  res.kinematic_solver_info.limits[2].has_position_limits=true;
  res.kinematic_solver_info.limits[2].min_position=(*arm).theta3_lower;
  res.kinematic_solver_info.limits[2].max_position=(*arm).theta3_upper;
  res.kinematic_solver_info.limits[2].has_velocity_limits=false;
  res.kinematic_solver_info.limits[2].has_acceleration_limits=false;
  
  res.kinematic_solver_info.limits[3].has_position_limits=true;
  res.kinematic_solver_info.limits[3].min_position=(*arm).theta4_lower;
  res.kinematic_solver_info.limits[3].max_position=(*arm).theta4_upper;
  res.kinematic_solver_info.limits[3].has_velocity_limits=false;
  res.kinematic_solver_info.limits[3].has_acceleration_limits=false; 
  
  
  ROS_INFO("Sending back Kinematic Info");

  //use appropiate mutex to shared variables if necessary 
  //this->alg_.lock(); 
  //this->GetKinematicSolverInfo_mutex_.enter(); 

  //if(this->alg_.isRunning()) 
  //{ 
    //ROS_INFO("TibiDaboArmKinematicsAlgNode::GetKinematicSolverInfoCallback: Processin New Request!"); 
    //do operations with req and output on res 
    //res.data2 = req.data1 + my_var; 
  //} 
  //else 
  //{ 
    //ROS_INFO("TibiDaboArmKinematicsAlgNode::GetKinematicSolverInfoCallback: ERROR: alg is not on run mode yet."); 
  //} 

  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  //this->GetKinematicSolverInfo_mutex_.exit(); 

  return true; 
}
bool TibiDaboArmKinematicsAlgNode::GetPositionFKCallback(kinematics_msgs::GetPositionFK::Request &req, kinematics_msgs::GetPositionFK::Response &res) 
{ 
  
  ROS_INFO("TibiDaboArmKinematicsAlgNode::GetPositionFKCallback: New Request Received!"); 
  
  //For FK, obtain 4 joints associated with their names
  int i;
  for(i=0;i<4;i++)
  {
    if(!req.robot_state.joint_state.name[i].compare((*arm).joint_name[0])) angles[0]=req.robot_state.joint_state.position[i];
    else if(!req.robot_state.joint_state.name[i].compare((*arm).joint_name[1])) angles[1]=req.robot_state.joint_state.position[i];
    else if(!req.robot_state.joint_state.name[i].compare((*arm).joint_name[2])) angles[2]=req.robot_state.joint_state.position[i];
    else if(!req.robot_state.joint_state.name[i].compare((*arm).joint_name[3])) angles[3]=req.robot_state.joint_state.position[i];
    else {
      ROS_INFO("Incorrect joint name. Joint name must be shoulder_pitch, shoulder_yaw, shoulder_roll or elbow");
      return false;
    }
  }
    
  //Calculate FK
  pose=(*arm).fkine(angles);

  //Fill srv
  res.pose_stamped.resize(1);
  res.pose_stamped[0].pose.position.x=pose[0];
  res.pose_stamped[0].pose.position.y=pose[1];
  res.pose_stamped[0].pose.position.z=pose[2];
  
  ROS_INFO("Sending back FK");
  
  //use appropiate mutex to shared variables if necessary 
  //this->alg_.lock(); 
  //this->GetPositionFK_mutex_.enter(); 

  //if(this->alg_.isRunning()) 
  //{ 
    //ROS_INFO("TibiDaboArmKinematicsAlgNode::GetPositionFKCallback: Processin New Request!"); 
    //do operations with req and output on res 
    //res.data2 = req.data1 + my_var; 
  //} 
  //else 
  //{ 
    //ROS_INFO("TibiDaboArmKinematicsAlgNode::GetPositionFKCallback: ERROR: alg is not on run mode yet."); 
  //} 

  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  //this->GetPositionFK_mutex_.exit(); 

  return true; 
}

/*  [action callbacks] */

/*  [action requests] */

void TibiDaboArmKinematicsAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();
  arm_id=config.arm_id;

  this->alg_.unlock();
}

void TibiDaboArmKinematicsAlgNode::addNodeDiagnostics(void)
{
}

/* main function */
int main(int argc,char *argv[])
{
  return algorithm_base::main<TibiDaboArmKinematicsAlgNode>(argc, argv, "tibi_dabo_arm_kinematics_alg_node");
}