iCub-main
iKinIpOpt.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT)
3  * Copyright (C) 2006-2010 RobotCub Consortium
4  * All rights reserved.
5  *
6  * This software may be modified and distributed under the terms
7  * of the BSD-3-Clause license. See the accompanying LICENSE file for
8  * details.
9 */
10 
26 #ifndef __IKINIPOPT_H__
27 #define __IKINIPOPT_H__
28 
29 #include <iCub/iKin/iKinInv.h>
30 
31 
32 namespace iCub
33 {
34 
35 namespace iKin
36 {
37 
44 {
45 private:
46  // Copy constructor: not implemented.
48  // Assignment operator: not implemented.
49  iKinIterateCallback &operator=(const iKinIterateCallback&);
50 
51 public:
53 
59  virtual void exec(const yarp::sig::Vector &xd, const yarp::sig::Vector &q) = 0;
60 };
61 
62 
70 {
71 protected:
72  yarp::sig::Matrix C;
73  yarp::sig::Vector uB;
74  yarp::sig::Vector lB;
75 
76  double lowerBoundInf;
77  double upperBoundInf;
78  bool active;
79 
80  virtual void clone(const iKinLinIneqConstr *obj);
81 
82 public:
87 
97  iKinLinIneqConstr(const double _lowerBoundInf, const double _upperBoundInf);
98 
105 
112  virtual iKinLinIneqConstr &operator=(const iKinLinIneqConstr &obj);
113 
118  yarp::sig::Matrix &getC() { return C; }
119 
124  yarp::sig::Vector &getuB() { return uB; }
125 
130  yarp::sig::Vector &getlB() { return lB; }
131 
136  double &getLowerBoundInf() { return lowerBoundInf; }
137 
142  double &getUpperBoundInf() { return upperBoundInf; }
143 
148  bool isActive() { return active; }
149 
154  void setActive(bool _active) { active=_active; }
155 
162  virtual void update(void*) { }
163 };
164 
165 
173 {
174 protected:
175  double shou_m, shou_n;
176  double elb_m, elb_n;
178 
179  void clone(const iKinLinIneqConstr *obj);
180 
181 public:
187 
188  void update(void*);
189 };
190 
191 
198 {
199 private:
200  // Default constructor: not implemented.
201  iKinIpOptMin();
202  // Copy constructor: not implemented.
203  iKinIpOptMin(const iKinIpOptMin&);
204  // Assignment operator: not implemented.
205  iKinIpOptMin &operator=(const iKinIpOptMin&);
206 
207 protected:
208  void *App;
209 
212 
215 
216  unsigned int ctrlPose;
217 
218  double obj_scaling;
219  double x_scaling;
220  double g_scaling;
223  std::string posePriority;
224 
225 public:
247  iKinIpOptMin(iKinChain &c, const unsigned int _ctrlPose,
248  const double tol, const double constr_tol,
249  const int max_iter=IKINCTRL_DISABLED,
250  const unsigned int verbose=0, bool useHessian=true);
251 
259  void set_ctrlPose(const unsigned int _ctrlPose);
260 
265  unsigned int get_ctrlPose() const { return ctrlPose; }
266 
273  bool set_posePriority(const std::string &priority);
274 
279  std::string get_posePriority() const { return posePriority; }
280 
287  void attachLIC(iKinLinIneqConstr &lic) { pLIC=&lic; }
288 
295  iKinLinIneqConstr &getLIC() { return *pLIC; }
296 
303  void specify2ndTaskEndEff(const unsigned int n);
304 
309  iKinChain &get2ndTaskChain();
310 
316  void setMaxIter(const int max_iter);
317 
322  int getMaxIter() const;
323 
329  void setMaxCpuTime(const double max_cpu_time);
330 
335  double getMaxCpuTime() const;
336 
341  void setTol(const double tol);
342 
347  double getTol() const;
348 
353  void setConstrTol(const double constr_tol);
354 
359  double getConstrTol() const;
360 
368  void setVerbosity(const unsigned int verbose);
369 
376  void setHessianOpt(const bool useHessian);
377 
386  void setUserScaling(const bool useUserScaling, const double _obj_scaling,
387  const double _x_scaling, const double _g_scaling);
388 
397  void setDerivativeTest(const bool enableTest, const bool enable2ndDer=false);
398 
404  void getBoundsInf(double &lower, double &upper);
405 
411  void setBoundsInf(const double lower, const double upper);
412 
451  virtual yarp::sig::Vector solve(const yarp::sig::Vector &q0, yarp::sig::Vector &xd,
452  double weight2ndTask, yarp::sig::Vector &xd_2nd, yarp::sig::Vector &w_2nd,
453  double weight3rdTask, yarp::sig::Vector &qd_3rd, yarp::sig::Vector &w_3rd,
454  int *exit_code=NULL, bool *exhalt=NULL, iKinIterateCallback *iterate=NULL);
455 
470  virtual yarp::sig::Vector solve(const yarp::sig::Vector &q0, yarp::sig::Vector &xd,
471  double weight2ndTask, yarp::sig::Vector &xd_2nd, yarp::sig::Vector &w_2nd);
472 
479  virtual yarp::sig::Vector solve(const yarp::sig::Vector &q0, yarp::sig::Vector &xd);
480 
484  virtual ~iKinIpOptMin();
485 };
486 
487 }
488 
489 }
490 
491 #endif
492 
493 
#define IKINCTRL_DISABLED
Definition: iKinInv.h:50
iKinLinIneqConstr noLIC
Definition: iKinIpOpt.h:213
double & getLowerBoundInf()
Returns a reference to the internal representation of -inf.
Definition: iKinIpOpt.h:136
unsigned int ctrlPose
Definition: iKinIpOpt.h:216
A Base class for defining a Serial Link Chain.
Definition: iKinFwd.h:354
int n
void attachLIC(iKinLinIneqConstr &lic)
Attach a iKinLinIneqConstr object in order to impose constraints of the form lB <= C*q <= uB...
Definition: iKinIpOpt.h:287
yarp::sig::Vector & getuB()
Returns a reference to the upper bounds vector uB.
Definition: iKinIpOpt.h:124
Class for defining Linear Inequality Constraints of the form lB <= C*q <= uB for the nonlinear proble...
Definition: iKinIpOpt.h:69
virtual void update(void *)
Updates internal state.
Definition: iKinIpOpt.h:162
yarp::sig::Vector lB
Definition: iKinIpOpt.h:74
std::string posePriority
Definition: iKinIpOpt.h:223
iKinLinIneqConstr * pLIC
Definition: iKinIpOpt.h:214
Class for defining iteration callback.
Definition: iKinIpOpt.h:43
yarp::sig::Matrix & getC()
Returns a reference to the constraints matrix C.
Definition: iKinIpOpt.h:118
virtual void exec(const yarp::sig::Vector &xd, const yarp::sig::Vector &q)=0
Defines the callback body to be called at each iteration.
std::string get_posePriority() const
Returns the Pose priority settings.
Definition: iKinIpOpt.h:279
bool lower(Value &a, Value &b)
Definition: main.cpp:336
double & getUpperBoundInf()
Returns a reference to the internal representation of +inf.
Definition: iKinIpOpt.h:142
yarp::sig::Vector & getlB()
Returns a reference to the lower bounds vector lB.
Definition: iKinIpOpt.h:130
yarp::sig::Vector uB
Definition: iKinIpOpt.h:73
Class for dealing with iCub shoulder&#39;s constraints due to the cables length.
Definition: iKinIpOpt.h:172
This file contains the definition of unique IDs for the body parts and the skin parts of the robot...
A class for defining the iCub Arm.
Definition: iKinFwd.h:1081
iKinLinIneqConstr & getLIC()
Returns a reference to the attached Linear Inequality Constraints object.
Definition: iKinIpOpt.h:295
bool isActive()
Returns the state of inequality constraints evaluation.
Definition: iKinIpOpt.h:148
Class for inverting chain&#39;s kinematics based on IpOpt lib.
Definition: iKinIpOpt.h:197
void setActive(bool _active)
Sets the state of inequality constraints evaluation.
Definition: iKinIpOpt.h:154
yarp::sig::Matrix C
Definition: iKinIpOpt.h:72
unsigned int get_ctrlPose() const
Returns the state of Pose control settings.
Definition: iKinIpOpt.h:265