chainiksolver_pos_nr_jl_mimic.hpp
Go to the documentation of this file.
1 // Copyright (C) 2007-2008 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
2 // Copyright (C) 2008 Mikael Mayer
3 // Copyright (C) 2008 Julia Jesse
4 
5 // Version: 1.0
6 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
7 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
8 // URL: http://www.orocos.org/kdl
9 
10 // This library is free software; you can redistribute it and/or
11 // modify it under the terms of the GNU Lesser General Public
12 // License as published by the Free Software Foundation; either
13 // version 2.1 of the License, or (at your option) any later version.
14 
15 // This library is distributed in the hope that it will be useful,
16 // but WITHOUT ANY WARRANTY; without even the implied warranty of
17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
18 // Lesser General Public License for more details.
19 
20 // You should have received a copy of the GNU Lesser General Public
21 // License along with this library; if not, write to the Free Software
22 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
23 
24 // Modified to account for "mimic" joints, i.e. joints whose motion has a
25 // linear relationship to that of another joint.
26 // Copyright (C) 2013 Sachin Chitta, Willow Garage
27 
28 #ifndef KDLCHAINIKSOLVERPOS_NR_JL_Mimic_HPP
29 #define KDLCHAINIKSOLVERPOS_NR_JL_Mimic_HPP
30 
31 #include "kdl/chainiksolver.hpp"
32 #include "kdl/chainfksolver.hpp"
33 
35 
36 namespace KDL
37 {
47 {
48 public:
68  double eps = 1e-6, bool position_ik = false);
69 
71 
72  virtual int CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out);
73 
74  virtual int CartToJntAdvanced(const JntArray& q_init, const Frame& p_in, JntArray& q_out, bool lock_redundant_joints);
75 
76  bool setMimicJoints(const std::vector<kdl_kinematics_plugin::JointMimic>& mimic_joints);
77 
78 private:
79  const Chain chain;
80  JntArray q_min; // These are the limits for the "reduced" state consisting of only active DOFs
81  JntArray q_min_mimic; // These are the limits for the full state
82  JntArray q_max; // These are the limits for the "reduced" state consisting of only active DOFs
83  JntArray q_max_mimic; // These are the limits for the full state
90  unsigned int maxiter;
91  double eps;
92  std::vector<kdl_kinematics_plugin::JointMimic> mimic_joints;
93  void qToqMimic(const JntArray& q,
94  JntArray& q_result); // Convert from the "reduced" state (only active DOFs) to the "full" state
95  void qMimicToq(const JntArray& q, JntArray& q_result); // Convert from the "full" state to the "reduced" state
97 };
98 }
99 
100 #endif
void qMimicToq(const JntArray &q, JntArray &q_result)
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)
virtual int CartToJntAdvanced(const JntArray &q_init, const Frame &p_in, JntArray &q_out, bool lock_redundant_joints)
std::vector< kdl_kinematics_plugin::JointMimic > mimic_joints
void qToqMimic(const JntArray &q, JntArray &q_result)
virtual int CartToJnt(const JntArray &q_init, const Frame &p_in, JntArray &q_out)
bool setMimicJoints(const std::vector< kdl_kinematics_plugin::JointMimic > &mimic_joints)


moveit_kinematics
Author(s): Dave Coleman , Ioan Sucan , Sachin Chitta
autogenerated on Sun Oct 18 2020 13:17:53