Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifndef KDLCHAINIKSOLVERPOS_NR_JL_Mimic_HPP
00029 #define KDLCHAINIKSOLVERPOS_NR_JL_Mimic_HPP
00030
00031 #include "kdl/chainiksolver.hpp"
00032 #include "kdl/chainfksolver.hpp"
00033
00034 #include <moveit/kdl_kinematics_plugin/joint_mimic.hpp>
00035
00036 namespace KDL
00037 {
00038
00047 class ChainIkSolverPos_NR_JL_Mimic : public ChainIkSolverPos
00048 {
00049 public:
00067 ChainIkSolverPos_NR_JL_Mimic(const Chain& chain,const JntArray& q_min, const JntArray& q_max, ChainFkSolverPos& fksolver,ChainIkSolverVel& iksolver,unsigned int maxiter=100,double eps=1e-6, bool position_ik = false);
00068
00069 ~ChainIkSolverPos_NR_JL_Mimic();
00070
00071 virtual int CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out);
00072
00073 virtual int CartToJntAdvanced(const JntArray& q_init, const Frame& p_in, JntArray& q_out, bool lock_redundant_joints);
00074
00075 bool setMimicJoints(const std::vector<kdl_kinematics_plugin::JointMimic>& mimic_joints);
00076
00077 private:
00078 const Chain chain;
00079 JntArray q_min;
00080 JntArray q_min_mimic;
00081 JntArray q_max;
00082 JntArray q_max_mimic;
00083 JntArray q_temp;
00084 ChainFkSolverPos& fksolver;
00085 ChainIkSolverVel& iksolver;
00086 JntArray delta_q;
00087 Frame f;
00088 Twist delta_twist;
00089 unsigned int maxiter;
00090 double eps;
00091 std::vector<kdl_kinematics_plugin::JointMimic> mimic_joints;
00092 void qToqMimic(const JntArray& q, JntArray& q_result);
00093 void qMimicToq(const JntArray& q, JntArray& q_result);
00094 bool position_ik;
00095
00096 };
00097
00098 }
00099
00100 #endif