darwin_kinematics_alg_node.cpp

Go to the documentation of this file.

#include "darwin_kinematics_alg_node.h"

DarwinKinematicsAlgNode::DarwinKinematicsAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<DarwinKinematicsAlgorithm>(),
  tf_listener(ros::Duration(10.0))
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]

  // [init publishers]
  
  // [init subscribers]
  
  // [init services]
  this->get_fk_server_ = this->public_node_handle_.advertiseService("get_fk", &DarwinKinematicsAlgNode::get_fkCallback, this);
  this->get_fk_solver_info_server_ = this->public_node_handle_.advertiseService("get_fk_solver_info", &DarwinKinematicsAlgNode::get_fk_solver_infoCallback, this);
  this->get_ik_solver_info_server_ = this->public_node_handle_.advertiseService("get_ik_solver_info", &DarwinKinematicsAlgNode::get_ik_solver_infoCallback, this);
  this->get_ik_server_ = this->public_node_handle_.advertiseService("get_ik", &DarwinKinematicsAlgNode::get_ikCallback, this);
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
}

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

void DarwinKinematicsAlgNode::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 DarwinKinematicsAlgNode::get_fkCallback(kinematics_msgs::GetPositionFK::Request &req, kinematics_msgs::GetPositionFK::Response &res) 
{ 
  ROS_INFO("DarwinKinematicsAlgNode::get_fkCallback: New Request Received!"); 

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


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

  return true; 
}
bool DarwinKinematicsAlgNode::get_fk_solver_infoCallback(kinematics_msgs::GetKinematicSolverInfo::Request &req, kinematics_msgs::GetKinematicSolverInfo::Response &res) 
{ 
  std::vector<double> upper,lower;
 
  ROS_INFO("DarwinKinematicsAlgNode::get_fk_solver_infoCallback: New Request Received!"); 

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

  res.kinematic_solver_info.joint_names.resize(8);
  res.kinematic_solver_info.joint_names[0]="j_shoulder_l";
  res.kinematic_solver_info.joint_names[1]="j_high_arm_l";
  res.kinematic_solver_info.joint_names[2]="j_low_arm_l";
  res.kinematic_solver_info.joint_names[3]="j_wrist_l";
  res.kinematic_solver_info.joint_names[4]="j_shoulder_r";
  res.kinematic_solver_info.joint_names[5]="j_high_arm_r";
  res.kinematic_solver_info.joint_names[6]="j_low_arm_r";
  res.kinematic_solver_info.joint_names[7]="j_wrist_r";
  res.kinematic_solver_info.limits.resize(8);
  this->alg_.get_left_arm_upper_limits(upper);
  this->alg_.get_left_arm_lower_limits(lower);
  res.kinematic_solver_info.limits[0].joint_name="j_shoulder_l";
  res.kinematic_solver_info.limits[0].has_position_limits=true;
  res.kinematic_solver_info.limits[0].min_position=lower[0];
  res.kinematic_solver_info.limits[0].max_position=upper[0];
  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].joint_name="j_high_arm_l";
  res.kinematic_solver_info.limits[1].has_position_limits=true;
  res.kinematic_solver_info.limits[1].min_position=lower[1];
  res.kinematic_solver_info.limits[1].max_position=upper[1];
  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].joint_name="j_low_arm_l";
  res.kinematic_solver_info.limits[2].has_position_limits=true;
  res.kinematic_solver_info.limits[2].min_position=lower[2];
  res.kinematic_solver_info.limits[2].max_position=upper[2];
  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].joint_name="j_wrist_l";
  res.kinematic_solver_info.limits[3].has_position_limits=true;
  res.kinematic_solver_info.limits[3].min_position=lower[3];
  res.kinematic_solver_info.limits[3].max_position=upper[3];
  res.kinematic_solver_info.limits[3].has_velocity_limits=false;
  res.kinematic_solver_info.limits[3].has_acceleration_limits=false;
  this->alg_.get_right_arm_upper_limits(upper);
  this->alg_.get_right_arm_lower_limits(lower);
  res.kinematic_solver_info.limits[4].joint_name="j_shoulder_r";
  res.kinematic_solver_info.limits[4].has_position_limits=true;
  res.kinematic_solver_info.limits[4].min_position=lower[0];
  res.kinematic_solver_info.limits[4].max_position=upper[0];
  res.kinematic_solver_info.limits[4].has_velocity_limits=false;
  res.kinematic_solver_info.limits[4].has_acceleration_limits=false;
  res.kinematic_solver_info.limits[5].joint_name="j_high_arm_r";
  res.kinematic_solver_info.limits[5].has_position_limits=true;
  res.kinematic_solver_info.limits[5].min_position=lower[1];
  res.kinematic_solver_info.limits[5].max_position=upper[1];
  res.kinematic_solver_info.limits[5].has_velocity_limits=false;
  res.kinematic_solver_info.limits[5].has_acceleration_limits=false;
  res.kinematic_solver_info.limits[6].joint_name="j_low_arm_r";
  res.kinematic_solver_info.limits[6].has_position_limits=true;
  res.kinematic_solver_info.limits[6].min_position=lower[2];
  res.kinematic_solver_info.limits[6].max_position=upper[2];
  res.kinematic_solver_info.limits[6].has_velocity_limits=false;
  res.kinematic_solver_info.limits[6].has_acceleration_limits=false;
  res.kinematic_solver_info.limits[7].joint_name="j_wrist_r";
  res.kinematic_solver_info.limits[7].has_position_limits=true;
  res.kinematic_solver_info.limits[7].min_position=lower[3];
  res.kinematic_solver_info.limits[7].max_position=upper[3];
  res.kinematic_solver_info.limits[7].has_velocity_limits=false;
  res.kinematic_solver_info.limits[7].has_acceleration_limits=false;
 
  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  //this->get_fk_solver_info_mutex_.exit(); 

  return true; 
}
bool DarwinKinematicsAlgNode::get_ik_solver_infoCallback(kinematics_msgs::GetKinematicSolverInfo::Request &req, kinematics_msgs::GetKinematicSolverInfo::Response &res) 
{ 
  std::vector<double> upper,lower;

  ROS_INFO("DarwinKinematicsAlgNode::get_ik_solver_infoCallback: New Request Received!"); 

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

  res.kinematic_solver_info.joint_names.resize(8);
  res.kinematic_solver_info.joint_names[0]="j_shoulder_l";
  res.kinematic_solver_info.joint_names[1]="j_high_arm_l";
  res.kinematic_solver_info.joint_names[2]="j_low_arm_l";
  res.kinematic_solver_info.joint_names[3]="j_wrist_l";
  res.kinematic_solver_info.joint_names[4]="j_shoulder_r";
  res.kinematic_solver_info.joint_names[5]="j_high_arm_r";
  res.kinematic_solver_info.joint_names[6]="j_low_arm_r";
  res.kinematic_solver_info.joint_names[7]="j_wrist_r";
  res.kinematic_solver_info.limits.resize(8);
  this->alg_.get_left_arm_upper_limits(upper);
  this->alg_.get_left_arm_lower_limits(lower);
  res.kinematic_solver_info.limits[0].joint_name="j_shoulder_l";
  res.kinematic_solver_info.limits[0].has_position_limits=true;
  res.kinematic_solver_info.limits[0].min_position=lower[0];
  res.kinematic_solver_info.limits[0].max_position=upper[0];
  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].joint_name="j_high_arm_l";
  res.kinematic_solver_info.limits[1].has_position_limits=true;
  res.kinematic_solver_info.limits[1].min_position=lower[1];
  res.kinematic_solver_info.limits[1].max_position=upper[1];
  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].joint_name="j_low_arm_l";
  res.kinematic_solver_info.limits[2].has_position_limits=true;
  res.kinematic_solver_info.limits[2].min_position=lower[2];
  res.kinematic_solver_info.limits[2].max_position=upper[2];
  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].joint_name="j_wrist_l";
  res.kinematic_solver_info.limits[3].has_position_limits=true;
  res.kinematic_solver_info.limits[3].min_position=lower[3];
  res.kinematic_solver_info.limits[3].max_position=upper[3];
  res.kinematic_solver_info.limits[3].has_velocity_limits=false;
  res.kinematic_solver_info.limits[3].has_acceleration_limits=false;
  this->alg_.get_right_arm_upper_limits(upper);
  this->alg_.get_right_arm_lower_limits(lower);
  res.kinematic_solver_info.limits[4].joint_name="j_shoulder_r";
  res.kinematic_solver_info.limits[4].has_position_limits=true;
  res.kinematic_solver_info.limits[4].min_position=lower[0];
  res.kinematic_solver_info.limits[4].max_position=upper[0];
  res.kinematic_solver_info.limits[4].has_velocity_limits=false;
  res.kinematic_solver_info.limits[4].has_acceleration_limits=false;
  res.kinematic_solver_info.limits[5].joint_name="j_high_arm_r";
  res.kinematic_solver_info.limits[5].has_position_limits=true;
  res.kinematic_solver_info.limits[5].min_position=lower[1];
  res.kinematic_solver_info.limits[5].max_position=upper[1];
  res.kinematic_solver_info.limits[5].has_velocity_limits=false;
  res.kinematic_solver_info.limits[5].has_acceleration_limits=false;
  res.kinematic_solver_info.limits[6].joint_name="j_low_arm_r";
  res.kinematic_solver_info.limits[6].has_position_limits=true;
  res.kinematic_solver_info.limits[6].min_position=lower[2];
  res.kinematic_solver_info.limits[6].max_position=upper[2];
  res.kinematic_solver_info.limits[6].has_velocity_limits=false;
  res.kinematic_solver_info.limits[6].has_acceleration_limits=false;
  res.kinematic_solver_info.limits[7].joint_name="j_wrist_r";
  res.kinematic_solver_info.limits[7].has_position_limits=true;
  res.kinematic_solver_info.limits[7].min_position=lower[3];
  res.kinematic_solver_info.limits[7].max_position=upper[3];
  res.kinematic_solver_info.limits[7].has_velocity_limits=false;
  res.kinematic_solver_info.limits[7].has_acceleration_limits=false;

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

  return true; 
}
bool DarwinKinematicsAlgNode::get_ikCallback(kinematics_msgs::GetPositionIK::Request &req, kinematics_msgs::GetPositionIK::Response &res) 
{ 
  geometry_msgs::PoseStamped local_goal;
  std::vector<double> position(3);
  std::vector<double> angles(4);
  std::vector<double> init_angles(4);
  bool tf_exists=false;

  ROS_INFO("DarwinKinematicsAlgNode::get_ikCallback: New Request Received!"); 
  //use appropiate mutex to shared variables if necessary 
  //this->alg_.lock(); 
  //this->get_ik_mutex_.enter(); 

  if(req.ik_request.ik_link_name=="MP_ARM_GRIPPER_FIX_L")// left arm wrist
  {
    // transform the input pose to the base of the left arm j_shoulder_l
    try{
      tf_exists = tf_listener.waitForTransform(req.ik_request.pose_stamped.header.frame_id,std::string("MP_BACK_L"), req.ik_request.pose_stamped.header.stamp,
                                               ros::Duration(10),ros::Duration(0.01));
      tf_listener.transformPose(std::string("MP_BACK_L"),req.ik_request.pose_stamped,local_goal);
      // get the desired position
      position[0]=local_goal.pose.position.x;
      position[1]=local_goal.pose.position.y;
      position[2]=local_goal.pose.position.z;
      std::cout << position[0] << "," << position[1] << "," << position[2] << std::endl;
      // get the seed for the angles
      if(req.ik_request.ik_seed_state.joint_state.position.size()!=4)
        res.error_code.val=arm_navigation_msgs::ArmNavigationErrorCodes::INCOMPLETE_ROBOT_STATE;
      else
      {
        init_angles[0]=req.ik_request.ik_seed_state.joint_state.position[0];
        init_angles[1]=req.ik_request.ik_seed_state.joint_state.position[1];
        init_angles[2]=req.ik_request.ik_seed_state.joint_state.position[2];
        init_angles[3]=req.ik_request.ik_seed_state.joint_state.position[3];
        // call the inverse kinematics function
        if(this->alg_.get_left_arm_ik(position,init_angles,angles))
        {
          // return the computed angles
          res.solution.joint_state.position.resize(4);
          res.solution.joint_state.position[0]=angles[0];
          res.solution.joint_state.position[1]=-angles[1];// change sign to comply with the real motion of the servo
          res.solution.joint_state.position[2]=-angles[2];// change sign to comply with the real motion of the servo
          res.solution.joint_state.position[3]=angles[3];
          res.error_code.val=arm_navigation_msgs::ArmNavigationErrorCodes::SUCCESS;// SUCCESS
        }
        else
          res.error_code.val=arm_navigation_msgs::ArmNavigationErrorCodes::NO_IK_SOLUTION;// NO SOLUTION
      }
    }catch(tf::TransformException ex){
      res.error_code.val=arm_navigation_msgs::ArmNavigationErrorCodes::FRAME_TRANSFORM_FAILURE;// 
      ROS_ERROR("NoCollisionAlgNode:: %s",ex.what());
    }
  }
  else if(req.ik_request.ik_link_name=="MP_ARM_GRIPPER_FIX_R")// right arm wrist
  {
    // transform the input pose to the base of the left arm j_shoulder_l
    try{
      tf_exists = tf_listener.waitForTransform(req.ik_request.pose_stamped.header.frame_id,std::string("MP_BACK_R"), req.ik_request.pose_stamped.header.stamp,
                                               ros::Duration(1),ros::Duration(0.01));
      tf_listener.transformPose(std::string("MP_BACK_R"),req.ik_request.pose_stamped,local_goal);
      // get the desired position
      position[0]=local_goal.pose.position.x;
      position[1]=local_goal.pose.position.y;
      position[2]=local_goal.pose.position.z;
      std::cout << position[0] << "," << position[1] << "," << position[2] << std::endl;
      // get the seed for the angles
      if(req.ik_request.ik_seed_state.joint_state.position.size()!=4)
        res.error_code.val=arm_navigation_msgs::ArmNavigationErrorCodes::INCOMPLETE_ROBOT_STATE;
      else
      {
        init_angles[0]=req.ik_request.ik_seed_state.joint_state.position[0];
        init_angles[1]=req.ik_request.ik_seed_state.joint_state.position[1];
        init_angles[2]=req.ik_request.ik_seed_state.joint_state.position[2];
        init_angles[3]=req.ik_request.ik_seed_state.joint_state.position[3];
        // call the inverse kinematics function
        if(this->alg_.get_right_arm_ik(position,init_angles,angles))
        {
          // return the computed angles
          res.solution.joint_state.position.resize(4);
          res.solution.joint_state.position[0]=angles[0];
          res.solution.joint_state.position[1]=-angles[1];// change sign to comply with the real motion of the servo
          res.solution.joint_state.position[2]=-angles[2];// chnage sign to comply with the real motion of the servo
          res.solution.joint_state.position[3]=angles[3];
          res.error_code.val=arm_navigation_msgs::ArmNavigationErrorCodes::SUCCESS;// SUCCESS
        }
        else
          res.error_code.val=arm_navigation_msgs::ArmNavigationErrorCodes::NO_IK_SOLUTION;// NO SOLUTION
      }
    }catch(tf::TransformException ex){
      res.error_code.val=arm_navigation_msgs::ArmNavigationErrorCodes::FRAME_TRANSFORM_FAILURE;// 
    }
  }

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

  return true; 
}

/*  [action callbacks] */

/*  [action requests] */

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

  this->alg_.unlock();
}

void DarwinKinematicsAlgNode::addNodeDiagnostics(void)
{
}

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