kinematics.h
Go to the documentation of this file.
1
26 #ifndef INDUSTRIAL_MOVEIT_STOMP_MOVEIT_INCLUDE_STOMP_MOVEIT_UTILS_KINEMATICS_H_
27 #define INDUSTRIAL_MOVEIT_STOMP_MOVEIT_INCLUDE_STOMP_MOVEIT_UTILS_KINEMATICS_H_
28
29 #include <ros/console.h>
30 #include <Eigen/Geometry>
31 #include <Eigen/StdVector>
36 #include <XmlRpcException.h>
37 #include <moveit_msgs/Constraints.h>
38 #include <moveit_msgs/PositionConstraint.h>
39 #include <moveit_msgs/OrientationConstraint.h>
40 #include <trac_ik/trac_ik.hpp>
41 #include <boost/optional.hpp>
42
43
44 namespace EigenSTL
45 {
46  typedef std::vector<Eigen::Affine3d, Eigen::aligned_allocator<Eigen::Affine3d>> vector_Affine3d;
47 }
48
49 namespace stomp_moveit
50 {
51 namespace utils
52 {
53
58 namespace kinematics
59 {
60
61  const static double EPSILON = 0.011;
62  const static double LAMBDA = 0.01;
65
70  class IKSolver
71  {
72  public:
79  IKSolver(const moveit::core::RobotState& robot_state,std::string group_name,double max_time = 0.005);
80  ~IKSolver();
81
86  void setKinematicState(const moveit::core::RobotState& state);
87
96  bool solve(const Eigen::VectorXd& seed,const Eigen::Affine3d& tool_pose,Eigen::VectorXd& solution,
97  Eigen::VectorXd tol = Eigen::VectorXd::Constant(6,0.005)) ;
98
107  bool solve(const std::vector<double>& seed, const Eigen::Affine3d& tool_pose,std::vector<double>& solution,
108  std::vector<double> tol = std::vector<double>(6,0.005)) ;
109
117  bool solve(const std::vector<double>& seed, const moveit_msgs::Constraints& tool_constraints,std::vector<double>& solution);
118
126  bool solve(const Eigen::VectorXd& seed, const moveit_msgs::Constraints& tool_constraints,Eigen::VectorXd& solution);
127
128  protected:
129
130  std::shared_ptr<TRAC_IK::TRAC_IK> ik_solver_impl_;
131  moveit::core::RobotModelConstPtr robot_model_;
132  moveit::core::RobotStatePtr robot_state_;
133  std::string group_name_;
134  Eigen::Affine3d tf_base_to_root_;
135
136  };
137
144  bool isCartesianConstraints(const moveit_msgs::Constraints& c);
145
154  boost::optional<moveit_msgs::Constraints> curateCartesianConstraints(const moveit_msgs::Constraints& c,const Eigen::Affine3d& ref_pose,
155  double default_pos_tol = 0.0005, double default_rot_tol = M_PI);
156
166  bool decodeCartesianConstraint(moveit::core::RobotModelConstPtr model,const moveit_msgs::Constraints& constraints,
167  Eigen::Affine3d& tool_pose, std::vector<double>& tolerance, std::string target_frame = "");
168
178  bool decodeCartesianConstraint(moveit::core::RobotModelConstPtr model,const moveit_msgs::Constraints& constraints,
179  Eigen::Affine3d& tool_pose, Eigen::VectorXd& tolerance,std::string target_frame = "");
180
188  EigenSTL::vector_Affine3d sampleCartesianPoses(const moveit_msgs::Constraints& c,
189  const std::vector<double> sampling_resolution = {0.05, 0.05, 0.05, M_PI_2,M_PI_2,M_PI_2},
190  int max_samples =20 );
191
192
202  void computeTwist(const Eigen::Affine3d& p0,
203  const Eigen::Affine3d& pf,
204  const Eigen::ArrayXi& nullity,Eigen::VectorXd& twist);
205
212  void reduceJacobian(const Eigen::MatrixXd& jacb,
213  const std::vector<int>& indices,Eigen::MatrixXd& jacb_reduced);
214
222  void calculateDampedPseudoInverse(const Eigen::MatrixXd &jacb, Eigen::MatrixXd &jacb_pseudo_inv,
223  double eps = 0.011, double lambda = 0.01);
224
235  bool computeJacobianNullSpace(moveit::core::RobotStatePtr state,std::string group,std::string tool_link,
236  const Eigen::ArrayXi& constrained_dofs,const Eigen::VectorXd& joint_pose,
237  Eigen::MatrixXd& jacb_nullspace);
238
239 } // kinematics
240 } // utils
241 } // stomp_moveit
242
243
244
245 #endif /* INDUSTRIAL_MOVEIT_STOMP_MOVEIT_INCLUDE_STOMP_MOVEIT_UTILS_KINEMATICS_H_ */
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
bool computeJacobianNullSpace(moveit::core::RobotStatePtr state, std::string group, std::string tool_link, const Eigen::ArrayXi &constrained_dofs, const Eigen::VectorXd &joint_pose, Eigen::MatrixXd &jacb_nullspace)
Convenience function to calculate the Jacobian&#39;s null space matrix for an under constrained tool cart...
Definition: kinematics.cpp:660
EigenSTL::vector_Affine3d sampleCartesianPoses(const moveit_msgs::Constraints &c, const std::vector< double > sampling_resolution={0.05, 0.05, 0.05, M_PI_2, M_PI_2, M_PI_2}, int max_samples=20)
Creates cartesian poses in accordance to the constraint and sampling resolution values.
Definition: kinematics.cpp:451
void reduceJacobian(const Eigen::MatrixXd &jacb, const std::vector< int > &indices, Eigen::MatrixXd &jacb_reduced)
Convenience function to remove entire rows of the Jacobian matrix.
Definition: kinematics.cpp:617
MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix)
#define M_PI
void computeTwist(const Eigen::Affine3d &p0, const Eigen::Affine3d &pf, const Eigen::ArrayXi &nullity, Eigen::VectorXd &twist)
Computes the twist vector [vx vy vz wx wy wz]&#39; relative to the current tool coordinate system...
Definition: kinematics.cpp:581
boost::optional< moveit_msgs::Constraints > curateCartesianConstraints(const moveit_msgs::Constraints &c, const Eigen::Affine3d &ref_pose, double default_pos_tol=0.0005, double default_rot_tol=M_PI)
Populates the missing parts of a Cartesian constraints in order to provide a constraint that can be u...
Definition: kinematics.cpp:240
#define M_PI_2
static const double LAMBDA
Used in dampening the matrix pseudo inverse calculation.
Definition: kinematics.h:62
const double EPSILON
bool isCartesianConstraints(const moveit_msgs::Constraints &c)
Checks if the constraint structured contains valid data from which a proper cartesian constraint can ...
Definition: kinematics.cpp:208
bool decodeCartesianConstraint(moveit::core::RobotModelConstPtr model, const moveit_msgs::Constraints &constraints, Eigen::Affine3d &tool_pose, Eigen::VectorXd &tolerance, std::string target_frame="")
Extracts the cartesian data from the constraint message.
Definition: kinematics.cpp:309
Wrapper around an IK solver implementation.
Definition: kinematics.h:70
void calculateDampedPseudoInverse(const Eigen::MatrixXd &jacb, Eigen::MatrixXd &jacb_pseudo_inv, double eps=0.011, double lambda=0.01)
Calculates the damped pseudo inverse of a matrix using singular value decomposition.
Definition: kinematics.cpp:628

stomp_moveit
Author(s): Jorge Nicho
autogenerated on Fri May 8 2020 03:35:47