KNIKinematics.cpp
Go to the documentation of this file.
1 /*
2  * UOS-ROS packages - Robot Operating System code by the University of Osnabrück
3  * Copyright (C) 2011 University of Osnabrück
4  *
5  * This program is free software; you can redistribute it and/or
6  * modify it under the terms of the GNU General Public License
7  * as published by the Free Software Foundation; either version 2
8  * of the License, or (at your option) any later version.
9  *
10  * This program is distributed in the hope that it will be useful,
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program; if not, write to the Free Software
17  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
18  *
19  * KNIKinematics.cpp
20  *
21  * Created on: 20.01.2011
22  * Author: Martin Günther
23  */
24 
25 #include <katana/KNIKinematics.h>
26 
27 namespace katana
28 {
29 
31 {
32  joint_names_.resize(NUM_JOINTS);
33 
34  // ------- get parameters
35  ros::NodeHandle pn("~");
37 
38  std::string config_file_path;
39 
40  pn.param("config_file_path", config_file_path, ros::package::getPath("kni")
41  + "/KNI_4.3.0/configfiles450/katana6M90A_G.cfg");
42 
43  converter_ = new KNIConverter(config_file_path);
44 
45  XmlRpc::XmlRpcValue joint_names;
46 
47  // ------- get joint names
48  if (!n.getParam("katana_joints", joint_names))
49  {
50  ROS_ERROR("No joints given. (namespace: %s)", n.getNamespace().c_str());
51  }
52  if (joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
53  {
54  ROS_ERROR("Malformed joint specification. (namespace: %s)", n.getNamespace().c_str());
55  }
56  if (joint_names.size() != (size_t)NUM_JOINTS)
57  {
58  ROS_ERROR("Wrong number of joints! was: %d, expected: %zu", joint_names.size(), NUM_JOINTS);
59  }
60  for (size_t i = 0; i < NUM_JOINTS; ++i)
61  {
62  XmlRpc::XmlRpcValue &name_value = joint_names[i];
63  if (name_value.getType() != XmlRpc::XmlRpcValue::TypeString)
64  {
65  ROS_ERROR("Array of joint names should contain all strings. (namespace: %s)",
66  n.getNamespace().c_str());
67  }
68 
69  joint_names_[i] = (std::string)name_value;
70  }
71 
72 
73  // ------- get joint limits
74  std::string robot_desc_string;
75  if (!n.getParam("robot_description", robot_desc_string)) {
76  ROS_FATAL("Couldn't get a robot_description from the param server");
77  return;
78  }
79 
80  urdf::Model model;
81  model.initString(robot_desc_string);
82 
83  joint_limits_.resize(joint_names_.size());
84  for (size_t i = 0; i < joint_names_.size(); i++)
85  {
86  joint_limits_[i].joint_name = joint_names_[i];
87  joint_limits_[i].has_position_limits = true;
88  joint_limits_[i].min_position = model.getJoint(joint_names_[i])->limits->lower;
89  joint_limits_[i].max_position = model.getJoint(joint_names_[i])->limits->upper;
90  joint_limits_[i].has_velocity_limits = false;
91  joint_limits_[i].has_acceleration_limits = false;
92  }
93 
94 
95  // ------- set up the KNI stuff
96  KNI::kmlFactory config;
97  bool success = config.openFile(config_file_path.c_str());
98 
99  if (!success)
100  ROS_ERROR("Could not open config file: %s", config_file_path.c_str());
101 
102  ikBase_.create(&config, NULL);
103 
104  // ------- register services
106 
108 }
109 
111 {
112  delete converter_;
113 }
114 
115 bool KNIKinematics::get_position_fk(moveit_msgs::GetPositionFK::Request &req,
116  moveit_msgs::GetPositionFK::Response &res)
117 {
118  std::vector<double> jointAngles, coordinates;
119 
120  if (req.fk_link_names.size() != 1 || req.fk_link_names[0] != "katana_gripper_tool_frame")
121  {
122  ROS_ERROR("The KNI kinematics solver can only solve requests for katana_gripper_tool_frame!");
123  return false;
124  }
125 
126  // ignoring req.robot_state.multi_dof_joint_state
127 
128  std::vector<int> lookup = makeJointsLookup(req.robot_state.joint_state.name);
129  if (lookup.size() == 0)
130  return false;
131 
132  for (size_t i = 0; i < joint_names_.size(); i++)
133  {
134  jointAngles.push_back(req.robot_state.joint_state.position[lookup[i]]);
135  }
136 
137  coordinates = getCoordinates(jointAngles);
138 
139  geometry_msgs::PoseStamped pose_in, pose_out;
140 
141  pose_in.header.frame_id = "katana_base_frame";
142  pose_in.header.stamp = ros::Time(0);
143 
144  pose_in.pose.position.x = coordinates[0];
145  pose_in.pose.position.y = coordinates[1];
146  pose_in.pose.position.z = coordinates[2];
147 
148  pose_in.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(coordinates[3], coordinates[4], coordinates[5]);
149 
150  // The frame_id in the header message is the frame in which
151  // the forward kinematics poses will be returned
152  try
153  {
154  bool success = tf_listener_.waitForTransform(req.header.frame_id, pose_in.header.frame_id, pose_in.header.stamp,
155  ros::Duration(1.0));
156 
157  if (!success)
158  {
159  ROS_ERROR("Could not get transform");
160  return false;
161  }
162 
163  tf_listener_.transformPose(req.header.frame_id, pose_in, pose_out);
164  }
165  catch (const tf::TransformException &ex)
166  {
167  ROS_ERROR("%s", ex.what());
168  return false;
169  }
170 
171  res.pose_stamped.push_back(pose_out);
172  res.fk_link_names = req.fk_link_names;
173  res.error_code.val = res.error_code.SUCCESS;
174 
175  return true;
176 }
177 
178 bool KNIKinematics::get_position_ik(moveit_msgs::GetPositionIK::Request &req,
179  moveit_msgs::GetPositionIK::Response &res)
180 {
181  std::vector<double> kni_coordinates(6, 0.0);
182  std::vector<int> solution(NUM_JOINTS + 1);
183  std::vector<int> seed_encoders(NUM_JOINTS + 1);
184 
185  if (req.ik_request.ik_link_name != "katana_gripper_tool_frame")
186  {
187  ROS_ERROR("The KNI kinematics solver can only solve requests for katana_gripper_tool_frame!");
188  return false;
189  }
190 
191  // ------- convert req.ik_request.ik_seed_state into seed_encoders
192  std::vector<int> lookup = makeJointsLookup(req.ik_request.robot_state.joint_state.name);
193  if (lookup.size() == 0)
194  return false;
195 
196  for (size_t i = 0; i < joint_names_.size(); i++)
197  {
198  seed_encoders[i] = converter_->angle_rad2enc(i, req.ik_request.robot_state.joint_state.position[lookup[i]]);
199  }
200 
201  // ------- convert req.ik_request.pose_stamped into kni_coordinates
202  geometry_msgs::PoseStamped pose_out;
203  try
204  {
205  bool success = tf_listener_.waitForTransform("katana_base_frame", req.ik_request.pose_stamped.header.frame_id, req.ik_request.pose_stamped.header.stamp,
206  ros::Duration(1.0));
207 
208  if (!success)
209  {
210  ROS_ERROR("Could not get transform");
211  return false;
212  }
213 
214  tf_listener_.transformPose("katana_base_frame", req.ik_request.pose_stamped, pose_out);
215  }
216  catch (const tf::TransformException &ex)
217  {
218  ROS_ERROR("%s", ex.what());
219  return false;
220  }
221 
222  kni_coordinates[0] = pose_out.pose.position.x / KNI_TO_ROS_LENGTH;
223  kni_coordinates[1] = pose_out.pose.position.y / KNI_TO_ROS_LENGTH;
224  kni_coordinates[2] = pose_out.pose.position.z / KNI_TO_ROS_LENGTH;
225 
226  tfScalar roll, pitch, yaw;
227  tf::Quaternion bt_q;
228  tf::quaternionMsgToTF(pose_out.pose.orientation, bt_q);
229  tf::Matrix3x3(bt_q).getRPY(roll, pitch,yaw);
230 
231  EulerTransformationMatrices::zyx_to_zxz_angles(yaw, pitch, roll, kni_coordinates[3], kni_coordinates[4], kni_coordinates[5]);
232 
233  try
234  {
235  ikBase_.IKCalculate(kni_coordinates[0], kni_coordinates[1], kni_coordinates[2], kni_coordinates[3],
236  kni_coordinates[4], kni_coordinates[5], solution.begin(), seed_encoders);
237  }
238  catch (const KNI::NoSolutionException &e)
239  {
240  res.error_code.val = res.error_code.NO_IK_SOLUTION;
241  return true; // this means that res is valid; the error code is stored inside
242  }
243 
244 
245  // ------- convert solution into radians
246  res.solution.joint_state.name = joint_names_;
247  res.solution.joint_state.position.resize(NUM_JOINTS);
248  for (size_t i = 0; i < NUM_JOINTS; i++) {
249  res.solution.joint_state.position[i] = converter_->angle_enc2rad(i, solution[i]);
250  }
251 
252  res.error_code.val = res.error_code.SUCCESS;
253  return true;
254 }
255 
257 std::vector<int> KNIKinematics::makeJointsLookup(std::vector<std::string> &joint_names)
258 {
259  std::vector<int> lookup(joint_names_.size(), -1); // Maps from an index in joint_names_ to an index in the msg
260  for (size_t j = 0; j < joint_names_.size(); ++j)
261  {
262  for (size_t k = 0; k < joint_names.size(); ++k)
263  {
264  if (joint_names[k] == joint_names_[j])
265  {
266  lookup[j] = k;
267  break;
268  }
269  }
270 
271  if (lookup[j] == -1)
272  {
273  ROS_ERROR("Unable to locate joint %s in the commanded trajectory.", joint_names_[j].c_str());
274  return std::vector<int>(); // return empty vector to signal error
275  }
276  }
277 
278  return lookup;
279 }
280 
287 std::vector<double> KNIKinematics::getCoordinates(std::vector<double> jointAngles)
288 {
289  std::vector<double> result(6, 0.0);
290  std::vector<double> pose(6, 0.0);
291  std::vector<int> encoders(NUM_JOINTS + 1, 0.0); // must be of size NUM_JOINTS + 1, otherwise KNI crashes
292  for (size_t i = 0; i < jointAngles.size(); i++)
293  encoders[i] = converter_->angle_rad2enc(i, jointAngles[i]);
294 
295  ikBase_.getCoordinatesFromEncoders(pose, encoders);
296 
297  // zyx = yaw, pitch, roll = result[5], result[4], result[3]
298  EulerTransformationMatrices::zxz_to_zyx_angles(pose[3], pose[4], pose[5], result[5], result[4], result[3]);
299 
300  result[0] = pose[0] * KNI_TO_ROS_LENGTH;
301  result[1] = pose[1] * KNI_TO_ROS_LENGTH;
302  result[2] = pose[2] * KNI_TO_ROS_LENGTH;
303 
304  return result;
305 }
306 
307 } // end namespace
308 
309 int main(int argc, char** argv)
310 {
311  ros::init(argc, argv, "katana_arm_kinematics");
312 
314 
315  ros::spin();
316  return 0;
317 }
bool get_position_fk(moveit_msgs::GetPositionFK::Request &req, moveit_msgs::GetPositionFK::Response &res)
#define ROS_FATAL(...)
URDF_EXPORT bool initString(const std::string &xmlstring)
bool openFile(const char *filepath)
double tfScalar
int size() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
int main(int argc, char **argv)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
FloatVector * pose
Type const & getType() const
ROSCPP_DECL void spin(Spinner &spinner)
bool waitForTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
bool get_position_ik(moveit_msgs::GetPositionIK::Request &req, moveit_msgs::GetPositionIK::Response &res)
ros::ServiceServer get_fk_server_
Definition: KNIKinematics.h:57
double angle_rad2enc(int index, double angle)
static void quaternionMsgToTF(const geometry_msgs::Quaternion &msg, Quaternion &bt)
void getRPY(tfScalar &roll, tfScalar &pitch, tfScalar &yaw, unsigned int solution_number=1) const
bool param(const std::string &param_name, T &param_val, const T &default_val) const
static void zyx_to_zxz_angles(const double in_a, const double in_e, const double in_r, double &out_a, double &out_e, double &out_r)
double angle_enc2rad(int index, int encoders)
const std::string & getNamespace() const
tf::TransformListener tf_listener_
Definition: KNIKinematics.h:65
std::vector< std::string > joint_names_
Definition: KNIKinematics.h:60
void getCoordinatesFromEncoders(std::vector< double > &pose, const std::vector< int > &encs)
ROSLIB_DECL std::string getPath(const std::string &package_name)
void create(const char *configurationFile, CCplBase *protocol)
KNIConverter * converter_
Definition: KNIKinematics.h:64
void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const
ros::ServiceServer get_ik_server_
Definition: KNIKinematics.h:58
bool getParam(const std::string &key, std::string &s) const
static geometry_msgs::Quaternion createQuaternionMsgFromRollPitchYaw(double roll, double pitch, double yaw)
void IKCalculate(double X, double Y, double Z, double Al, double Be, double Ga, std::vector< int >::iterator solution_iter)
ros::NodeHandle nh_
Definition: KNIKinematics.h:56
const size_t NUM_JOINTS
The number of joints in the katana (= only the 5 "real" joints)
std::vector< double > getCoordinates()
static const double KNI_TO_ROS_LENGTH
the conversion factor from KNI coordinates (in mm) to ROS coordinates (in m)
static void zxz_to_zyx_angles(const double in_a, const double in_e, const double in_r, double &out_a, double &out_e, double &out_r)
#define ROS_ERROR(...)
std::vector< int > encoders
std::vector< moveit_msgs::JointLimits > joint_limits_
Definition: KNIKinematics.h:61
std::vector< int > makeJointsLookup(std::vector< std::string > &joint_names)
copied from joint_trajectory_action_controller


katana
Author(s): Martin Günther
autogenerated on Fri Jan 3 2020 04:01:25