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;
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 
281 
288  void attachLIC(iKinLinIneqConstr &lic) { pLIC=&lic; }
289 
296  iKinLinIneqConstr &getLIC() { return *pLIC; }
297 
304  void specify2ndTaskEndEff(const unsigned int n);
305 
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 
iCub::iKin::iKinLinIneqConstr::getuB
yarp::sig::Vector & getuB()
Returns a reference to the upper bounds vector uB.
Definition: iKinIpOpt.h:124
iCub
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.
Definition: emotionInterface.h:17
iCub::iKin::iKinIpOptMin::getLIC
iKinLinIneqConstr & getLIC()
Returns a reference to the attached Linear Inequality Constraints object.
Definition: iKinIpOpt.h:296
iCub::iKin::iKinIpOptMin::setUserScaling
void setUserScaling(const bool useUserScaling, const double _obj_scaling, const double _x_scaling, const double _g_scaling)
Enables/disables user scaling factors.
Definition: iKinIpOpt.cpp:973
iCub::iKin::iKinLinIneqConstr
Definition: iKinIpOpt.h:69
iCub::iKin::iKinIpOptMin::attachLIC
void attachLIC(iKinLinIneqConstr &lic)
Attach a iKinLinIneqConstr object in order to impose constraints of the form lB <= C*q <= uB.
Definition: iKinIpOpt.h:288
iKinInv.h
iCub::iKin::iKinIpOptMin::get_ctrlPose
unsigned int get_ctrlPose() const
Returns the state of Pose control settings.
Definition: iKinIpOpt.h:266
iCub::iKin::iKinIpOptMin::chain2ndTask
iKinChain chain2ndTask
Definition: iKinIpOpt.h:212
iCub::iKin::iKinChain
Definition: iKinFwd.h:354
IKINCTRL_DISABLED
#define IKINCTRL_DISABLED
Definition: iKinInv.h:50
iCub::iKin::iKinIpOptMin
Definition: iKinIpOpt.h:198
iCub::iKin::iKinIterateCallback
Definition: iKinIpOpt.h:43
iCub::iKin::iKinIpOptMin::x_scaling
double x_scaling
Definition: iKinIpOpt.h:220
iCub::iKin::iCubAdditionalArmConstraints
Definition: iKinIpOpt.h:171
iCub::iKin::iKinIpOptMin::posePriority
std::string posePriority
Definition: iKinIpOpt.h:224
iCub::iKin::iKinLinIneqConstr::isActive
bool isActive()
Returns the state of inequality constraints evaluation.
Definition: iKinIpOpt.h:148
iCub::iKin::iCubAdditionalArmConstraints::iCubAdditionalArmConstraints
iCubAdditionalArmConstraints(iCubArm &arm)
Constructor.
Definition: iKinIpOpt.cpp:97
iCub::iKin::iCubAdditionalArmConstraints::shou_m
double shou_m
Definition: iKinIpOpt.h:174
iCub::iKin::iKinLinIneqConstr::clone
virtual void clone(const iKinLinIneqConstr *obj)
Definition: iKinIpOpt.cpp:51
iCub::iKin::iKinIpOptMin::setMaxCpuTime
void setMaxCpuTime(const double max_cpu_time)
Sets Maximum CPU seconds.
Definition: iKinIpOpt.cpp:901
iCub::iKin::iKinLinIneqConstr::uB
yarp::sig::Vector uB
Definition: iKinIpOpt.h:73
iCub::iKin::iCubArm
Definition: iKinFwd.h:1081
relationalOperators::lower
bool lower(Value &a, Value &b)
Definition: main.cpp:337
iCub::iKin::iKinLinIneqConstr::C
yarp::sig::Matrix C
Definition: iKinIpOpt.h:72
iCub::iKin::iKinIpOptMin::g_scaling
double g_scaling
Definition: iKinIpOpt.h:221
iCub::iKin::iKinIpOptMin::set_ctrlPose
void set_ctrlPose(const unsigned int _ctrlPose)
Sets the state of Pose control settings.
Definition: iKinIpOpt.cpp:833
iCub::iKin::iKinIpOptMin::setVerbosity
void setVerbosity(const unsigned int verbose)
Sets Verbosity.
Definition: iKinIpOpt.cpp:952
iCub::iKin::iKinIterateCallback::exec
virtual void exec(const yarp::sig::Vector &xd, const yarp::sig::Vector &q)=0
Defines the callback body to be called at each iteration.
iCub::iKin::iKinIpOptMin::upperBoundInf
double upperBoundInf
Definition: iKinIpOpt.h:223
iCub::iKin::iKinIpOptMin::ctrlPose
unsigned int ctrlPose
Definition: iKinIpOpt.h:217
iCub::iKin::iKinIpOptMin::obj_scaling
double obj_scaling
Definition: iKinIpOpt.h:219
iCub::iKin::iKinIpOptMin::~iKinIpOptMin
virtual ~iKinIpOptMin()
Default destructor.
Definition: iKinIpOpt.cpp:1074
iCub::iKin::iKinIpOptMin::get2ndTaskChain
iKinChain & get2ndTaskChain()
Retrieves the 2nd task's chain.
Definition: iKinIpOpt.cpp:873
iCub::iKin::iKinLinIneqConstr::setActive
void setActive(bool _active)
Sets the state of inequality constraints evaluation.
Definition: iKinIpOpt.h:154
iCub::iKin::iKinIpOptMin::getMaxCpuTime
double getMaxCpuTime() const
Retrieves the current value of Maximum CPU seconds.
Definition: iKinIpOpt.cpp:909
n
int n
Definition: debugFunctions.cpp:58
iCub::iKin::iKinLinIneqConstr::getUpperBoundInf
double & getUpperBoundInf()
Returns a reference to the internal representation of +inf.
Definition: iKinIpOpt.h:142
iCub::iKin::iKinIpOptMin::getConstrTol
double getConstrTol() const
Retrieves constraints tolerance.
Definition: iKinIpOpt.cpp:943
iCub::iKin::iKinIpOptMin::setDerivativeTest
void setDerivativeTest(const bool enableTest, const bool enable2ndDer=false)
Enable\disable derivative test at each call to solve method (disabled at start-up by default).
Definition: iKinIpOpt.cpp:992
iCub::iKin::iKinIterateCallback::iKinIterateCallback
iKinIterateCallback()
Definition: iKinIpOpt.h:52
iCub::iKin::iCubAdditionalArmConstraints::hw_version
double hw_version
Definition: iKinIpOpt.h:178
iCub::iKin::iKinIpOptMin::setBoundsInf
void setBoundsInf(const double lower, const double upper)
Sets the lower and upper bounds to represent -inf and +inf.
Definition: iKinIpOpt.cpp:1019
iCub::iKin::iKinLinIneqConstr::getlB
yarp::sig::Vector & getlB()
Returns a reference to the lower bounds vector lB.
Definition: iKinIpOpt.h:130
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
iCub::iKin::iKinLinIneqConstr::update
virtual void update(void *)
Updates internal state.
Definition: iKinIpOpt.h:162
iCub::iKin::iKinIpOptMin::noLIC
iKinLinIneqConstr noLIC
Definition: iKinIpOpt.h:214
iCub::iKin::iKinLinIneqConstr::lB
yarp::sig::Vector lB
Definition: iKinIpOpt.h:74
iCub::iKin::iKinLinIneqConstr::active
bool active
Definition: iKinIpOpt.h:78
iCub::iKin::iKinIpOptMin::set_posePriority
bool set_posePriority(const std::string &priority)
Sets the Pose priority for weighting more either position or orientation while reaching in full pose.
Definition: iKinIpOpt.cpp:843
iCub::iKin::iKinLinIneqConstr::getLowerBoundInf
double & getLowerBoundInf()
Returns a reference to the internal representation of -inf.
Definition: iKinIpOpt.h:136
iCub::iKin::iKinLinIneqConstr::getC
yarp::sig::Matrix & getC()
Returns a reference to the constraints matrix C.
Definition: iKinIpOpt.h:118
iCub::iKin::iCubAdditionalArmConstraints::clone
void clone(const iKinLinIneqConstr *obj)
Definition: iKinIpOpt.cpp:80
iCub::iKin::iCubAdditionalArmConstraints::elb_m
double elb_m
Definition: iKinIpOpt.h:175
iCub::iKin::iKinIpOptMin::getBoundsInf
void getBoundsInf(double &lower, double &upper)
Returns the lower and upper bounds to represent -inf and +inf.
Definition: iKinIpOpt.cpp:1011
iCub::iKin::iCubAdditionalArmConstraints::elb_n
double elb_n
Definition: iKinIpOpt.h:175
iCub::iKin::iKinLinIneqConstr::operator=
virtual iKinLinIneqConstr & operator=(const iKinLinIneqConstr &obj)
Copies a LinIneqConstr object into the current one.
Definition: iKinIpOpt.cpp:71
iCub::iKin::iCubAdditionalArmConstraints::shou_n
double shou_n
Definition: iKinIpOpt.h:174
iCub::iKin::iKinIpOptMin::lowerBoundInf
double lowerBoundInf
Definition: iKinIpOpt.h:222
iCub::iKin::iKinLinIneqConstr::iKinLinIneqConstr
iKinLinIneqConstr()
Default Constructor.
Definition: iKinIpOpt.cpp:32
iCub::iKin::iKinIpOptMin::getMaxIter
int getMaxIter() const
Retrieves the current value of Maximum Iteration.
Definition: iKinIpOpt.cpp:892
iCub::iKin::iKinIpOptMin::setConstrTol
void setConstrTol(const double constr_tol)
Sets constraints tolerance.
Definition: iKinIpOpt.cpp:935
iCub::iKin::iKinLinIneqConstr::upperBoundInf
double upperBoundInf
Definition: iKinIpOpt.h:77
iCub::iKin::iKinIpOptMin::specify2ndTaskEndEff
void specify2ndTaskEndEff(const unsigned int n)
Selects the End-Effector of the 2nd task by giving the ordinal number n of last joint pointing at it.
Definition: iKinIpOpt.cpp:856
iCub::iKin::iKinIpOptMin::App
void * App
Definition: iKinIpOpt.h:209
iCub::iKin::iKinIpOptMin::pLIC
iKinLinIneqConstr * pLIC
Definition: iKinIpOpt.h:215
iCub::iKin::iKinIpOptMin::get_posePriority
std::string get_posePriority() const
Returns the Pose priority settings.
Definition: iKinIpOpt.h:280
iCub::iKin::iKinIpOptMin::chain
iKinChain & chain
Definition: iKinIpOpt.h:211
iCub::iKin::iKinIpOptMin::solve
virtual yarp::sig::Vector solve(const yarp::sig::Vector &q0, yarp::sig::Vector &xd, double weight2ndTask, yarp::sig::Vector &xd_2nd, yarp::sig::Vector &w_2nd, double weight3rdTask, yarp::sig::Vector &qd_3rd, yarp::sig::Vector &w_3rd, int *exit_code=NULL, bool *exhalt=NULL, iKinIterateCallback *iterate=NULL)
Executes the IpOpt algorithm trying to converge on target.
Definition: iKinIpOpt.cpp:1030
iCub::iKin::iKinIpOptMin::setHessianOpt
void setHessianOpt(const bool useHessian)
Selects whether to rely on exact Hessian computation or enable Quasi-Newton approximation (Hessian is...
Definition: iKinIpOpt.cpp:961
iCub::iKin::iKinIpOptMin::setTol
void setTol(const double tol)
Sets cost function tolerance.
Definition: iKinIpOpt.cpp:918
iCub::iKin::iKinIpOptMin::setMaxIter
void setMaxIter(const int max_iter)
Sets Maximum Iteration.
Definition: iKinIpOpt.cpp:880
iCub::iKin::iCubAdditionalArmConstraints::chain
iKinChain * chain
Definition: iKinIpOpt.h:177
iCub::iKin::iKinLinIneqConstr::lowerBoundInf
double lowerBoundInf
Definition: iKinIpOpt.h:76
iCub::iKin::iCubAdditionalArmConstraints::update
void update(void *)
Updates internal state.
Definition: iKinIpOpt.cpp:128
iCub::iKin::iKinIpOptMin::getTol
double getTol() const
Retrieves cost function tolerance.
Definition: iKinIpOpt.cpp:926