KNIKinematics.cpp
Go to the documentation of this 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_fk_server_ = nh_.advertiseService("get_fk", &KNIKinematics::get_position_fk, this);
00106 
00107   get_ik_server_ = nh_.advertiseService("get_ik", &KNIKinematics::get_position_ik, this);
00108 }
00109 
00110 KNIKinematics::~KNIKinematics()
00111 {
00112   delete converter_;
00113 }
00114 
00115 bool KNIKinematics::get_position_fk(moveit_msgs::GetPositionFK::Request &req,
00116                                     moveit_msgs::GetPositionFK::Response &res)
00117 {
00118   std::vector<double> jointAngles, coordinates;
00119 
00120   if (req.fk_link_names.size() != 1 || req.fk_link_names[0] != "katana_gripper_tool_frame")
00121   {
00122     ROS_ERROR("The KNI kinematics solver can only solve requests for katana_gripper_tool_frame!");
00123     return false;
00124   }
00125 
00126   // ignoring req.robot_state.multi_dof_joint_state
00127 
00128   std::vector<int> lookup = makeJointsLookup(req.robot_state.joint_state.name);
00129   if (lookup.size() == 0)
00130     return false;
00131 
00132   for (size_t i = 0; i < joint_names_.size(); i++)
00133   {
00134     jointAngles.push_back(req.robot_state.joint_state.position[lookup[i]]);
00135   }
00136 
00137   coordinates = getCoordinates(jointAngles);
00138 
00139   geometry_msgs::PoseStamped pose_in, pose_out;
00140 
00141   pose_in.header.frame_id = "katana_base_frame";
00142   pose_in.header.stamp = ros::Time(0);
00143 
00144   pose_in.pose.position.x = coordinates[0];
00145   pose_in.pose.position.y = coordinates[1];
00146   pose_in.pose.position.z = coordinates[2];
00147 
00148   pose_in.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(coordinates[3], coordinates[4], coordinates[5]);
00149 
00150   // The frame_id in the header message is the frame in which
00151   // the forward kinematics poses will be returned
00152   try
00153   {
00154     bool success = tf_listener_.waitForTransform(req.header.frame_id, pose_in.header.frame_id, pose_in.header.stamp,
00155                                                  ros::Duration(1.0));
00156 
00157     if (!success)
00158     {
00159       ROS_ERROR("Could not get transform");
00160       return false;
00161     }
00162 
00163     tf_listener_.transformPose(req.header.frame_id, pose_in, pose_out);
00164   }
00165   catch (const tf::TransformException &ex)
00166   {
00167     ROS_ERROR("%s", ex.what());
00168     return false;
00169   }
00170 
00171   res.pose_stamped.push_back(pose_out);
00172   res.fk_link_names = req.fk_link_names;
00173   res.error_code.val = res.error_code.SUCCESS;
00174 
00175   return true;
00176 }
00177 
00178 bool KNIKinematics::get_position_ik(moveit_msgs::GetPositionIK::Request &req,
00179                                     moveit_msgs::GetPositionIK::Response &res)
00180 {
00181   std::vector<double> kni_coordinates(6, 0.0);
00182   std::vector<int> solution(NUM_JOINTS + 1);
00183   std::vector<int> seed_encoders(NUM_JOINTS + 1);
00184 
00185   if (req.ik_request.ik_link_name != "katana_gripper_tool_frame")
00186   {
00187     ROS_ERROR("The KNI kinematics solver can only solve requests for katana_gripper_tool_frame!");
00188     return false;
00189   }
00190 
00191   // ------- convert req.ik_request.ik_seed_state into seed_encoders
00192   std::vector<int> lookup = makeJointsLookup(req.ik_request.robot_state.joint_state.name);
00193   if (lookup.size() == 0)
00194     return false;
00195 
00196   for (size_t i = 0; i < joint_names_.size(); i++)
00197   {
00198     seed_encoders[i] = converter_->angle_rad2enc(i, req.ik_request.robot_state.joint_state.position[lookup[i]]);
00199   }
00200 
00201   // ------- convert req.ik_request.pose_stamped into kni_coordinates
00202   geometry_msgs::PoseStamped pose_out;
00203   try
00204   {
00205     bool success = tf_listener_.waitForTransform("katana_base_frame", req.ik_request.pose_stamped.header.frame_id, req.ik_request.pose_stamped.header.stamp,
00206                                                  ros::Duration(1.0));
00207 
00208     if (!success)
00209     {
00210       ROS_ERROR("Could not get transform");
00211       return false;
00212     }
00213 
00214     tf_listener_.transformPose("katana_base_frame", req.ik_request.pose_stamped, pose_out);
00215   }
00216   catch (const tf::TransformException &ex)
00217   {
00218     ROS_ERROR("%s", ex.what());
00219     return false;
00220   }
00221 
00222   kni_coordinates[0] = pose_out.pose.position.x / KNI_TO_ROS_LENGTH;
00223   kni_coordinates[1] = pose_out.pose.position.y / KNI_TO_ROS_LENGTH;
00224   kni_coordinates[2] = pose_out.pose.position.z / KNI_TO_ROS_LENGTH;
00225 
00226   tfScalar roll, pitch, yaw;
00227   tf::Quaternion bt_q;
00228   tf::quaternionMsgToTF(pose_out.pose.orientation, bt_q);
00229   tf::Matrix3x3(bt_q).getRPY(roll, pitch,yaw);
00230 
00231   EulerTransformationMatrices::zyx_to_zxz_angles(yaw, pitch, roll, kni_coordinates[3], kni_coordinates[4], kni_coordinates[5]);
00232 
00233   try
00234   {
00235     ikBase_.IKCalculate(kni_coordinates[0], kni_coordinates[1], kni_coordinates[2], kni_coordinates[3],
00236                         kni_coordinates[4], kni_coordinates[5], solution.begin(), seed_encoders);
00237   }
00238   catch (const KNI::NoSolutionException &e)
00239   {
00240     res.error_code.val = res.error_code.NO_IK_SOLUTION;
00241     return true; // this means that res is valid; the error code is stored inside
00242   }
00243 
00244 
00245   // ------- convert solution into radians
00246   res.solution.joint_state.name = joint_names_;
00247   res.solution.joint_state.position.resize(NUM_JOINTS);
00248   for (size_t i = 0; i < NUM_JOINTS; i++) {
00249     res.solution.joint_state.position[i] = converter_->angle_enc2rad(i, solution[i]);
00250   }
00251 
00252   res.error_code.val = res.error_code.SUCCESS;
00253   return true;
00254 }
00255 
00257 std::vector<int> KNIKinematics::makeJointsLookup(std::vector<std::string> &joint_names)
00258 {
00259   std::vector<int> lookup(joint_names_.size(), -1); // Maps from an index in joint_names_ to an index in the msg
00260   for (size_t j = 0; j < joint_names_.size(); ++j)
00261   {
00262     for (size_t k = 0; k < joint_names.size(); ++k)
00263     {
00264       if (joint_names[k] == joint_names_[j])
00265       {
00266         lookup[j] = k;
00267         break;
00268       }
00269     }
00270 
00271     if (lookup[j] == -1)
00272     {
00273       ROS_ERROR("Unable to locate joint %s in the commanded trajectory.", joint_names_[j].c_str());
00274       return std::vector<int>(); // return empty vector to signal error
00275     }
00276   }
00277 
00278   return lookup;
00279 }
00280 
00287 std::vector<double> KNIKinematics::getCoordinates(std::vector<double> jointAngles)
00288 {
00289   std::vector<double> result(6, 0.0);
00290   std::vector<double> pose(6, 0.0);
00291   std::vector<int> encoders(NUM_JOINTS + 1, 0.0); // must be of size NUM_JOINTS + 1, otherwise KNI crashes
00292   for (size_t i = 0; i < jointAngles.size(); i++)
00293     encoders[i] = converter_->angle_rad2enc(i, jointAngles[i]);
00294 
00295   ikBase_.getCoordinatesFromEncoders(pose, encoders);
00296 
00297   // zyx = yaw, pitch, roll = result[5], result[4], result[3]
00298   EulerTransformationMatrices::zxz_to_zyx_angles(pose[3], pose[4], pose[5], result[5], result[4], result[3]);
00299 
00300   result[0] = pose[0] * KNI_TO_ROS_LENGTH;
00301   result[1] = pose[1] * KNI_TO_ROS_LENGTH;
00302   result[2] = pose[2] * KNI_TO_ROS_LENGTH;
00303 
00304   return result;
00305 }
00306 
00307 } // end namespace
00308 
00309 int main(int argc, char** argv)
00310 {
00311   ros::init(argc, argv, "katana_arm_kinematics");
00312 
00313   katana::KNIKinematics node;
00314 
00315   ros::spin();
00316   return 0;
00317 }


katana
Author(s): Martin Günther
autogenerated on Mon Aug 14 2017 02:45:49