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 #ifndef KDL_CHAIN_IKSOLVERVEL_PINV_Mimic_HPP
00027 #define KDL_CHAIN_IKSOLVERVEL_PINV_Mimic_HPP
00028
00029 #include "kdl/chainiksolver.hpp"
00030 #include "kdl/chainjnttojacsolver.hpp"
00031 #include "kdl/utilities/svd_HH.hpp"
00032 #include "kdl/utilities/svd_eigen_HH.hpp"
00033
00034 #include <moveit/kdl_kinematics_plugin/joint_mimic.hpp>
00035
00036 namespace KDL
00037 {
00047 class ChainIkSolverVel_pinv_mimic : public ChainIkSolverVel
00048 {
00049 public:
00066 explicit ChainIkSolverVel_pinv_mimic(const Chain& chain, int num_mimic_joints =0, int num_redundant_joints =0, bool position_ik=false, double eps=0.00001, int maxiter=150);
00067
00068 ~ChainIkSolverVel_pinv_mimic();
00069
00070 virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
00071
00072 virtual int CartToJntRedundant(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
00073
00078 virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out){return -1;};
00079
00088 bool setMimicJoints(const std::vector<kdl_kinematics_plugin::JointMimic> &_mimic_joints);
00089
00097 bool setRedundantJointsMapIndex(const std::vector<unsigned int> & redundant_joints_map_index);
00098
00099 void lockRedundantJoints()
00100 {
00101 redundant_joints_locked = true;
00102 }
00103
00104 void unlockRedundantJoints()
00105 {
00106 redundant_joints_locked = false;
00107 }
00108
00109 private:
00110
00111 bool jacToJacReduced(const Jacobian &jac, Jacobian &jac_mimic);
00112 bool jacToJacLocked(const Jacobian &jac, Jacobian &jac_locked);
00113
00114 const Chain chain;
00115 ChainJntToJacSolver jnt2jac;
00116
00117
00118
00119 Jacobian jac;
00120 std::vector<JntArray> U;
00121 JntArray S;
00122 std::vector<JntArray> V;
00123 JntArray tmp;
00124
00125
00126
00127 Jacobian jac_reduced;
00128 JntArray qdot_out_reduced;
00129
00130
00131 Eigen::MatrixXd U_translate;
00132 Eigen::VectorXd S_translate;
00133 Eigen::MatrixXd V_translate;
00134 Eigen::VectorXd tmp_translate;
00135
00136
00137 Jacobian jac_locked;
00138 JntArray qdot_out_reduced_locked, qdot_out_locked;
00139
00140 SVD_HH svd;
00141 double eps;
00142 int maxiter;
00143
00144
00145 std::vector<kdl_kinematics_plugin::JointMimic> mimic_joints_;
00146 int num_mimic_joints;
00147
00148 bool position_ik;
00149
00150
00151
00152 Eigen::MatrixXd U_locked;
00153 Eigen::VectorXd S_locked;
00154 Eigen::MatrixXd V_locked;
00155 Eigen::VectorXd tmp_locked;
00156
00157
00158
00159 Eigen::MatrixXd U_translate_locked;
00160 Eigen::VectorXd S_translate_locked;
00161 Eigen::MatrixXd V_translate_locked;
00162 Eigen::VectorXd tmp_translate_locked;
00163
00164
00165 std::vector<unsigned int> locked_joints_map_index;
00166 unsigned int num_redundant_joints;
00167 bool redundant_joints_locked;
00168
00169
00170 };
00171 }
00172 #endif