kdl_manager.hpp
Go to the documentation of this file.
1 #ifndef __KDL_MANAGER__
2 #define __KDL_MANAGER__
3 
6 #include <generic_control_toolbox/ArmInfo.h>
7 #include <geometry_msgs/WrenchStamped.h>
9 #include <ros/ros.h>
10 #include <sensor_msgs/JointState.h>
11 #include <tf/transform_listener.h>
12 #include <urdf/model.h>
15 #include <kdl/chaindynparam.hpp>
16 #include <kdl/chainfksolverpos_recursive.hpp>
17 #include <kdl/chainfksolvervel_recursive.hpp>
18 #include <kdl/chainiksolverpos_lma.hpp>
19 #include <kdl/chainiksolvervel_pinv_nso.hpp>
20 #include <kdl/chainiksolvervel_wdls.hpp>
21 #include <kdl/chainjnttojacsolver.hpp>
22 #include <kdl/frames.hpp>
23 #include <kdl/kdl.hpp>
25 #include <stdexcept>
26 
27 const std::string WDLS_SOLVER("wdls"), NSO_SOLVER("nso");
28 
30 {
35 class KDLManager : public ManagerBase
36 {
37  public:
38  KDLManager(const std::string &chain_base_link,
40  ~KDLManager();
41 
50  bool initializeArm(const std::string &end_effector_link);
51 
59  bool isInitialized(const std::string &end_effector_link) const;
60 
71  bool checkStateMessage(const std::string &end_effector_link,
72  const sensor_msgs::JointState &state) const;
82  bool setGrippingPoint(const std::string &end_effector_link,
83  const std::string &gripping_point_frame);
84 
94  bool setSensorPoint(const std::string &end_effector_link,
95  const std::string &sensor_point_frame);
96 
106  bool createJointState(const std::string &end_effector_link,
107  const Eigen::VectorXd &q, const Eigen::VectorXd &qdot,
108  sensor_msgs::JointState &state) const;
118  bool getJointState(const std::string &end_effector_link,
119  const Eigen::VectorXd &qdot,
120  sensor_msgs::JointState &state) const;
121 
132  bool getJointState(const std::string &end_effector_link,
133  const Eigen::VectorXd &q, const Eigen::VectorXd &qdot,
134  sensor_msgs::JointState &state) const;
135 
148  bool getJointState(const std::string &end_effector_link,
149  const Eigen::VectorXd &q, const Eigen::VectorXd &qdot,
150  const Eigen::VectorXd &effort,
151  sensor_msgs::JointState &state) const;
152 
162  bool getGrippingPoint(const std::string &end_effector_link,
163  const sensor_msgs::JointState &state,
164  KDL::Frame &out) const;
165 
175  bool getSensorPoint(const std::string &end_effector_link,
176  const sensor_msgs::JointState &state,
177  KDL::Frame &out) const;
178 
188  bool getGrippingTwist(const std::string &end_effector_link,
189  const sensor_msgs::JointState &state,
190  KDL::Twist &out) const;
191 
199  bool verifyPose(const std::string &end_effector_link,
200  const KDL::Frame &in) const;
201 
213  bool getPoseIK(const std::string &end_effector_link,
214  const sensor_msgs::JointState &state, const KDL::Frame &in,
215  KDL::JntArray &out) const;
216 
228  bool getGrippingPoseIK(const std::string &end_effector_link,
229  const sensor_msgs::JointState &state,
230  const KDL::Frame &in, KDL::JntArray &out) const;
231 
242  bool getPoseFK(const std::string &end_effector_link,
243  const sensor_msgs::JointState &state, const KDL::JntArray &in,
244  KDL::Frame &out) const;
245 
256  bool getGrippingVelIK(const std::string &end_effector_link,
257  const sensor_msgs::JointState &state,
258  const KDL::Twist &in, KDL::JntArray &out) const;
259 
271  bool grippingTwistToEef(const std::string &end_effector_link,
272  const sensor_msgs::JointState &state,
273  const KDL::Twist &in, KDL::Twist &out) const;
274 
285  bool getVelIK(const std::string &end_effector_link,
286  const sensor_msgs::JointState &state, const KDL::Twist &in,
287  KDL::JntArray &out) const;
288 
297  bool getJacobian(const std::string &end_effector_link,
298  const sensor_msgs::JointState &state,
299  KDL::Jacobian &out) const;
300 
309  bool getEefPose(const std::string &end_effector_link,
310  const sensor_msgs::JointState &state, KDL::Frame &out) const;
311 
320  bool getEefTwist(const std::string &end_effector_link,
321  const sensor_msgs::JointState &state,
322  KDL::FrameVel &out) const;
323 
334  bool getJointLimits(const std::string &end_effector_link,
335  KDL::JntArray &q_min, KDL::JntArray &q_max,
336  KDL::JntArray &q_vel_lim) const;
337 
346  bool getJointPositions(const std::string &end_effector_link,
347  const sensor_msgs::JointState &state,
348  KDL::JntArray &q) const;
349  bool getJointPositions(const std::string &end_effector_link,
350  const sensor_msgs::JointState &state,
351  Eigen::VectorXd &q) const;
352 
361  bool getJointVelocities(const std::string &end_effector_link,
362  const sensor_msgs::JointState &state,
363  KDL::JntArray &q_dot) const;
364 
373  bool getInertia(const std::string &end_effector_link,
374  const sensor_msgs::JointState &state, Eigen::MatrixXd &H);
375 
384  bool getGravity(const std::string &end_effector_link,
385  const sensor_msgs::JointState &state, Eigen::MatrixXd &g);
386 
395  bool getCoriolis(const std::string &end_effector_link,
396  const sensor_msgs::JointState &state,
397  Eigen::MatrixXd &coriolis);
398 
406  bool getNumJoints(const std::string &end_effector_link,
407  unsigned int &num_joints) const;
408 
419  bool addSegment(const std::string &end_effector_link,
420  KDL::Segment &new_segment);
421 
422  private:
428 
429  typedef std::shared_ptr<IkSolverVel> IkSolverVelPtr;
430  typedef std::shared_ptr<IkSolverPos> IkSolverPosPtr;
431  typedef std::shared_ptr<FkSolverPos> FkSolverPosPtr;
432  typedef std::shared_ptr<FkSolverVel> FkSolverVelPtr;
433  typedef std::shared_ptr<JacSolver> JacSolverPtr;
434  typedef std::shared_ptr<KDL::ChainDynParam> ChainDynParamPtr;
435 
436  std::map<std::string, IkSolverVelPtr> ikvel_;
437  std::map<std::string, IkSolverPosPtr> ikpos_;
438  std::map<std::string, FkSolverPosPtr> fkpos_;
439  std::map<std::string, FkSolverVelPtr> fkvel_;
440  std::map<std::string, JacSolverPtr> jac_solver_;
441  std::map<std::string, KDL::Frame> eef_to_gripping_point_;
442  std::map<std::string, KDL::Frame> eef_to_sensor_point_;
443  std::map<std::string, KDL::Chain> chain_;
444  std::map<std::string, ChainDynParamPtr> dynamic_chain_;
445 
450  std::map<std::string, std::vector<std::string> >
455 
461  bool getParam();
462 
469  bool initializeArmCommon(const std::string &end_effector_link);
470 
479  bool getRigidTransform(const std::string &base_frame,
480  const std::string &target_frame,
481  KDL::Frame &out) const;
482 
495  bool getChainJointState(const sensor_msgs::JointState &current_state,
496  const std::string &end_effector_link,
497  KDL::JntArray &positions,
498  KDL::JntArrayVel &velocities) const;
499 
507  bool hasJoint(const KDL::Chain &chain, const std::string &joint_name) const;
508 
512  void updateSolvers();
513 };
514 
523 bool setKDLManager(const ArmInfo &arm_info,
524  std::shared_ptr<KDLManager> manager);
525 
534 bool setKDLManagerNso(const ArmInfo &arm_info,
535  std::shared_ptr<KDLManager> manager);
536 } // namespace generic_control_toolbox
537 #endif
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
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
const std::string NSO_SOLVER("nso")
bool addSegment(const std::string &end_effector_link, KDL::Segment &new_segment)
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_
KDL::ChainIkSolverVel IkSolverVel
std::map< std::string, FkSolverPosPtr > fkpos_
bool hasJoint(const KDL::Chain &chain, const std::string &joint_name) 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
bool getChainJointState(const sensor_msgs::JointState &current_state, const std::string &end_effector_link, KDL::JntArray &positions, KDL::JntArrayVel &velocities) const
bool initializeArmCommon(const std::string &end_effector_link)
std::map< std::string, JacSolverPtr > jac_solver_
std::map< std::string, KDL::Chain > chain_
std::map< std::string, ChainDynParamPtr > dynamic_chain_
std::string chain_base_link_
list of actuated joints per arm
std::map< std::string, KDL::Frame > eef_to_sensor_point_
KDL::ChainFkSolverPos_recursive FkSolverPos
std::shared_ptr< IkSolverPos > IkSolverPosPtr
std::shared_ptr< FkSolverVel > FkSolverVelPtr
KDLManager(const std::string &chain_base_link, ros::NodeHandle nh=ros::NodeHandle("~"))
Definition: kdl_manager.cpp:5
bool getPoseFK(const std::string &end_effector_link, const sensor_msgs::JointState &state, const KDL::JntArray &in, KDL::Frame &out) const
std::map< std::string, IkSolverVelPtr > ikvel_
bool isInitialized(const std::string &end_effector_link) const
bool getEefPose(const std::string &end_effector_link, const sensor_msgs::JointState &state, KDL::Frame &out) const
bool getNumJoints(const std::string &end_effector_link, unsigned int &num_joints) const
std::shared_ptr< JacSolver > JacSolverPtr
KDL::ChainFkSolverVel_recursive FkSolverVel
bool setSensorPoint(const std::string &end_effector_link, const std::string &sensor_point_frame)
bool setKDLManagerNso(const ArmInfo &arm_info, std::shared_ptr< KDLManager > manager)
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)
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_
bool setKDLManager(const ArmInfo &arm_info, std::shared_ptr< KDLManager > manager)
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
bool getJointState(const std::string &end_effector_link, const Eigen::VectorXd &qdot, sensor_msgs::JointState &state) const
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