darwin_kinematics_alg_node.cpp
Go to the documentation of this file.
00001 #include "darwin_kinematics_alg_node.h"
00002 
00003 DarwinKinematicsAlgNode::DarwinKinematicsAlgNode(void) :
00004   algorithm_base::IriBaseAlgorithm<DarwinKinematicsAlgorithm>(),
00005   tf_listener(ros::Duration(10.0))
00006 {
00007   //init class attributes if necessary
00008   //this->loop_rate_ = 2;//in [Hz]
00009 
00010   // [init publishers]
00011   
00012   // [init subscribers]
00013   
00014   // [init services]
00015   this->get_fk_server_ = this->public_node_handle_.advertiseService("get_fk", &DarwinKinematicsAlgNode::get_fkCallback, this);
00016   this->get_fk_solver_info_server_ = this->public_node_handle_.advertiseService("get_fk_solver_info", &DarwinKinematicsAlgNode::get_fk_solver_infoCallback, this);
00017   this->get_ik_solver_info_server_ = this->public_node_handle_.advertiseService("get_ik_solver_info", &DarwinKinematicsAlgNode::get_ik_solver_infoCallback, this);
00018   this->get_ik_server_ = this->public_node_handle_.advertiseService("get_ik", &DarwinKinematicsAlgNode::get_ikCallback, this);
00019   
00020   // [init clients]
00021   
00022   // [init action servers]
00023   
00024   // [init action clients]
00025 }
00026 
00027 DarwinKinematicsAlgNode::~DarwinKinematicsAlgNode(void)
00028 {
00029   // [free dynamic memory]
00030 }
00031 
00032 void DarwinKinematicsAlgNode::mainNodeThread(void)
00033 {
00034   // [fill msg structures]
00035   
00036   // [fill srv structure and make request to the server]
00037   
00038   // [fill action structure and make request to the action server]
00039 
00040   // [publish messages]
00041 }
00042 
00043 /*  [subscriber callbacks] */
00044 
00045 /*  [service callbacks] */
00046 bool DarwinKinematicsAlgNode::get_fkCallback(kinematics_msgs::GetPositionFK::Request &req, kinematics_msgs::GetPositionFK::Response &res) 
00047 { 
00048   ROS_INFO("DarwinKinematicsAlgNode::get_fkCallback: New Request Received!"); 
00049 
00050   //use appropiate mutex to shared variables if necessary 
00051   //this->alg_.lock(); 
00052   //this->get_fk_mutex_.enter(); 
00053 
00054 
00055   //unlock previously blocked shared variables 
00056   //this->alg_.unlock(); 
00057   //this->get_fk_mutex_.exit(); 
00058 
00059   return true; 
00060 }
00061 bool DarwinKinematicsAlgNode::get_fk_solver_infoCallback(kinematics_msgs::GetKinematicSolverInfo::Request &req, kinematics_msgs::GetKinematicSolverInfo::Response &res) 
00062 { 
00063   std::vector<double> upper,lower;
00064  
00065   ROS_INFO("DarwinKinematicsAlgNode::get_fk_solver_infoCallback: New Request Received!"); 
00066 
00067   //use appropiate mutex to shared variables if necessary 
00068   //this->alg_.lock(); 
00069   //this->get_fk_solver_info_mutex_.enter(); 
00070 
00071   res.kinematic_solver_info.joint_names.resize(8);
00072   res.kinematic_solver_info.joint_names[0]="j_shoulder_l";
00073   res.kinematic_solver_info.joint_names[1]="j_high_arm_l";
00074   res.kinematic_solver_info.joint_names[2]="j_low_arm_l";
00075   res.kinematic_solver_info.joint_names[3]="j_wrist_l";
00076   res.kinematic_solver_info.joint_names[4]="j_shoulder_r";
00077   res.kinematic_solver_info.joint_names[5]="j_high_arm_r";
00078   res.kinematic_solver_info.joint_names[6]="j_low_arm_r";
00079   res.kinematic_solver_info.joint_names[7]="j_wrist_r";
00080   res.kinematic_solver_info.limits.resize(8);
00081   this->alg_.get_left_arm_upper_limits(upper);
00082   this->alg_.get_left_arm_lower_limits(lower);
00083   res.kinematic_solver_info.limits[0].joint_name="j_shoulder_l";
00084   res.kinematic_solver_info.limits[0].has_position_limits=true;
00085   res.kinematic_solver_info.limits[0].min_position=lower[0];
00086   res.kinematic_solver_info.limits[0].max_position=upper[0];
00087   res.kinematic_solver_info.limits[0].has_velocity_limits=false;
00088   res.kinematic_solver_info.limits[0].has_acceleration_limits=false;
00089   res.kinematic_solver_info.limits[1].joint_name="j_high_arm_l";
00090   res.kinematic_solver_info.limits[1].has_position_limits=true;
00091   res.kinematic_solver_info.limits[1].min_position=lower[1];
00092   res.kinematic_solver_info.limits[1].max_position=upper[1];
00093   res.kinematic_solver_info.limits[1].has_velocity_limits=false;
00094   res.kinematic_solver_info.limits[1].has_acceleration_limits=false;
00095   res.kinematic_solver_info.limits[2].joint_name="j_low_arm_l";
00096   res.kinematic_solver_info.limits[2].has_position_limits=true;
00097   res.kinematic_solver_info.limits[2].min_position=lower[2];
00098   res.kinematic_solver_info.limits[2].max_position=upper[2];
00099   res.kinematic_solver_info.limits[2].has_velocity_limits=false;
00100   res.kinematic_solver_info.limits[2].has_acceleration_limits=false;
00101   res.kinematic_solver_info.limits[3].joint_name="j_wrist_l";
00102   res.kinematic_solver_info.limits[3].has_position_limits=true;
00103   res.kinematic_solver_info.limits[3].min_position=lower[3];
00104   res.kinematic_solver_info.limits[3].max_position=upper[3];
00105   res.kinematic_solver_info.limits[3].has_velocity_limits=false;
00106   res.kinematic_solver_info.limits[3].has_acceleration_limits=false;
00107   this->alg_.get_right_arm_upper_limits(upper);
00108   this->alg_.get_right_arm_lower_limits(lower);
00109   res.kinematic_solver_info.limits[4].joint_name="j_shoulder_r";
00110   res.kinematic_solver_info.limits[4].has_position_limits=true;
00111   res.kinematic_solver_info.limits[4].min_position=lower[0];
00112   res.kinematic_solver_info.limits[4].max_position=upper[0];
00113   res.kinematic_solver_info.limits[4].has_velocity_limits=false;
00114   res.kinematic_solver_info.limits[4].has_acceleration_limits=false;
00115   res.kinematic_solver_info.limits[5].joint_name="j_high_arm_r";
00116   res.kinematic_solver_info.limits[5].has_position_limits=true;
00117   res.kinematic_solver_info.limits[5].min_position=lower[1];
00118   res.kinematic_solver_info.limits[5].max_position=upper[1];
00119   res.kinematic_solver_info.limits[5].has_velocity_limits=false;
00120   res.kinematic_solver_info.limits[5].has_acceleration_limits=false;
00121   res.kinematic_solver_info.limits[6].joint_name="j_low_arm_r";
00122   res.kinematic_solver_info.limits[6].has_position_limits=true;
00123   res.kinematic_solver_info.limits[6].min_position=lower[2];
00124   res.kinematic_solver_info.limits[6].max_position=upper[2];
00125   res.kinematic_solver_info.limits[6].has_velocity_limits=false;
00126   res.kinematic_solver_info.limits[6].has_acceleration_limits=false;
00127   res.kinematic_solver_info.limits[7].joint_name="j_wrist_r";
00128   res.kinematic_solver_info.limits[7].has_position_limits=true;
00129   res.kinematic_solver_info.limits[7].min_position=lower[3];
00130   res.kinematic_solver_info.limits[7].max_position=upper[3];
00131   res.kinematic_solver_info.limits[7].has_velocity_limits=false;
00132   res.kinematic_solver_info.limits[7].has_acceleration_limits=false;
00133  
00134   //unlock previously blocked shared variables 
00135   //this->alg_.unlock(); 
00136   //this->get_fk_solver_info_mutex_.exit(); 
00137 
00138   return true; 
00139 }
00140 bool DarwinKinematicsAlgNode::get_ik_solver_infoCallback(kinematics_msgs::GetKinematicSolverInfo::Request &req, kinematics_msgs::GetKinematicSolverInfo::Response &res) 
00141 { 
00142   std::vector<double> upper,lower;
00143 
00144   ROS_INFO("DarwinKinematicsAlgNode::get_ik_solver_infoCallback: New Request Received!"); 
00145 
00146   //use appropiate mutex to shared variables if necessary 
00147   //this->alg_.lock(); 
00148   //this->get_ik_solver_info_mutex_.enter(); 
00149 
00150   res.kinematic_solver_info.joint_names.resize(8);
00151   res.kinematic_solver_info.joint_names[0]="j_shoulder_l";
00152   res.kinematic_solver_info.joint_names[1]="j_high_arm_l";
00153   res.kinematic_solver_info.joint_names[2]="j_low_arm_l";
00154   res.kinematic_solver_info.joint_names[3]="j_wrist_l";
00155   res.kinematic_solver_info.joint_names[4]="j_shoulder_r";
00156   res.kinematic_solver_info.joint_names[5]="j_high_arm_r";
00157   res.kinematic_solver_info.joint_names[6]="j_low_arm_r";
00158   res.kinematic_solver_info.joint_names[7]="j_wrist_r";
00159   res.kinematic_solver_info.limits.resize(8);
00160   this->alg_.get_left_arm_upper_limits(upper);
00161   this->alg_.get_left_arm_lower_limits(lower);
00162   res.kinematic_solver_info.limits[0].joint_name="j_shoulder_l";
00163   res.kinematic_solver_info.limits[0].has_position_limits=true;
00164   res.kinematic_solver_info.limits[0].min_position=lower[0];
00165   res.kinematic_solver_info.limits[0].max_position=upper[0];
00166   res.kinematic_solver_info.limits[0].has_velocity_limits=false;
00167   res.kinematic_solver_info.limits[0].has_acceleration_limits=false;
00168   res.kinematic_solver_info.limits[1].joint_name="j_high_arm_l";
00169   res.kinematic_solver_info.limits[1].has_position_limits=true;
00170   res.kinematic_solver_info.limits[1].min_position=lower[1];
00171   res.kinematic_solver_info.limits[1].max_position=upper[1];
00172   res.kinematic_solver_info.limits[1].has_velocity_limits=false;
00173   res.kinematic_solver_info.limits[1].has_acceleration_limits=false;
00174   res.kinematic_solver_info.limits[2].joint_name="j_low_arm_l";
00175   res.kinematic_solver_info.limits[2].has_position_limits=true;
00176   res.kinematic_solver_info.limits[2].min_position=lower[2];
00177   res.kinematic_solver_info.limits[2].max_position=upper[2];
00178   res.kinematic_solver_info.limits[2].has_velocity_limits=false;
00179   res.kinematic_solver_info.limits[2].has_acceleration_limits=false;
00180   res.kinematic_solver_info.limits[3].joint_name="j_wrist_l";
00181   res.kinematic_solver_info.limits[3].has_position_limits=true;
00182   res.kinematic_solver_info.limits[3].min_position=lower[3];
00183   res.kinematic_solver_info.limits[3].max_position=upper[3];
00184   res.kinematic_solver_info.limits[3].has_velocity_limits=false;
00185   res.kinematic_solver_info.limits[3].has_acceleration_limits=false;
00186   this->alg_.get_right_arm_upper_limits(upper);
00187   this->alg_.get_right_arm_lower_limits(lower);
00188   res.kinematic_solver_info.limits[4].joint_name="j_shoulder_r";
00189   res.kinematic_solver_info.limits[4].has_position_limits=true;
00190   res.kinematic_solver_info.limits[4].min_position=lower[0];
00191   res.kinematic_solver_info.limits[4].max_position=upper[0];
00192   res.kinematic_solver_info.limits[4].has_velocity_limits=false;
00193   res.kinematic_solver_info.limits[4].has_acceleration_limits=false;
00194   res.kinematic_solver_info.limits[5].joint_name="j_high_arm_r";
00195   res.kinematic_solver_info.limits[5].has_position_limits=true;
00196   res.kinematic_solver_info.limits[5].min_position=lower[1];
00197   res.kinematic_solver_info.limits[5].max_position=upper[1];
00198   res.kinematic_solver_info.limits[5].has_velocity_limits=false;
00199   res.kinematic_solver_info.limits[5].has_acceleration_limits=false;
00200   res.kinematic_solver_info.limits[6].joint_name="j_low_arm_r";
00201   res.kinematic_solver_info.limits[6].has_position_limits=true;
00202   res.kinematic_solver_info.limits[6].min_position=lower[2];
00203   res.kinematic_solver_info.limits[6].max_position=upper[2];
00204   res.kinematic_solver_info.limits[6].has_velocity_limits=false;
00205   res.kinematic_solver_info.limits[6].has_acceleration_limits=false;
00206   res.kinematic_solver_info.limits[7].joint_name="j_wrist_r";
00207   res.kinematic_solver_info.limits[7].has_position_limits=true;
00208   res.kinematic_solver_info.limits[7].min_position=lower[3];
00209   res.kinematic_solver_info.limits[7].max_position=upper[3];
00210   res.kinematic_solver_info.limits[7].has_velocity_limits=false;
00211   res.kinematic_solver_info.limits[7].has_acceleration_limits=false;
00212 
00213   //unlock previously blocked shared variables 
00214   //this->alg_.unlock(); 
00215   //this->get_ik_solver_info_mutex_.exit(); 
00216 
00217   return true; 
00218 }
00219 bool DarwinKinematicsAlgNode::get_ikCallback(kinematics_msgs::GetPositionIK::Request &req, kinematics_msgs::GetPositionIK::Response &res) 
00220 { 
00221   geometry_msgs::PoseStamped local_goal;
00222   std::vector<double> position(3);
00223   std::vector<double> angles(4);
00224   std::vector<double> init_angles(4);
00225   bool tf_exists=false;
00226 
00227   ROS_INFO("DarwinKinematicsAlgNode::get_ikCallback: New Request Received!"); 
00228   //use appropiate mutex to shared variables if necessary 
00229   //this->alg_.lock(); 
00230   //this->get_ik_mutex_.enter(); 
00231 
00232   if(req.ik_request.ik_link_name=="MP_ARM_GRIPPER_FIX_L")// left arm wrist
00233   {
00234     // transform the input pose to the base of the left arm j_shoulder_l
00235     try{
00236       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,
00237                                                ros::Duration(10),ros::Duration(0.01));
00238       tf_listener.transformPose(std::string("MP_BACK_L"),req.ik_request.pose_stamped,local_goal);
00239       // get the desired position
00240       position[0]=local_goal.pose.position.x;
00241       position[1]=local_goal.pose.position.y;
00242       position[2]=local_goal.pose.position.z;
00243       std::cout << position[0] << "," << position[1] << "," << position[2] << std::endl;
00244       // get the seed for the angles
00245       if(req.ik_request.ik_seed_state.joint_state.position.size()!=4)
00246         res.error_code.val=arm_navigation_msgs::ArmNavigationErrorCodes::INCOMPLETE_ROBOT_STATE;
00247       else
00248       {
00249         init_angles[0]=req.ik_request.ik_seed_state.joint_state.position[0];
00250         init_angles[1]=req.ik_request.ik_seed_state.joint_state.position[1];
00251         init_angles[2]=req.ik_request.ik_seed_state.joint_state.position[2];
00252         init_angles[3]=req.ik_request.ik_seed_state.joint_state.position[3];
00253         // call the inverse kinematics function
00254         if(this->alg_.get_left_arm_ik(position,init_angles,angles))
00255         {
00256           // return the computed angles
00257           res.solution.joint_state.position.resize(4);
00258           res.solution.joint_state.position[0]=angles[0];
00259           res.solution.joint_state.position[1]=-angles[1];// change sign to comply with the real motion of the servo
00260           res.solution.joint_state.position[2]=-angles[2];// change sign to comply with the real motion of the servo
00261           res.solution.joint_state.position[3]=angles[3];
00262           res.error_code.val=arm_navigation_msgs::ArmNavigationErrorCodes::SUCCESS;// SUCCESS
00263         }
00264         else
00265           res.error_code.val=arm_navigation_msgs::ArmNavigationErrorCodes::NO_IK_SOLUTION;// NO SOLUTION
00266       }
00267     }catch(tf::TransformException ex){
00268       res.error_code.val=arm_navigation_msgs::ArmNavigationErrorCodes::FRAME_TRANSFORM_FAILURE;// 
00269       ROS_ERROR("NoCollisionAlgNode:: %s",ex.what());
00270     }
00271   }
00272   else if(req.ik_request.ik_link_name=="MP_ARM_GRIPPER_FIX_R")// right arm wrist
00273   {
00274     // transform the input pose to the base of the left arm j_shoulder_l
00275     try{
00276       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,
00277                                                ros::Duration(1),ros::Duration(0.01));
00278       tf_listener.transformPose(std::string("MP_BACK_R"),req.ik_request.pose_stamped,local_goal);
00279       // get the desired position
00280       position[0]=local_goal.pose.position.x;
00281       position[1]=local_goal.pose.position.y;
00282       position[2]=local_goal.pose.position.z;
00283       std::cout << position[0] << "," << position[1] << "," << position[2] << std::endl;
00284       // get the seed for the angles
00285       if(req.ik_request.ik_seed_state.joint_state.position.size()!=4)
00286         res.error_code.val=arm_navigation_msgs::ArmNavigationErrorCodes::INCOMPLETE_ROBOT_STATE;
00287       else
00288       {
00289         init_angles[0]=req.ik_request.ik_seed_state.joint_state.position[0];
00290         init_angles[1]=req.ik_request.ik_seed_state.joint_state.position[1];
00291         init_angles[2]=req.ik_request.ik_seed_state.joint_state.position[2];
00292         init_angles[3]=req.ik_request.ik_seed_state.joint_state.position[3];
00293         // call the inverse kinematics function
00294         if(this->alg_.get_right_arm_ik(position,init_angles,angles))
00295         {
00296           // return the computed angles
00297           res.solution.joint_state.position.resize(4);
00298           res.solution.joint_state.position[0]=angles[0];
00299           res.solution.joint_state.position[1]=-angles[1];// change sign to comply with the real motion of the servo
00300           res.solution.joint_state.position[2]=-angles[2];// chnage sign to comply with the real motion of the servo
00301           res.solution.joint_state.position[3]=angles[3];
00302           res.error_code.val=arm_navigation_msgs::ArmNavigationErrorCodes::SUCCESS;// SUCCESS
00303         }
00304         else
00305           res.error_code.val=arm_navigation_msgs::ArmNavigationErrorCodes::NO_IK_SOLUTION;// NO SOLUTION
00306       }
00307     }catch(tf::TransformException ex){
00308       res.error_code.val=arm_navigation_msgs::ArmNavigationErrorCodes::FRAME_TRANSFORM_FAILURE;// 
00309     }
00310   }
00311 
00312   //unlock previously blocked shared variables 
00313   //this->alg_.unlock(); 
00314   //this->get_ik_mutex_.exit(); 
00315 
00316   return true; 
00317 }
00318 
00319 /*  [action callbacks] */
00320 
00321 /*  [action requests] */
00322 
00323 void DarwinKinematicsAlgNode::node_config_update(Config &config, uint32_t level)
00324 {
00325   this->alg_.lock();
00326 
00327   this->alg_.unlock();
00328 }
00329 
00330 void DarwinKinematicsAlgNode::addNodeDiagnostics(void)
00331 {
00332 }
00333 
00334 /* main function */
00335 int main(int argc,char *argv[])
00336 {
00337   return algorithm_base::main<DarwinKinematicsAlgNode>(argc, argv, "darwin_kinematics_alg_node");
00338 }


iri_darwin_kinematics
Author(s): Sergi Hernandez Juan
autogenerated on Fri Dec 6 2013 20:52:58