kdl_manager.cpp
Go to the documentation of this file.
2 
4 {
5 KDLManager::KDLManager(const std::string &chain_base_link, ros::NodeHandle nh)
6  : chain_base_link_(chain_base_link), nh_(nh)
7 {
8  if (!model_.initParam("/robot_description"))
9  {
10  throw std::runtime_error(
11  "ERROR getting robot description (/robot_description)");
12  }
13 
14  getParam();
15 }
16 
18 
20 {
21  if (!nh_.getParam("kdl_manager/eps", eps_))
22  {
23  ROS_WARN("KDLManager: Missing eps parameter, setting default");
24  eps_ = 0.001;
25  }
26 
27  if (!nh_.getParam("kdl_manager/max_tf_attempts", max_tf_attempts_))
28  {
29  ROS_WARN("KDLManager: Missing max_tf_attempts parameter, setting default");
30  max_tf_attempts_ = 5;
31  }
32 
33  if (!nh_.getParam("kdl_manager/ikvel_solver", ikvel_solver_))
34  {
35  ROS_WARN("KDLManager: Missing ikvel_solver parameter, setting default");
37  }
38 
39  if (!nh_.getParam("kdl_manager/ik_angle_tolerance", ik_angle_tolerance_))
40  {
41  ROS_WARN(
42  "KDLManager: Missing ik_angle_tolerance parameter, setting default");
43  ik_angle_tolerance_ = 0.01;
44  }
45 
46  if (!nh_.getParam("kdl_manager/ik_pos_tolerance", ik_pos_tolerance_))
47  {
48  ROS_WARN("KDLManager: Missing ik_pos_tolerance parameter, setting default");
49  ik_pos_tolerance_ = 0.005;
50  }
51 
53  {
54  ROS_ERROR_STREAM("KDLManager: ikvel_solver has value "
55  << ikvel_solver_ << "but admissible values are "
56  << WDLS_SOLVER << " and " << NSO_SOLVER);
57  ROS_WARN_STREAM("KDLManager: setting ikvel_solver to " << WDLS_SOLVER);
59  }
60 
62  {
63  if (!nh_.getParam("kdl_manager/nso_weight", nso_weight_))
64  {
65  ROS_WARN("KDLManager: Missing nso_weight parameter, setting default");
66  nso_weight_ = 4;
67  }
68  }
69 
70  std::vector<double> gravity;
71  if (!nh_.getParam("kdl_manager/gravity_in_base_link", gravity))
72  {
74  "KDLManager: Missing kdl_manager/gravity_in_base_link parameter. This "
75  "will affect the dynamic solvers");
77  }
78  else
79  {
80  if (gravity.size() != 3)
81  {
82  ROS_WARN_STREAM("KDLManager: Got gravity vector of size "
83  << gravity.size() << ". Should have size 3");
85  }
86  else
87  {
89  KDL::Vector(gravity[0], gravity[1], gravity[2]);
90  }
91  }
92 
93  return true;
94 }
95 
96 bool KDLManager::initializeArm(const std::string &end_effector_link)
97 {
98  if (!initializeArmCommon(end_effector_link))
99  {
100  return false;
101  }
102 
103  if (chain_.find(end_effector_link) == chain_.end())
104  {
105  return false;
106  }
107 
108  if (ikvel_solver_ == WDLS_SOLVER)
109  {
110  ikvel_[end_effector_link] = IkSolverVelPtr(
111  new KDL::ChainIkSolverVel_wdls(chain_.at(end_effector_link), eps_));
112  }
113  else
114  {
115  unsigned int joint_n = chain_.at(end_effector_link).getNrOfJoints();
116 
117  if (joint_n < 6)
118  {
119  ROS_WARN(
120  "Number of joints for kinematic chain is smaller than 6 (%d). The "
121  "NSO ik solver vel has issues with under-actuated chains. Using WDLS",
122  joint_n);
123  ikvel_[end_effector_link] = IkSolverVelPtr(
124  new KDL::ChainIkSolverVel_wdls(chain_.at(end_effector_link), eps_));
125  }
126  else
127  {
128  KDL::JntArray w(joint_n), q_min(joint_n), q_max(joint_n),
129  q_vel_lim(joint_n), q_desired(joint_n);
130  getJointLimits(end_effector_link, q_min, q_max, q_vel_lim);
131 
132  for (unsigned int i = 0; i < joint_n; i++)
133  {
134  w(i) = nso_weight_;
135  q_desired(i) = (q_max(i) + q_min(i)) / 2;
136  }
137 
138  ikvel_[end_effector_link] =
140  chain_.at(end_effector_link), q_desired, w, eps_));
141  }
142  }
143 
144  return true;
145 }
146 
147 bool KDLManager::isInitialized(const std::string &end_effector_link) const
148 {
149  return (chain_.find(end_effector_link) != chain_.end());
150 }
151 
152 bool KDLManager::initializeArmCommon(const std::string &end_effector_link)
153 {
154  if (chain_.find(end_effector_link) != chain_.end())
155  {
156  ROS_ERROR_STREAM("Tried to initialize arm "
157  << end_effector_link
158  << ", but it was already initialized");
159  return false;
160  }
161 
162  KDL::Tree tree;
163  KDL::Joint kdl_joint;
164  KDL::Chain chain;
166  model_, tree); // convert URDF description of the robot into a KDL tree
167  if (!tree.getChain(chain_base_link_, end_effector_link, chain))
168  {
169  ROS_ERROR_STREAM("Failed to find chain <" << chain_base_link_ << ", "
170  << end_effector_link
171  << "> in the kinematic tree");
172  ROS_DEBUG("Existing tree: ");
173  auto tree_map = tree.getSegments();
174  for (auto it = tree_map.begin(); it != tree_map.end(); it++)
175  {
176  ROS_DEBUG_STREAM(it->first);
177  }
178  return false;
179  }
180 
181  // Ready to accept the end-effector as valid
182  chain_[end_effector_link] = chain;
183  dynamic_chain_[end_effector_link] = ChainDynParamPtr(new KDL::ChainDynParam(
184  chain_.at(end_effector_link), gravity_in_chain_base_link_));
185  std::vector<std::string> new_vector;
186 
187  ROS_DEBUG_STREAM("Initializing chain for arm " << end_effector_link);
188  for (unsigned int i = 0; i < chain.getNrOfSegments();
189  i++) // check for non-movable joints
190  {
191  kdl_joint = chain.getSegment(i).getJoint();
192 
193  if (kdl_joint.getTypeName() == "None")
194  {
195  continue;
196  }
197 
198  ROS_DEBUG_STREAM(kdl_joint.getName());
199  new_vector.push_back(kdl_joint.getName());
200  }
201 
202  actuated_joint_names_[end_effector_link] = new_vector;
203 
204  // Initialize solvers
205  fkpos_[end_effector_link] =
206  FkSolverPosPtr(new FkSolverPos(chain_.at(end_effector_link)));
207  fkvel_[end_effector_link] =
208  FkSolverVelPtr(new FkSolverVel(chain_.at(end_effector_link)));
209  ikpos_[end_effector_link] =
210  IkSolverPosPtr(new IkSolverPos(chain_.at(end_effector_link)));
211  jac_solver_[end_effector_link] =
212  JacSolverPtr(new JacSolver(chain_.at(end_effector_link)));
213  eef_to_gripping_point_[end_effector_link] =
214  KDL::Frame::Identity(); // Initialize a neutral transform.
215  eef_to_sensor_point_[end_effector_link] =
216  KDL::Frame::Identity(); // Initialize a neutral transform.
217 
218  return true;
219 }
220 
221 bool KDLManager::setGrippingPoint(const std::string &end_effector_link,
222  const std::string &gripping_point_frame)
223 {
224  if (chain_.find(end_effector_link) == chain_.end())
225  {
226  return false;
227  }
228 
229  KDL::Frame eef_to_gripping_point;
230 
231  if (!getRigidTransform(end_effector_link, gripping_point_frame,
232  eef_to_gripping_point))
233  {
234  return false;
235  }
236 
237  eef_to_gripping_point_.at(end_effector_link) =
238  eef_to_gripping_point.Inverse();
239  return true;
240 }
241 
242 bool KDLManager::setSensorPoint(const std::string &end_effector_link,
243  const std::string &sensor_point_frame)
244 {
245  if (chain_.find(end_effector_link) == chain_.end())
246  {
247  return false;
248  }
249 
250  KDL::Frame eef_to_sensor_point;
251 
252  if (!getRigidTransform(end_effector_link, sensor_point_frame,
253  eef_to_sensor_point))
254  {
255  return false;
256  }
257 
258  eef_to_sensor_point_.at(end_effector_link) = eef_to_sensor_point;
259  return true;
260 }
261 
262 bool KDLManager::getJointState(const std::string &end_effector_link,
263  const Eigen::VectorXd &qdot,
264  sensor_msgs::JointState &state) const
265 {
266  if (chain_.find(end_effector_link) == chain_.end())
267  {
268  return false;
269  }
270 
271  if (chain_.at(end_effector_link).getNrOfJoints() != qdot.rows())
272  {
273  ROS_ERROR(
274  "Joint chain for eef %s has a different number of joints than the "
275  "provided",
276  end_effector_link.c_str());
277  return false;
278  }
279 
280  Eigen::VectorXd q(chain_.at(end_effector_link).getNrOfJoints());
281  int joint_index = 0;
282 
283  for (unsigned long i = 0; i < state.name.size(); i++)
284  {
285  if (hasJoint(chain_.at(end_effector_link), state.name[i]))
286  {
287  q[joint_index] = state.position[i];
288  joint_index++;
289  }
290 
291  if (joint_index == chain_.at(end_effector_link).getNrOfJoints())
292  {
293  break;
294  }
295  }
296 
297  if (joint_index != chain_.at(end_effector_link).getNrOfJoints())
298  {
299  ROS_ERROR(
300  "Provided joint state does not have all of the required chain joints");
301  return false;
302  }
303 
304  return getJointState(end_effector_link, q, qdot, state);
305 }
306 
307 bool KDLManager::createJointState(const std::string &end_effector_link,
308  const Eigen::VectorXd &q,
309  const Eigen::VectorXd &qdot,
310  sensor_msgs::JointState &state) const
311 {
312  sensor_msgs::JointState ret;
313  if (q.rows() != qdot.rows())
314  {
315  ROS_ERROR(
316  "Given joint state with a different number of joint positions and "
317  "velocities");
318  return false;
319  }
320 
321  if (chain_.find(end_effector_link) == chain_.end())
322  {
323  return false;
324  }
325 
326  if (chain_.at(end_effector_link).getNrOfJoints() != qdot.rows())
327  {
328  ROS_ERROR(
329  "Joint chain for eef %s has a different number of joints than the "
330  "provided",
331  end_effector_link.c_str());
332  return false;
333  }
334 
335  state.name.resize(actuated_joint_names_.at(end_effector_link).size());
336  state.position.resize(actuated_joint_names_.at(end_effector_link).size());
337  state.velocity.resize(actuated_joint_names_.at(end_effector_link).size());
338  state.effort.resize(actuated_joint_names_.at(end_effector_link).size());
339  for (unsigned long i = 0;
340  i < actuated_joint_names_.at(end_effector_link).size(); i++)
341  {
342  state.name[i] = actuated_joint_names_.at(end_effector_link)[i];
343  state.position[i] = q[i];
344  state.velocity[i] = qdot[i];
345  }
346 
347  return true;
348 }
349 
350 bool KDLManager::getJointState(const std::string &end_effector_link,
351  const Eigen::VectorXd &q,
352  const Eigen::VectorXd &qdot,
353  sensor_msgs::JointState &state) const
354 {
355  if (q.rows() != qdot.rows())
356  {
357  ROS_ERROR(
358  "Given joint state with a different number of joint positions and "
359  "velocities");
360  return false;
361  }
362 
363  if (chain_.find(end_effector_link) == chain_.end())
364  {
365  return false;
366  }
367 
368  if (chain_.at(end_effector_link).getNrOfJoints() != qdot.rows())
369  {
370  ROS_ERROR(
371  "Joint chain for eef %s has a different number of joints than the "
372  "provided",
373  end_effector_link.c_str());
374  return false;
375  }
376 
377  bool found;
378 
379  for (unsigned long i = 0;
380  i < actuated_joint_names_.at(end_effector_link).size(); i++)
381  {
382  found = false;
383  for (unsigned long j = 0; j < state.name.size(); j++)
384  {
385  if (state.name[j] == actuated_joint_names_.at(end_effector_link)[i])
386  {
387  state.position[j] = q[i];
388  state.velocity[j] = qdot[i];
389  found = true;
390  break;
391  }
392  }
393 
394  if (!found)
395  {
396  ROS_ERROR_STREAM("KDLManager: Missing joint "
397  << actuated_joint_names_.at(end_effector_link)[i]
398  << " from given joint state");
399  return false;
400  }
401  }
402 
403  return true;
404 }
405 
406 bool KDLManager::getJointState(const std::string &end_effector_link,
407  const Eigen::VectorXd &q,
408  const Eigen::VectorXd &qdot,
409  const Eigen::VectorXd &effort,
410  sensor_msgs::JointState &state) const
411 {
412  if (!getJointState(end_effector_link, q, qdot, state))
413  {
414  return false;
415  }
416 
417  if (chain_.find(end_effector_link) == chain_.end())
418  {
419  return false;
420  }
421 
422  if (chain_.at(end_effector_link).getNrOfJoints() != effort.rows())
423  {
424  ROS_ERROR_STREAM("Joint chain for eef "
425  << end_effector_link
426  << " has a different number of joints than the provided");
427  return false;
428  }
429 
430  bool found;
431  for (unsigned long i = 0;
432  i < actuated_joint_names_.at(end_effector_link).size(); i++)
433  {
434  found = false;
435  for (unsigned long j = 0; j < state.name.size(); j++)
436  {
437  if (state.name[j] == actuated_joint_names_.at(end_effector_link)[i])
438  {
439  state.effort[j] = effort[i];
440  found = true;
441  break;
442  }
443  }
444 
445  if (!found)
446  {
447  ROS_ERROR_STREAM("KDLManager: Missing joint "
448  << actuated_joint_names_.at(end_effector_link)[i]
449  << " from given joint state");
450  return false;
451  }
452  }
453 
454  return true;
455 }
456 
457 bool KDLManager::getGrippingPoint(const std::string &end_effector_link,
458  const sensor_msgs::JointState &state,
459  KDL::Frame &out) const
460 {
461  if (chain_.find(end_effector_link) == chain_.end())
462  {
463  return false;
464  }
465 
466  KDL::Frame eef_pose;
467  if (!getEefPose(end_effector_link, state, eef_pose))
468  {
469  return false;
470  }
471 
472  out = eef_pose * eef_to_gripping_point_.at(end_effector_link);
473  return true;
474 }
475 
476 bool KDLManager::getSensorPoint(const std::string &end_effector_link,
477  const sensor_msgs::JointState &state,
478  KDL::Frame &out) const
479 {
480  if (chain_.find(end_effector_link) == chain_.end())
481  {
482  return false;
483  }
484 
485  KDL::Frame eef_pose;
486  if (!getEefPose(end_effector_link, state, eef_pose))
487  {
488  return false;
489  }
490 
491  out = eef_pose * eef_to_sensor_point_.at(end_effector_link);
492  return true;
493 }
494 
495 bool KDLManager::getGrippingTwist(const std::string &end_effector_link,
496  const sensor_msgs::JointState &state,
497  KDL::Twist &out) const
498 {
499  if (chain_.find(end_effector_link) == chain_.end())
500  {
501  return false;
502  }
503 
504  KDL::Frame gripping_pose;
505  if (!getGrippingPoint(end_effector_link, state, gripping_pose))
506  {
507  return false;
508  }
509 
510  KDL::Frame eef_pose;
511  if (!getEefPose(end_effector_link, state, eef_pose))
512  {
513  return false;
514  }
515 
516  KDL::FrameVel eef_twist;
517  if (!getEefTwist(end_effector_link, state, eef_twist))
518  {
519  return false;
520  }
521 
522  Eigen::Vector3d vel_eig, rot_eig, converted_vel, r_eig;
523  KDL::Vector r = gripping_pose.p - eef_pose.p;
524 
525  vel_eig << eef_twist.GetTwist().vel.data[0], eef_twist.GetTwist().vel.data[1],
526  eef_twist.GetTwist().vel.data[2];
527  rot_eig << eef_twist.GetTwist().rot.data[0], eef_twist.GetTwist().rot.data[1],
528  eef_twist.GetTwist().rot.data[2];
529  r_eig << r.data[0], r.data[1], r.data[2];
530 
531  converted_vel = vel_eig - MatrixParser::computeSkewSymmetric(r_eig) * rot_eig;
532  out.vel = KDL::Vector(converted_vel[0], converted_vel[1], converted_vel[2]);
533  out.rot = eef_twist.GetTwist().rot;
534 
535  return true;
536 }
537 
538 bool KDLManager::getEefPose(const std::string &end_effector_link,
539  const sensor_msgs::JointState &state,
540  KDL::Frame &out) const
541 {
542  if (chain_.find(end_effector_link) == chain_.end())
543  {
544  return false;
545  }
546 
547  KDL::JntArray positions(chain_.at(end_effector_link).getNrOfJoints());
548  KDL::JntArrayVel velocities(chain_.at(end_effector_link).getNrOfJoints());
549 
550  if (!getChainJointState(state, end_effector_link, positions, velocities))
551  {
552  return false;
553  }
554 
555  fkpos_.at(end_effector_link)->JntToCart(positions, out);
556  return true;
557 }
558 
559 bool KDLManager::getEefTwist(const std::string &end_effector_link,
560  const sensor_msgs::JointState &state,
561  KDL::FrameVel &out) const
562 {
563  if (chain_.find(end_effector_link) == chain_.end())
564  {
565  return false;
566  }
567 
568  KDL::JntArray positions(chain_.at(end_effector_link).getNrOfJoints());
569  KDL::JntArrayVel velocities(chain_.at(end_effector_link).getNrOfJoints());
570  if (!getChainJointState(state, end_effector_link, positions, velocities))
571  {
572  return false;
573  }
574 
575  fkvel_.at(end_effector_link)->JntToCart(velocities, out);
576  return true;
577 }
578 
579 bool KDLManager::getJointLimits(const std::string &end_effector_link,
580  KDL::JntArray &q_min, KDL::JntArray &q_max,
581  KDL::JntArray &q_vel_lim) const
582 {
583  if (chain_.find(end_effector_link) == chain_.end())
584  {
585  return false;
586  }
587 
588  unsigned int joint_n = chain_.at(end_effector_link).getNrOfJoints();
589  if (q_min.rows() != joint_n || q_max.rows() != joint_n ||
590  q_vel_lim.rows() != joint_n)
591  {
592  ROS_ERROR(
593  "KDLManager::getJointPositionLimits requires initialized joint arrays");
594  return false;
595  }
596 
597  urdf::JointConstSharedPtr joint;
598  urdf::JointLimitsSharedPtr limits;
599  int j = 0;
600  // run through the kinematic chain joints and get the limits from the urdf
601  // model
602  for (unsigned int i = 0; i < chain_.at(end_effector_link).getNrOfSegments();
603  i++)
604  {
605  if (chain_.at(end_effector_link).getSegment(i).getJoint().getType() ==
606  KDL::Joint::JointType::None)
607  {
608  continue;
609  }
610 
611  joint = model_.getJoint(
612  chain_.at(end_effector_link).getSegment(i).getJoint().getName());
613  limits = joint->limits;
614  q_min(j) = limits->lower;
615  q_max(j) = limits->upper;
616  q_vel_lim(j) = limits->velocity;
617  j++;
618  }
619 
620  return true;
621 }
622 
623 bool KDLManager::getJointPositions(const std::string &end_effector_link,
624  const sensor_msgs::JointState &state,
625  KDL::JntArray &q) const
626 {
627  if (chain_.find(end_effector_link) == chain_.end())
628  {
629  return false;
630  }
631 
632  q.resize(chain_.at(end_effector_link).getNrOfJoints());
633  KDL::JntArrayVel v(q.rows());
634  if (!getChainJointState(state, end_effector_link, q, v))
635  {
636  return false;
637  }
638 
639  return true;
640 }
641 
642 bool KDLManager::getJointPositions(const std::string &end_effector_link,
643  const sensor_msgs::JointState &state,
644  Eigen::VectorXd &q) const
645 {
646  if (chain_.find(end_effector_link) == chain_.end())
647  {
648  return false;
649  }
650 
651  KDL::JntArray q_kdl;
652 
653  if (!getJointPositions(end_effector_link, state, q_kdl))
654  {
655  return false;
656  }
657 
658  q = q_kdl.data;
659  return true;
660 }
661 
662 bool KDLManager::getJointVelocities(const std::string &end_effector_link,
663  const sensor_msgs::JointState &state,
664  KDL::JntArray &q_dot) const
665 {
666  if (chain_.find(end_effector_link) == chain_.end())
667  {
668  return false;
669  }
670 
671  q_dot.resize(chain_.at(end_effector_link).getNrOfJoints());
672  KDL::JntArray q(q_dot.rows());
673  KDL::JntArrayVel v(q_dot.rows());
674  if (!getChainJointState(state, end_effector_link, q, v))
675  {
676  return false;
677  }
678 
679  q_dot = v.qdot;
680  return true;
681 }
682 
683 bool KDLManager::getInertia(const std::string &end_effector_link,
684  const sensor_msgs::JointState &state,
685  Eigen::MatrixXd &H)
686 {
687  if (chain_.find(end_effector_link) == chain_.end())
688  {
689  return false;
690  }
691 
692  KDL::JntArray q(chain_.at(end_effector_link).getNrOfJoints());
693  KDL::JntArrayVel q_dot(chain_.at(end_effector_link).getNrOfJoints());
694  KDL::JntSpaceInertiaMatrix B(chain_.at(end_effector_link).getNrOfJoints());
695 
696  if (!getChainJointState(state, end_effector_link, q, q_dot))
697  {
698  return false;
699  }
700 
701  dynamic_chain_.at(end_effector_link)->JntToMass(q, B);
702 
703  H = B.data;
704  return true;
705 }
706 
707 bool KDLManager::getGravity(const std::string &end_effector_link,
708  const sensor_msgs::JointState &state,
709  Eigen::MatrixXd &g)
710 {
711  if (chain_.find(end_effector_link) == chain_.end())
712  {
713  return false;
714  }
715 
716  KDL::JntArray q(chain_.at(end_effector_link).getNrOfJoints());
717  KDL::JntArrayVel q_dot(chain_.at(end_effector_link).getNrOfJoints());
718  if (!getChainJointState(state, end_effector_link, q, q_dot))
719  {
720  return false;
721  }
722 
723  KDL::JntArray q_gravity(chain_.at(end_effector_link).getNrOfJoints());
724  dynamic_chain_.at(end_effector_link)->JntToGravity(q, q_gravity);
725 
726  g = q_gravity.data;
727 
728  return true;
729 }
730 
731 bool KDLManager::getCoriolis(const std::string &end_effector_link,
732  const sensor_msgs::JointState &state,
733  Eigen::MatrixXd &coriolis)
734 {
735  if (chain_.find(end_effector_link) == chain_.end())
736  {
737  return false;
738  }
739 
740  KDL::JntArray q(chain_.at(end_effector_link).getNrOfJoints());
741  KDL::JntArrayVel q_dot(chain_.at(end_effector_link).getNrOfJoints());
742  if (!getChainJointState(state, end_effector_link, q, q_dot))
743  {
744  return false;
745  }
746 
747  KDL::JntArray cor(chain_.at(end_effector_link).getNrOfJoints());
748  dynamic_chain_.at(end_effector_link)->JntToCoriolis(q, q_dot.qdot, cor);
749  coriolis = cor.data;
750  return true;
751 }
752 
753 bool KDLManager::getRigidTransform(const std::string &base_frame,
754  const std::string &target_frame,
755  KDL::Frame &out) const
756 {
757  geometry_msgs::PoseStamped base_to_target;
758  base_to_target.header.frame_id = base_frame;
759  base_to_target.header.stamp = ros::Time(0);
760  base_to_target.pose.position.x = 0;
761  base_to_target.pose.position.y = 0;
762  base_to_target.pose.position.z = 0;
763  base_to_target.pose.orientation.x = 0;
764  base_to_target.pose.orientation.y = 0;
765  base_to_target.pose.orientation.z = 0;
766  base_to_target.pose.orientation.w = 1;
767 
768  int attempts;
769  for (attempts = 0; attempts < max_tf_attempts_; attempts++)
770  {
771  try
772  {
773  listener_.transformPose(target_frame, base_to_target, base_to_target);
774  break;
775  }
776  catch (tf::TransformException ex)
777  {
778  ROS_WARN("TF exception in kdl manager: %s", ex.what());
779  ros::Duration(0.1).sleep();
780  }
781  }
782 
783  if (attempts >= max_tf_attempts_)
784  {
785  ROS_ERROR(
786  "KDL manager could not find the transform between frames %s and %s",
787  base_frame.c_str(), target_frame.c_str());
788  return false;
789  }
790 
791  tf::poseMsgToKDL(base_to_target.pose, out);
792  return true;
793 }
794 
795 bool KDLManager::verifyPose(const std::string &end_effector_link,
796  const KDL::Frame &in) const
797 {
798  if (chain_.find(end_effector_link) == chain_.end())
799  {
800  return false;
801  }
802 
803  sensor_msgs::JointState dummy_state;
804 
805  for (unsigned int i = 0;
806  i < actuated_joint_names_.at(end_effector_link).size(); i++)
807  {
808  dummy_state.name.push_back(actuated_joint_names_.at(end_effector_link)[i]);
809  dummy_state.position.push_back(0);
810  dummy_state.velocity.push_back(0);
811  dummy_state.effort.push_back(0);
812  }
813 
814  KDL::JntArray dummy_out(dummy_state.name.size());
815  if (getPoseIK(end_effector_link, dummy_state, in, dummy_out))
816  {
817  return true;
818  }
819 
820  return false;
821 }
822 
823 bool KDLManager::getPoseIK(const std::string &end_effector_link,
824  const sensor_msgs::JointState &state,
825  const KDL::Frame &in, KDL::JntArray &out) const
826 {
827  if (chain_.find(end_effector_link) == chain_.end())
828  {
829  return false;
830  }
831 
832  KDL::JntArray positions(chain_.at(end_effector_link).getNrOfJoints());
833  KDL::JntArrayVel velocities(chain_.at(end_effector_link).getNrOfJoints());
834  KDL::Frame computedPose, difference;
835  if (!getChainJointState(state, end_effector_link, positions, velocities))
836  {
837  return false;
838  }
839 
840  out.resize(chain_.at(end_effector_link).getNrOfJoints());
841  ikpos_.at(end_effector_link)->CartToJnt(positions, in, out);
842  getPoseFK(end_effector_link, state, out,
843  computedPose); // verify if the forward kinematics of the computed
844  // solution are close to the desired pose
845 
846  difference = computedPose.Inverse() * in;
847  Eigen::Vector3d quat_v;
848  double quat_w;
849 
850  difference.M.GetQuaternion(quat_v[0], quat_v[1], quat_v[2], quat_w);
851  double angle = 2 * atan2(quat_v.norm(), quat_w);
852 
853  if (fabs(angle) > ik_angle_tolerance_)
854  {
855  ROS_ERROR(
856  "KDL manager could not compute pose ik for end-effector %s. Final "
857  "orientation error was %.2f",
858  end_effector_link.c_str(), angle);
859  return false;
860  }
861 
862  if (fabs(difference.p.Norm()) > ik_pos_tolerance_)
863  {
864  ROS_ERROR(
865  "KDL manager could not compute pose ik for end-effector %s. Final "
866  "position error was %.2f",
867  end_effector_link.c_str(), difference.p.Norm());
868  return false;
869  }
870 
871  return true;
872 }
873 
874 bool KDLManager::getGrippingPoseIK(const std::string &end_effector_link,
875  const sensor_msgs::JointState &state,
876  const KDL::Frame &in,
877  KDL::JntArray &out) const
878 {
879  if (chain_.find(end_effector_link) == chain_.end())
880  {
881  return false;
882  }
883 
884  KDL::Frame pose_in_eef =
885  in * eef_to_gripping_point_.at(end_effector_link).Inverse();
886 
887  return getPoseIK(end_effector_link, state, pose_in_eef, out);
888 }
889 
890 bool KDLManager::getPoseFK(const std::string &end_effector_link,
891  const sensor_msgs::JointState &state,
892  const KDL::JntArray &in, KDL::Frame &out) const
893 {
894  if (chain_.find(end_effector_link) == chain_.end())
895  {
896  return false;
897  }
898 
899  fkpos_.at(end_effector_link)->JntToCart(in, out);
900  return true;
901 }
902 
903 bool KDLManager::getGrippingVelIK(const std::string &end_effector_link,
904  const sensor_msgs::JointState &state,
905  const KDL::Twist &in,
906  KDL::JntArray &out) const
907 {
908  KDL::Twist modified_in;
909 
910  if (!grippingTwistToEef(end_effector_link, state, in, modified_in))
911  {
912  return false;
913  }
914 
915  if (!getVelIK(end_effector_link, state, modified_in, out))
916  {
917  return false;
918  }
919 
920  return true;
921 }
922 
923 bool KDLManager::grippingTwistToEef(const std::string &end_effector_link,
924  const sensor_msgs::JointState &state,
925  const KDL::Twist &in, KDL::Twist &out) const
926 {
927  KDL::Frame pgrip, peef;
928 
929  if (chain_.find(end_effector_link) == chain_.end())
930  {
931  return false;
932  }
933 
934  if (!getGrippingPoint(end_effector_link, state, pgrip))
935  {
936  return false;
937  }
938 
939  if (!getEefPose(end_effector_link, state, peef))
940  {
941  return false;
942  }
943 
944  // convert the input twist (in the gripping frame) to the base frame
945  Eigen::Vector3d vel_eig, rot_eig, peef_eig, pgrip_eig;
946  out = pgrip.M * in;
947 
948  tf::vectorKDLToEigen(peef.p, peef_eig);
949  tf::vectorKDLToEigen(pgrip.p, pgrip_eig);
950  tf::vectorKDLToEigen(out.vel, vel_eig);
951  tf::vectorKDLToEigen(out.rot, rot_eig);
952 
953  vel_eig = MatrixParser::computeSkewSymmetric(pgrip_eig - peef_eig) * rot_eig +
954  vel_eig;
955 
956  tf::vectorEigenToKDL(vel_eig, out.vel);
957  return true;
958 }
959 
960 bool KDLManager::getVelIK(const std::string &end_effector_link,
961  const sensor_msgs::JointState &state,
962  const KDL::Twist &in, KDL::JntArray &out) const
963 {
964  if (chain_.find(end_effector_link) == chain_.end())
965  {
966  return false;
967  }
968 
969  KDL::JntArray positions(chain_.at(end_effector_link).getNrOfJoints());
970  KDL::JntArrayVel velocities(chain_.at(end_effector_link).getNrOfJoints());
971  if (!getChainJointState(state, end_effector_link, positions, velocities))
972  {
973  return false;
974  }
975 
976  out.resize(chain_.at(end_effector_link).getNrOfJoints());
977  ikvel_.at(end_effector_link)->CartToJnt(positions, in, out);
978  return true;
979 }
980 
981 bool KDLManager::getJacobian(const std::string &end_effector_link,
982  const sensor_msgs::JointState &state,
983  KDL::Jacobian &out) const
984 {
985  if (chain_.find(end_effector_link) == chain_.end())
986  {
987  return false;
988  }
989 
990  KDL::JntArray positions(chain_.at(end_effector_link).getNrOfJoints());
991  KDL::JntArrayVel velocities(chain_.at(end_effector_link).getNrOfJoints());
992  if (!getChainJointState(state, end_effector_link, positions, velocities))
993  {
994  return false;
995  }
996 
997  out.resize(positions.rows());
998  jac_solver_.at(end_effector_link)->JntToJac(positions, out);
999  return true;
1000 }
1001 
1002 bool KDLManager::checkStateMessage(const std::string &end_effector_link,
1003  const sensor_msgs::JointState &state) const
1004 {
1005  if (chain_.find(end_effector_link) == chain_.end())
1006  {
1007  return false;
1008  }
1009 
1010  unsigned int processed_joints = 0, name_size, pos_size, vel_size;
1011  name_size = state.name.size();
1012  pos_size = state.position.size();
1013  vel_size = state.velocity.size();
1014 
1015  if (name_size != pos_size || name_size != vel_size)
1016  {
1017  ROS_ERROR(
1018  "Got joint state where the name, position and velocity dimensions "
1019  "(resp. %d, %d, %d) are different",
1020  name_size, pos_size, vel_size);
1021  return false;
1022  }
1023 
1024  for (unsigned long i = 0;
1025  i < actuated_joint_names_.at(end_effector_link).size(); i++)
1026  {
1027  for (unsigned long j = 0; j < state.name.size(); j++)
1028  {
1029  if (actuated_joint_names_.at(end_effector_link)[i] == state.name[j])
1030  {
1031  processed_joints++;
1032  }
1033  }
1034  }
1035 
1036  if (processed_joints != actuated_joint_names_.at(end_effector_link).size())
1037  {
1038  return false;
1039  }
1040 
1041  return true;
1042 }
1043 
1045  const sensor_msgs::JointState &current_state,
1046  const std::string &end_effector_link, KDL::JntArray &positions,
1047  KDL::JntArrayVel &velocities) const
1048 {
1049  unsigned int processed_joints = 0;
1050  unsigned int name_size, pos_size, vel_size;
1051 
1052  name_size = current_state.name.size();
1053  pos_size = current_state.position.size();
1054  vel_size = current_state.velocity.size();
1055 
1056  if (name_size != pos_size || name_size != vel_size)
1057  {
1058  ROS_ERROR(
1059  "Got joint state where the name, position and velocity dimensions "
1060  "(resp. %d, %d, %d) are different",
1061  name_size, pos_size, vel_size);
1062  return false;
1063  }
1064 
1065  for (unsigned long i = 0;
1066  i < actuated_joint_names_.at(end_effector_link).size(); i++)
1067  {
1068  for (unsigned long j = 0; j < current_state.name.size(); j++)
1069  {
1070  if (actuated_joint_names_.at(end_effector_link)[i] ==
1071  current_state.name[j])
1072  {
1073  positions(processed_joints) = current_state.position[j];
1074  velocities.q(processed_joints) = current_state.position[j];
1075  velocities.qdot(processed_joints) = current_state.velocity[j];
1076  processed_joints++;
1077  break;
1078  }
1079  }
1080  }
1081 
1082  if (processed_joints != actuated_joint_names_.at(end_effector_link).size())
1083  {
1084  ROS_ERROR("Failed to acquire chain joint state");
1085  return false;
1086  }
1087 
1088  return true;
1089 }
1090 
1092  const std::string &joint_name) const
1093 {
1094  for (unsigned int i = 0; i < chain.getNrOfSegments(); i++)
1095  {
1096  if (chain.segments[i].getJoint().getName() == joint_name)
1097  {
1098  return true;
1099  }
1100  }
1101 
1102  return false;
1103 }
1104 
1105 bool KDLManager::getNumJoints(const std::string &end_effector_link,
1106  unsigned int &num_joints) const
1107 {
1108  if (chain_.find(end_effector_link) == chain_.end())
1109  {
1110  return false;
1111  }
1112 
1113  num_joints = chain_.at(end_effector_link).getNrOfJoints();
1114  return true;
1115 }
1116 
1117 bool KDLManager::addSegment(const std::string &end_effector_link,
1118  KDL::Segment &new_segment)
1119 {
1120  if (chain_.find(end_effector_link) == chain_.end())
1121  {
1122  return false;
1123  }
1124 
1125  chain_.at(end_effector_link).addSegment(new_segment);
1126  updateSolvers();
1127  return true;
1128 }
1129 
1131 {
1132  for (auto const &item : chain_)
1133  {
1134  ikvel_[item.first]->updateInternalDataStructures();
1135  fkpos_[item.first]->updateInternalDataStructures();
1136  jac_solver_[item.first]->updateInternalDataStructures();
1137  dynamic_chain_[item.first]->updateInternalDataStructures();
1138  }
1139 }
1140 
1141 bool setKDLManager(const ArmInfo &arm_info, std::shared_ptr<KDLManager> manager)
1142 {
1143  if (!manager->initializeArm(arm_info.kdl_eef_frame))
1144  {
1145  return false;
1146  }
1147 
1148  if (!manager->setGrippingPoint(arm_info.kdl_eef_frame,
1149  arm_info.gripping_frame))
1150  {
1151  return false;
1152  }
1153 
1154  ROS_DEBUG(
1155  "Successfully set up arm %s with eef_frame %s and gripping_frame %s",
1156  arm_info.name.c_str(), arm_info.kdl_eef_frame.c_str(),
1157  arm_info.gripping_frame.c_str());
1158 
1159  return true;
1160 }
1161 } // namespace generic_control_toolbox
bool getGrippingVelIK(const std::string &end_effector_link, const sensor_msgs::JointState &state, const KDL::Twist &in, KDL::JntArray &out) const
bool verifyPose(const std::string &end_effector_link, const KDL::Frame &in) const
bool initializeArm(const std::string &end_effector_link)
Definition: kdl_manager.cpp:96
bool getSensorPoint(const std::string &end_effector_link, const sensor_msgs::JointState &state, KDL::Frame &out) const
static Eigen::Matrix3d computeSkewSymmetric(const Eigen::Vector3d &v)
const std::string WDLS_SOLVER("wdls")
bool getJointLimits(const std::string &end_effector_link, KDL::JntArray &q_min, KDL::JntArray &q_max, KDL::JntArray &q_vel_lim) const
void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const
KDL_PARSER_PUBLIC bool treeFromUrdfModel(const urdf::ModelInterface &robot_model, KDL::Tree &tree)
void resize(unsigned int newNrOfColumns)
const std::string NSO_SOLVER("nso")
bool addSegment(const std::string &end_effector_link, KDL::Segment &new_segment)
Vector vel
unsigned int getNrOfSegments() const
bool getGrippingPoseIK(const std::string &end_effector_link, const sensor_msgs::JointState &state, const KDL::Frame &in, KDL::JntArray &out) const
std::map< std::string, IkSolverPosPtr > ikpos_
std::map< std::string, FkSolverPosPtr > fkpos_
bool hasJoint(const KDL::Chain &chain, const std::string &joint_name) const
IMETHOD Twist GetTwist() const
bool getJacobian(const std::string &end_effector_link, const sensor_msgs::JointState &state, KDL::Jacobian &out) const
bool getGrippingPoint(const std::string &end_effector_link, const sensor_msgs::JointState &state, KDL::Frame &out) const
const SegmentMap & getSegments() const
bool getChainJointState(const sensor_msgs::JointState &current_state, const std::string &end_effector_link, KDL::JntArray &positions, KDL::JntArrayVel &velocities) const
const Segment & getSegment(unsigned int nr) const
bool initializeArmCommon(const std::string &end_effector_link)
#define ROS_ERROR(...)
std::map< std::string, JacSolverPtr > jac_solver_
#define ROS_WARN(...)
const std::string getTypeName() const
std::map< std::string, KDL::Chain > chain_
std::map< std::string, ChainDynParamPtr > dynamic_chain_
unsigned int rows() const
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
std::string chain_base_link_
list of actuated joints per arm
#define ROS_DEBUG(...)
std::map< std::string, KDL::Frame > eef_to_sensor_point_
Rotation M
KDL::ChainFkSolverPos_recursive FkSolverPos
std::shared_ptr< IkSolverPos > IkSolverPosPtr
const Joint & getJoint() const
std::shared_ptr< FkSolverVel > FkSolverVelPtr
KDLManager(const std::string &chain_base_link, ros::NodeHandle nh=ros::NodeHandle("~"))
Definition: kdl_manager.cpp:5
Frame Inverse() const
Vector rot
bool getPoseFK(const std::string &end_effector_link, const sensor_msgs::JointState &state, const KDL::JntArray &in, KDL::Frame &out) const
bool getParam(const std::string &key, std::string &s) const
std::map< std::string, IkSolverVelPtr > ikvel_
double data[3]
bool isInitialized(const std::string &end_effector_link) const
Eigen::VectorXd data
void poseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k)
bool getChain(const std::string &chain_root, const std::string &chain_tip, Chain &chain) const
bool getEefPose(const std::string &end_effector_link, const sensor_msgs::JointState &state, KDL::Frame &out) const
URDF_EXPORT bool initParam(const std::string &param)
void vectorEigenToKDL(const Eigen::Matrix< double, 3, 1 > &e, KDL::Vector &k)
bool getNumJoints(const std::string &end_effector_link, unsigned int &num_joints) const
#define ROS_WARN_STREAM(args)
#define ROS_DEBUG_STREAM(args)
std::shared_ptr< JacSolver > JacSolverPtr
KDL::ChainFkSolverVel_recursive FkSolverVel
bool setSensorPoint(const std::string &end_effector_link, const std::string &sensor_point_frame)
std::shared_ptr< KDL::ChainDynParam > ChainDynParamPtr
KDL::ChainJntToJacSolver JacSolver
bool checkStateMessage(const std::string &end_effector_link, const sensor_msgs::JointState &state) const
bool getCoriolis(const std::string &end_effector_link, const sensor_msgs::JointState &state, Eigen::MatrixXd &coriolis)
bool getInertia(const std::string &end_effector_link, const sensor_msgs::JointState &state, Eigen::MatrixXd &H)
void resize(unsigned int newSize)
bool getPoseIK(const std::string &end_effector_link, const sensor_msgs::JointState &state, const KDL::Frame &in, KDL::JntArray &out) const
bool getJointVelocities(const std::string &end_effector_link, const sensor_msgs::JointState &state, KDL::JntArray &q_dot) const
std::map< std::string, std::vector< std::string > > actuated_joint_names_
void vectorKDLToEigen(const KDL::Vector &k, Eigen::Matrix< double, 3, 1 > &e)
static Frame Identity()
std::vector< Segment > segments
bool setKDLManager(const ArmInfo &arm_info, std::shared_ptr< KDLManager > manager)
void GetQuaternion(double &x, double &y, double &z, double &w) const
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
bool setGrippingPoint(const std::string &end_effector_link, const std::string &gripping_point_frame)
bool getJointPositions(const std::string &end_effector_link, const sensor_msgs::JointState &state, KDL::JntArray &q) const
bool getGrippingTwist(const std::string &end_effector_link, const sensor_msgs::JointState &state, KDL::Twist &out) const
const std::string & getName() const
bool getJointState(const std::string &end_effector_link, const Eigen::VectorXd &qdot, sensor_msgs::JointState &state) const
bool sleep() const
#define ROS_ERROR_STREAM(args)
std::shared_ptr< FkSolverPos > FkSolverPosPtr
std::map< std::string, KDL::Frame > eef_to_gripping_point_
bool getVelIK(const std::string &end_effector_link, const sensor_msgs::JointState &state, const KDL::Twist &in, KDL::JntArray &out) const
KDL::ChainIkSolverPos_LMA IkSolverPos
bool grippingTwistToEef(const std::string &end_effector_link, const sensor_msgs::JointState &state, const KDL::Twist &in, KDL::Twist &out) const
std::map< std::string, FkSolverVelPtr > fkvel_
bool getEefTwist(const std::string &end_effector_link, const sensor_msgs::JointState &state, KDL::FrameVel &out) const
std::shared_ptr< IkSolverVel > IkSolverVelPtr
bool getRigidTransform(const std::string &base_frame, const std::string &target_frame, KDL::Frame &out) const
bool getGravity(const std::string &end_effector_link, const sensor_msgs::JointState &state, Eigen::MatrixXd &g)
bool createJointState(const std::string &end_effector_link, const Eigen::VectorXd &q, const Eigen::VectorXd &qdot, sensor_msgs::JointState &state) const


generic_control_toolbox
Author(s): diogo
autogenerated on Wed Apr 28 2021 03:00:27