nlopt_ik.hpp
Go to the documentation of this file.
1 /********************************************************************************
2 Copyright (c) 2015, TRACLabs, Inc.
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without modification,
6  are permitted provided that the following conditions are met:
7 
8  1. Redistributions of source code must retain the above copyright notice,
9  this list of conditions and the following disclaimer.
10 
11  2. Redistributions in binary form must reproduce the above copyright notice,
12  this list of conditions and the following disclaimer in the documentation
13  and/or other materials provided with the distribution.
14 
15  3. Neither the name of the copyright holder nor the names of its contributors
16  may be used to endorse or promote products derived from this software
17  without specific prior written permission.
18 
19 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
20 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
21 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
22 IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
23 INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
24 BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
25 DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
26 LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
27 OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
28 OF THE POSSIBILITY OF SUCH DAMAGE.
29 ********************************************************************************/
30 
31 #ifndef NLOPT_IK_HPP
32 #define NLOPT_IK_HPP
33 
34 #include <trac_ik/kdl_tl.hpp>
35 #include <nlopt.hpp>
36 
37 
38 namespace NLOPT_IK
39 {
40 
42 
43 
44 class NLOPT_IK
45 {
46  friend class TRAC_IK::TRAC_IK;
47 public:
48  NLOPT_IK(const KDL::Chain& chain, const KDL::JntArray& q_min, const KDL::JntArray& q_max, double maxtime = 0.005, double eps = 1e-3, OptType type = SumSq);
49 
50  ~NLOPT_IK() {};
51  int CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& p_in, KDL::JntArray& q_out, const KDL::Twist bounds = KDL::Twist::Zero(), const KDL::JntArray& q_desired = KDL::JntArray());
52 
53  double minJoints(const std::vector<double>& x, std::vector<double>& grad);
54  // void cartFourPointError(const std::vector<double>& x, double error[]);
55  void cartSumSquaredError(const std::vector<double>& x, double error[]);
56  void cartDQError(const std::vector<double>& x, double error[]);
57  void cartL2NormError(const std::vector<double>& x, double error[]);
58 
59  inline void setMaxtime(double t)
60  {
61  maxtime = t;
62  }
63 
64 private:
65 
66  inline void abort()
67  {
68  aborted = true;
69  }
70 
71  inline void reset()
72  {
73  aborted = false;
74  }
75 
76 
77  std::vector<double> lb;
78  std::vector<double> ub;
79 
81  std::vector<double> des;
82 
83 
85 
86  double maxtime;
87  double eps;
90 
98 
99  std::vector<KDL::BasicJointType> types;
100 
101  nlopt::opt opt;
102 
104 
105  std::vector<double> best_x;
106  int progress;
107  bool aborted;
108 
110 
111  inline static double fRand(double min, double max)
112  {
113  double f = (double)rand() / RAND_MAX;
114  return min + f * (max - min);
115  }
116 
117 
118 };
119 
120 }
121 
122 #endif
static Twist Zero()
const KDL::Chain chain
Definition: nlopt_ik.hpp:80
KDL::Twist bounds
Definition: trac_ik.hpp:141
f
KDL::Chain chain
Definition: trac_ik.hpp:111
std::vector< double > ub
Definition: nlopt_ik.hpp:78
KDL::Frame targetPose
Definition: nlopt_ik.hpp:91
std::vector< KDL::BasicJointType > types
Definition: nlopt_ik.hpp:99
std::vector< double > lb
Definition: nlopt_ik.hpp:77
void setMaxtime(double t)
Definition: nlopt_ik.hpp:59
int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out, const KDL::Twist &bounds=KDL::Twist::Zero())
Definition: trac_ik.cpp:415
nlopt::opt opt
Definition: nlopt_ik.hpp:101
KDL::Twist bounds
Definition: nlopt_ik.hpp:109
KDL::Frame y_target
Definition: nlopt_ik.hpp:97
std::vector< double > des
Definition: nlopt_ik.hpp:81
static double fRand(double min, double max)
Definition: nlopt_ik.hpp:111
KDL::Frame z_up
Definition: nlopt_ik.hpp:92
KDL::Frame x_target
Definition: nlopt_ik.hpp:96
KDL::Frame x_out
Definition: nlopt_ik.hpp:93
KDL::ChainFkSolverPos_recursive fksolver
Definition: nlopt_ik.hpp:84
KDL::Frame z_target
Definition: nlopt_ik.hpp:95
KDL::Frame y_out
Definition: nlopt_ik.hpp:94
std::vector< double > best_x
Definition: nlopt_ik.hpp:105
KDL::Frame currentPose
Definition: nlopt_ik.hpp:103


trac_ik_lib
Author(s): Patrick Beeson, Barrett Ames
autogenerated on Sat Sep 19 2020 03:40:57