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 
172 {
173 protected:
174  double shou_m, shou_n;
175  double elb_m, elb_n;
176 
178  double hw_version;
179 
180  void clone(const iKinLinIneqConstr *obj);
181 
182 public:
188 
189  void update(void*);
190 };
191 
192 
199 {
200 private:
201  // Default constructor: not implemented.
202  iKinIpOptMin();
203  // Copy constructor: not implemented.
204  iKinIpOptMin(const iKinIpOptMin&);
205  // Assignment operator: not implemented.
206  iKinIpOptMin &operator=(const iKinIpOptMin&);
207 
208 protected:
209  void *App;
210 
213 
216 
217  unsigned int ctrlPose;
218 
219  double obj_scaling;
220  double x_scaling;
221  double g_scaling;
224  std::string posePriority;
225 
226 public:
248  iKinIpOptMin(iKinChain &c, const unsigned int _ctrlPose,
249  const double tol, const double constr_tol,
250  const int max_iter=IKINCTRL_DISABLED,
251  const unsigned int verbose=0, bool useHessian=true);
252 
260  void set_ctrlPose(const unsigned int _ctrlPose);
261 
266  unsigned int get_ctrlPose() const { return ctrlPose; }
267 
274  bool set_posePriority(const std::string &priority);
275 
280  std::string get_posePriority() const { return posePriority; }
281 
288  void attachLIC(iKinLinIneqConstr &lic) { pLIC=&lic; }
289 
296  iKinLinIneqConstr &getLIC() { return *pLIC; }
297 
304  void specify2ndTaskEndEff(const unsigned int n);
305 
310  iKinChain &get2ndTaskChain();
311 
317  void setMaxIter(const int max_iter);
318 
323  int getMaxIter() const;
324 
330  void setMaxCpuTime(const double max_cpu_time);
331 
336  double getMaxCpuTime() const;
337 
342  void setTol(const double tol);
343 
348  double getTol() const;
349 
354  void setConstrTol(const double constr_tol);
355 
360  double getConstrTol() const;
361 
369  void setVerbosity(const unsigned int verbose);
370 
377  void setHessianOpt(const bool useHessian);
378 
387  void setUserScaling(const bool useUserScaling, const double _obj_scaling,
388  const double _x_scaling, const double _g_scaling);
389 
398  void setDerivativeTest(const bool enableTest, const bool enable2ndDer=false);
399 
405  void getBoundsInf(double &lower, double &upper);
406 
412  void setBoundsInf(const double lower, const double upper);
413 
452  virtual yarp::sig::Vector solve(const yarp::sig::Vector &q0, yarp::sig::Vector &xd,
453  double weight2ndTask, yarp::sig::Vector &xd_2nd, yarp::sig::Vector &w_2nd,
454  double weight3rdTask, yarp::sig::Vector &qd_3rd, yarp::sig::Vector &w_3rd,
455  int *exit_code=NULL, bool *exhalt=NULL, iKinIterateCallback *iterate=NULL);
456 
471  virtual yarp::sig::Vector solve(const yarp::sig::Vector &q0, yarp::sig::Vector &xd,
472  double weight2ndTask, yarp::sig::Vector &xd_2nd, yarp::sig::Vector &w_2nd);
473 
480  virtual yarp::sig::Vector solve(const yarp::sig::Vector &q0, yarp::sig::Vector &xd);
481 
485  virtual ~iKinIpOptMin();
486 };
487 
488 }
489 
490 }
491 
492 #endif
493 
494 
#define IKINCTRL_DISABLED
Definition: iKinInv.h:50
iKinLinIneqConstr noLIC
Definition: iKinIpOpt.h:214
double & getLowerBoundInf()
Returns a reference to the internal representation of -inf.
Definition: iKinIpOpt.h:136
unsigned int ctrlPose
Definition: iKinIpOpt.h:217
Class for dealing with additional iCub arm&#39;s constraints.
Definition: iKinIpOpt.h:171
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:288
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:224
iKinLinIneqConstr * pLIC
Definition: iKinIpOpt.h:215
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:280
bool lower(Value &a, Value &b)
Definition: main.cpp:337
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
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:296
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:198
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:266