openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
iksolver.h
Go to the documentation of this file.
1 // -*- coding: utf-8 -*-
2 // Copyright (C) 2006-2012 Rosen Diankov <rosen.diankov@gmail.com>
3 //
4 // This file is part of OpenRAVE.
5 // OpenRAVE is free software: you can redistribute it and/or modify
6 // it under the terms of the GNU Lesser General Public License as published by
7 // the Free Software Foundation, either version 3 of the License, or
8 // at your option) any later version.
9 //
10 // This program is distributed in the hope that it will be useful,
11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 // GNU Lesser General Public License for more details.
14 //
15 // You should have received a copy of the GNU Lesser General Public License
16 // along with this program. If not, see <http://www.gnu.org/licenses/>.
22 #ifndef OPENRAVE_IKSOLVER_H
23 #define OPENRAVE_IKSOLVER_H
24 
25 namespace OpenRAVE {
26 
29 {
37 };
38 
41 {
44  IKRA_Quit = 2,
45 
46  // reasons why rejected
53  IKRA_RejectCustomFilter = (IKRA_Reject|0x00008000), // the reason should be set in the upper 16 bits
54 };
55 
57 static const IkReturnAction IKFR_Success RAVE_DEPRECATED = IKRA_Success;
58 static const IkReturnAction IKFR_Reject RAVE_DEPRECATED = IKRA_Reject;
59 static const IkReturnAction IKFR_Quit RAVE_DEPRECATED = IKRA_Quit;
60 typedef IkReturnAction IkFilterReturn RAVE_DEPRECATED;
61 
63 {
64 public:
65  IkReturn(IkReturnAction action) : _action(action) {
66  }
67 
68  inline bool operator != (IkReturnAction action) const {
69  return _action != action;
70  }
71  inline bool operator == (IkReturnAction action) const {
72  return _action == action;
73  }
74 
79  bool Append(const IkReturn& r);
80 
82  void Clear();
83 
84  typedef std::map<std::string, std::vector<dReal> > CustomData;
86  std::vector< dReal > _vsolution;
89 };
90 
95 {
96 public:
109  typedef boost::function<IkReturn(std::vector<dReal>&, RobotBase::ManipulatorConstPtr, const IkParameterization&)> IkFilterCallbackFn;
110 
111  IkSolverBase(EnvironmentBasePtr penv) : InterfaceBase(PT_InverseKinematicsSolver, penv) {
112  }
113  virtual ~IkSolverBase() {
114  }
115 
117  static inline InterfaceType GetInterfaceTypeStatic() {
118  return PT_InverseKinematicsSolver;
119  }
120 
125  virtual bool Init(RobotBase::ManipulatorConstPtr pmanip) = 0;
126 
127  virtual RobotBase::ManipulatorPtr GetManipulator() const = 0;
128 
136  virtual UserDataPtr RegisterCustomFilter(int priority, const IkFilterCallbackFn& filterfn);
137 
139  virtual void SetCustomFilter(const IkFilterCallbackFn& filterfn) RAVE_DEPRECATED
140  {
141  RAVELOG_WARN("IkSolverBase::SetCustomFilter is deprecated, have to use handle=AddCustomFilter. This call will will leak memory\n");
142  if( __listRegisteredFilters.size() > 0 ) {
143  RAVELOG_WARN("IkSolverBase::SetCustomFilter is deprecated, deleting all current filters!\n");
144  }
145  new UserDataPtr(RegisterCustomFilter(0,filterfn));
146  }
147 
151  virtual int GetNumFreeParameters() const = 0;
152 
158  virtual bool GetFreeParameters(std::vector<dReal>& vFreeParameters) const = 0;
159 
169  virtual bool Solve(const IkParameterization& param, const std::vector<dReal>& q0, int filteroptions, boost::shared_ptr< std::vector<dReal> > solution = boost::shared_ptr< std::vector<dReal> >()) = 0;
170 
180  virtual bool Solve(const IkParameterization& param, const std::vector<dReal>& q0, int filteroptions, IkReturnPtr ikreturn);
181 
190  virtual bool SolveAll(const IkParameterization& param, int filteroptions, std::vector< std::vector<dReal> >& solutions) = 0;
191 
193  virtual bool Solve(const IkParameterization& param, int filteroptions, std::vector< std::vector<dReal> >& solutions) RAVE_DEPRECATED {
194  return SolveAll(param,filteroptions,solutions);
195  }
196 
205  virtual bool SolveAll(const IkParameterization& param, int filteroptions, std::vector<IkReturnPtr>& ikreturns);
206 
217  virtual bool Solve(const IkParameterization& param, const std::vector<dReal>& q0, const std::vector<dReal>& vFreeParameters, int filteroptions, boost::shared_ptr< std::vector<dReal> > solution=boost::shared_ptr< std::vector<dReal> >()) = 0;
218 
229  virtual bool Solve(const IkParameterization& param, const std::vector<dReal>& q0, const std::vector<dReal>& vFreeParameters, int filteroptions, IkReturnPtr ikreturn);
230 
241  virtual bool SolveAll(const IkParameterization& param, const std::vector<dReal>& vFreeParameters, int filteroptions, std::vector< std::vector<dReal> >& solutions) = 0;
242 
244  virtual bool Solve(const IkParameterization& param, const std::vector<dReal>& vFreeParameters, int filteroptions, std::vector< std::vector<dReal> >& solutions) RAVE_DEPRECATED {
245  return SolveAll(param,vFreeParameters,filteroptions,solutions);
246  }
247 
258  virtual bool SolveAll(const IkParameterization& param, const std::vector<dReal>& vFreeParameters, int filteroptions, std::vector<IkReturnPtr>& ikreturns);
259 
261  virtual bool Supports(IkParameterizationType iktype) const OPENRAVE_DUMMY_IMPLEMENTATION;
262 
263 protected:
265  return boost::static_pointer_cast<IkSolverBase>(shared_from_this());
266  }
268  return boost::static_pointer_cast<IkSolverBase const>(shared_from_this());
269  }
270 
275  // to be optimized away.
276  virtual IkReturnAction _CallFilters(std::vector<dReal>& solution, RobotBase::ManipulatorPtr manipulator, const IkParameterization& param, IkReturnPtr ikreturn=IkReturnPtr());
277 
278 private:
279  virtual const char* GetHash() const {
280  return OPENRAVE_IKSOLVER_HASH;
281  }
282 
283  std::list<UserDataWeakPtr> __listRegisteredFilters;
284 
286 };
287 
288 } // end namespace OpenRAVE
289 
290 #endif