katana: katana: KNIKinematics.cpp Source File

00001 /*
00002  * UOS-ROS packages - Robot Operating System code by the University of Osnabrück
00003  * Copyright (C) 2011  University of Osnabrück
00004  *
00005  * This program is free software; you can redistribute it and/or
00006  * modify it under the terms of the GNU General Public License
00007  * as published by the Free Software Foundation; either version 2
00008  * of the License, or (at your option) any later version.
00009  *
00010  * This program is distributed in the hope that it will be useful,
00011  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00012  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013  * GNU General Public License for more details.
00014  *
00015  * You should have received a copy of the GNU General Public License
00016  * along with this program; if not, write to the Free Software
00017  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
00018  *
00019  * KNIKinematics.cpp
00020  *
00021  *  Created on: 20.01.2011
00022  *      Author: Martin Günther
00023  */
00024 
00025 #include <katana/KNIKinematics.h>
00026 
00027 namespace katana
00028 {
00029 
00030 KNIKinematics::KNIKinematics()
00031 {
00032   joint_names_.resize(NUM_JOINTS);
00033 
00034   // ------- get parameters
00035   ros::NodeHandle pn("~");
00036   ros::NodeHandle n;
00037 
00038   std::string config_file_path;
00039 
00040   pn.param("config_file_path", config_file_path, ros::package::getPath("kni")
00041       + "/KNI_4.3.0/configfiles450/katana6M90A_G.cfg");
00042 
00043   converter_ = new KNIConverter(config_file_path);
00044 
00045   XmlRpc::XmlRpcValue joint_names;
00046 
00047   // ------- get joint names
00048   if (!n.getParam("katana_joints", joint_names))
00049   {
00050     ROS_ERROR("No joints given. (namespace: %s)", n.getNamespace().c_str());
00051   }
00052   if (joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
00053   {
00054     ROS_ERROR("Malformed joint specification.  (namespace: %s)", n.getNamespace().c_str());
00055   }
00056   if (joint_names.size() != (size_t)NUM_JOINTS)
00057   {
00058     ROS_ERROR("Wrong number of joints! was: %d, expected: %zu", joint_names.size(), NUM_JOINTS);
00059   }
00060   for (size_t i = 0; i < NUM_JOINTS; ++i)
00061   {
00062     XmlRpc::XmlRpcValue &name_value = joint_names[i];
00063     if (name_value.getType() != XmlRpc::XmlRpcValue::TypeString)
00064     {
00065       ROS_ERROR("Array of joint names should contain all strings.  (namespace: %s)",
00066           n.getNamespace().c_str());
00067     }
00068 
00069     joint_names_[i] = (std::string)name_value;
00070   }
00071 
00072 
00073   // ------- get joint limits
00074   std::string robot_desc_string;
00075   if (!n.getParam("robot_description", robot_desc_string)) {
00076     ROS_FATAL("Couldn't get a robot_description from the param server");
00077     return;
00078   }
00079 
00080   urdf::Model model;
00081   model.initString(robot_desc_string);
00082 
00083   joint_limits_.resize(joint_names_.size());
00084   for (size_t i = 0; i < joint_names_.size(); i++)
00085   {
00086     joint_limits_[i].joint_name = joint_names_[i];
00087     joint_limits_[i].has_position_limits = true;
00088     joint_limits_[i].min_position = model.getJoint(joint_names_[i])->limits->lower;
00089     joint_limits_[i].max_position = model.getJoint(joint_names_[i])->limits->upper;
00090     joint_limits_[i].has_velocity_limits = false;
00091     joint_limits_[i].has_acceleration_limits = false;
00092   }
00093 
00094 
00095   // ------- set up the KNI stuff
00096   KNI::kmlFactory config;
00097   bool success = config.openFile(config_file_path.c_str());
00098 
00099   if (!success)
00100     ROS_ERROR("Could not open config file: %s", config_file_path.c_str());
00101 
00102   ikBase_.create(&config, NULL);
00103 
00104   // ------- register services
00105   get_kinematic_solver_info_server_ = nh_.advertiseService("get_kinematic_solver_info",
00106                                                            &KNIKinematics::get_kinematic_solver_info, this);
00107 
00108   get_fk_server_ = nh_.advertiseService("get_fk", &KNIKinematics::get_position_fk, this);
00109 
00110   get_ik_server_ = nh_.advertiseService("get_ik", &KNIKinematics::get_position_ik, this);
00111 }
00112 
00113 KNIKinematics::~KNIKinematics()
00114 {
00115   delete converter_;
00116 }
00117 
00118 bool KNIKinematics::get_kinematic_solver_info(kinematics_msgs::GetKinematicSolverInfo::Request &req,
00119                                               kinematics_msgs::GetKinematicSolverInfo::Response &res)
00120 {
00121   res.kinematic_solver_info.joint_names = joint_names_;
00122   res.kinematic_solver_info.limits = joint_limits_;
00123 
00124   return true;
00125 }
00126 
00127 bool KNIKinematics::get_position_fk(kinematics_msgs::GetPositionFK::Request &req,
00128                                     kinematics_msgs::GetPositionFK::Response &res)
00129 {
00130   std::vector<double> jointAngles, coordinates;
00131 
00132   if (req.fk_link_names.size() != 1 || req.fk_link_names[0] != "katana_gripper_tool_frame")
00133   {
00134     ROS_ERROR("The KNI kinematics solver can only solve requests for katana_gripper_tool_frame!");
00135     return false;
00136   }
00137 
00138   // ignoring req.robot_state.multi_dof_joint_state
00139 
00140   std::vector<int> lookup = makeJointsLookup(req.robot_state.joint_state.name);
00141   if (lookup.size() == 0)
00142     return false;
00143 
00144   for (size_t i = 0; i < joint_names_.size(); i++)
00145   {
00146     jointAngles.push_back(req.robot_state.joint_state.position[lookup[i]]);
00147   }
00148 
00149   coordinates = getCoordinates(jointAngles);
00150 
00151   geometry_msgs::PoseStamped pose_in, pose_out;
00152 
00153   pose_in.header.frame_id = "katana_base_frame";
00154   pose_in.header.stamp = ros::Time(0);
00155 
00156   pose_in.pose.position.x = coordinates[0];
00157   pose_in.pose.position.y = coordinates[1];
00158   pose_in.pose.position.z = coordinates[2];
00159 
00160   pose_in.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(coordinates[3], coordinates[4], coordinates[5]);
00161 
00162   // The frame_id in the header message is the frame in which
00163   // the forward kinematics poses will be returned
00164   try
00165   {
00166     bool success = tf_listener_.waitForTransform(req.header.frame_id, pose_in.header.frame_id, pose_in.header.stamp,
00167                                                  ros::Duration(1.0));
00168 
00169     if (!success)
00170     {
00171       ROS_ERROR("Could not get transform");
00172       return false;
00173     }
00174 
00175     tf_listener_.transformPose(req.header.frame_id, pose_in, pose_out);
00176   }
00177   catch (const tf::TransformException &ex)
00178   {
00179     ROS_ERROR("%s", ex.what());
00180     return false;
00181   }
00182 
00183   res.pose_stamped.push_back(pose_out);
00184   res.fk_link_names = req.fk_link_names;
00185   res.error_code.val = res.error_code.SUCCESS;
00186 
00187   return true;
00188 }
00189 
00190 bool KNIKinematics::get_position_ik(kinematics_msgs::GetPositionIK::Request &req,
00191                                     kinematics_msgs::GetPositionIK::Response &res)
00192 {
00193   std::vector<double> kni_coordinates(6, 0.0);
00194   std::vector<int> solution(NUM_JOINTS + 1);
00195   std::vector<int> seed_encoders(NUM_JOINTS + 1);
00196 
00197   if (req.ik_request.ik_link_name != "katana_gripper_tool_frame")
00198   {
00199     ROS_ERROR("The KNI kinematics solver can only solve requests for katana_gripper_tool_frame!");
00200     return false;
00201   }
00202 
00203   // ------- convert req.ik_request.ik_seed_state into seed_encoders
00204   std::vector<int> lookup = makeJointsLookup(req.ik_request.ik_seed_state.joint_state.name);
00205   if (lookup.size() == 0)
00206     return false;
00207 
00208   for (size_t i = 0; i < joint_names_.size(); i++)
00209   {
00210     seed_encoders[i] = converter_->angle_rad2enc(i, req.ik_request.ik_seed_state.joint_state.position[lookup[i]]);
00211   }
00212 
00213   // ------- convert req.ik_request.pose_stamped into kni_coordinates
00214   geometry_msgs::PoseStamped pose_out;
00215   try
00216   {
00217     bool success = tf_listener_.waitForTransform("katana_base_frame", req.ik_request.pose_stamped.header.frame_id, req.ik_request.pose_stamped.header.stamp,
00218                                                  ros::Duration(1.0));
00219 
00220     if (!success)
00221     {
00222       ROS_ERROR("Could not get transform");
00223       return false;
00224     }
00225 
00226     tf_listener_.transformPose("katana_base_frame", req.ik_request.pose_stamped, pose_out);
00227   }
00228   catch (const tf::TransformException &ex)
00229   {
00230     ROS_ERROR("%s", ex.what());
00231     return false;
00232   }
00233 
00234   kni_coordinates[0] = pose_out.pose.position.x / KNI_TO_ROS_LENGTH;
00235   kni_coordinates[1] = pose_out.pose.position.y / KNI_TO_ROS_LENGTH;
00236   kni_coordinates[2] = pose_out.pose.position.z / KNI_TO_ROS_LENGTH;
00237 
00238   btScalar roll, pitch, yaw;
00239   btQuaternion bt_q;
00240   tf::quaternionMsgToTF(pose_out.pose.orientation, bt_q);
00241   btMatrix3x3(bt_q).getRPY(roll, pitch,yaw);
00242 
00243   EulerTransformationMatrices::zyx_to_zxz_angles(yaw, pitch, roll, kni_coordinates[3], kni_coordinates[4], kni_coordinates[5]);
00244 
00245   try
00246   {
00247     ikBase_.IKCalculate(kni_coordinates[0], kni_coordinates[1], kni_coordinates[2], kni_coordinates[3],
00248                         kni_coordinates[4], kni_coordinates[5], solution.begin(), seed_encoders);
00249   }
00250   catch (const KNI::NoSolutionException &e)
00251   {
00252     res.error_code.val = res.error_code.NO_IK_SOLUTION;
00253     return true; // this means that res is valid; the error code is stored inside
00254   }
00255 
00256 
00257   // ------- convert solution into radians
00258   res.solution.joint_state.name = joint_names_;
00259   res.solution.joint_state.position.resize(NUM_JOINTS);
00260   for (size_t i = 0; i < NUM_JOINTS; i++) {
00261     res.solution.joint_state.position[i] = converter_->angle_enc2rad(i, solution[i]);
00262   }
00263 
00264   res.error_code.val = res.error_code.SUCCESS;
00265   return true;
00266 }
00267 
00269 std::vector<int> KNIKinematics::makeJointsLookup(std::vector<std::string> &joint_names)
00270 {
00271   std::vector<int> lookup(joint_names_.size(), -1); // Maps from an index in joint_names_ to an index in the msg
00272   for (size_t j = 0; j < joint_names_.size(); ++j)
00273   {
00274     for (size_t k = 0; k < joint_names.size(); ++k)
00275     {
00276       if (joint_names[k] == joint_names_[j])
00277       {
00278         lookup[j] = k;
00279         break;
00280       }
00281     }
00282 
00283     if (lookup[j] == -1)
00284     {
00285       ROS_ERROR("Unable to locate joint %s in the commanded trajectory.", joint_names_[j].c_str());
00286       return std::vector<int>(); // return empty vector to signal error
00287     }
00288   }
00289 
00290   return lookup;
00291 }
00292 
00299 std::vector<double> KNIKinematics::getCoordinates(std::vector<double> jointAngles)
00300 {
00301   std::vector<double> result(6, 0.0);
00302   std::vector<double> pose(6, 0.0);
00303   std::vector<int> encoders(NUM_JOINTS + 1, 0.0); // must be of size NUM_JOINTS + 1, otherwise KNI crashes
00304   for (size_t i = 0; i < jointAngles.size(); i++)
00305     encoders[i] = converter_->angle_rad2enc(i, jointAngles[i]);
00306 
00307   ikBase_.getCoordinatesFromEncoders(pose, encoders);
00308 
00309   // zyx = yaw, pitch, roll = result[5], result[4], result[3]
00310   EulerTransformationMatrices::zxz_to_zyx_angles(pose[3], pose[4], pose[5], result[5], result[4], result[3]);
00311 
00312   result[0] = pose[0] * KNI_TO_ROS_LENGTH;
00313   result[1] = pose[1] * KNI_TO_ROS_LENGTH;
00314   result[2] = pose[2] * KNI_TO_ROS_LENGTH;
00315 
00316   return result;
00317 }
00318 
00319 } // end namespace
00320 
00321 int main(int argc, char** argv)
00322 {
00323   ros::init(argc, argv, "katana_arm_kinematics");
00324 
00325   katana::KNIKinematics node;
00326 
00327   ros::spin();
00328   return 0;
00329 }