iCub-main
Public Member Functions | Protected Attributes | List of all members
iCub::iKin::iKinIpOptMin Class Reference

Class for inverting chain's kinematics based on IpOpt lib. More...

#include <iKinIpOpt.h>

+ Inheritance diagram for iCub::iKin::iKinIpOptMin:

Public Member Functions

 iKinIpOptMin (iKinChain &c, const unsigned int _ctrlPose, const double tol, const double constr_tol, const int max_iter=IKINCTRL_DISABLED, const unsigned int verbose=0, bool useHessian=true)
 Constructor. More...
 
void set_ctrlPose (const unsigned int _ctrlPose)
 Sets the state of Pose control settings. More...
 
unsigned int get_ctrlPose () const
 Returns the state of Pose control settings. More...
 
bool set_posePriority (const std::string &priority)
 Sets the Pose priority for weighting more either position or orientation while reaching in full pose. More...
 
std::string get_posePriority () const
 Returns the Pose priority settings. More...
 
void attachLIC (iKinLinIneqConstr &lic)
 Attach a iKinLinIneqConstr object in order to impose constraints of the form lB <= C*q <= uB. More...
 
iKinLinIneqConstrgetLIC ()
 Returns a reference to the attached Linear Inequality Constraints object. More...
 
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. More...
 
iKinChainget2ndTaskChain ()
 Retrieves the 2nd task's chain. More...
 
void setMaxIter (const int max_iter)
 Sets Maximum Iteration. More...
 
int getMaxIter () const
 Retrieves the current value of Maximum Iteration. More...
 
void setMaxCpuTime (const double max_cpu_time)
 Sets Maximum CPU seconds. More...
 
double getMaxCpuTime () const
 Retrieves the current value of Maximum CPU seconds. More...
 
void setTol (const double tol)
 Sets cost function tolerance. More...
 
double getTol () const
 Retrieves cost function tolerance. More...
 
void setConstrTol (const double constr_tol)
 Sets constraints tolerance. More...
 
double getConstrTol () const
 Retrieves constraints tolerance. More...
 
void setVerbosity (const unsigned int verbose)
 Sets Verbosity. More...
 
void setHessianOpt (const bool useHessian)
 Selects whether to rely on exact Hessian computation or enable Quasi-Newton approximation (Hessian is enabled at start-up by default). More...
 
void setUserScaling (const bool useUserScaling, const double _obj_scaling, const double _x_scaling, const double _g_scaling)
 Enables/disables user scaling factors. More...
 
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). More...
 
void getBoundsInf (double &lower, double &upper)
 Returns the lower and upper bounds to represent -inf and +inf. More...
 
void setBoundsInf (const double lower, const double upper)
 Sets the lower and upper bounds to represent -inf and +inf. More...
 
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. More...
 
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)
 Executes the IpOpt algorithm trying to converge on target. More...
 
virtual yarp::sig::Vector solve (const yarp::sig::Vector &q0, yarp::sig::Vector &xd)
 Executes the IpOpt algorithm trying to converge on target. More...
 
virtual ~iKinIpOptMin ()
 Default destructor. More...
 

Protected Attributes

void * App
 
iKinChainchain
 
iKinChain chain2ndTask
 
iKinLinIneqConstr noLIC
 
iKinLinIneqConstrpLIC
 
unsigned int ctrlPose
 
double obj_scaling
 
double x_scaling
 
double g_scaling
 
double lowerBoundInf
 
double upperBoundInf
 
std::string posePriority
 

Detailed Description

Class for inverting chain's kinematics based on IpOpt lib.

Definition at line 198 of file iKinIpOpt.h.

Constructor & Destructor Documentation

◆ iKinIpOptMin()

iKinIpOptMin::iKinIpOptMin ( iKinChain c,
const unsigned int  _ctrlPose,
const double  tol,
const double  constr_tol,
const int  max_iter = IKINCTRL_DISABLED,
const unsigned int  verbose = 0,
bool  useHessian = true 
)

Constructor.

Parameters
cis the Chain object on which the control operates. Do not change Chain DOF from this point onwards!!
_ctrlPoseone of the following: IKINCTRL_POSE_FULL => complete pose control. IKINCTRL_POSE_XYZ => translational part of pose controlled. IKINCTRL_POSE_ANG => rotational part of pose controlled.
tolcost function tolerance.
constr_tolconstraints tolerance.
max_iterexits if iter>=max_iter (max_iter<0 disables this check, IKINCTRL_DISABLED(==-1) by default).
verboseinteger which progressively enables different levels of warning messages or status dump. The larger this value the more detailed is the output (0=>off by default).
useHessianrelies on exact Hessian computation or
enable Quasi-Newton approximation (true by default).

Definition at line 796 of file iKinIpOpt.cpp.

◆ ~iKinIpOptMin()

iKinIpOptMin::~iKinIpOptMin ( )
virtual

Default destructor.

Definition at line 1074 of file iKinIpOpt.cpp.

Member Function Documentation

◆ attachLIC()

void iCub::iKin::iKinIpOptMin::attachLIC ( iKinLinIneqConstr lic)
inline

Attach a iKinLinIneqConstr object in order to impose constraints of the form lB <= C*q <= uB.

Parameters
licis the iKinLinIneqConstr object to attach.
See also
iKinLinIneqConstr

Definition at line 288 of file iKinIpOpt.h.

◆ get2ndTaskChain()

iKinChain & iKinIpOptMin::get2ndTaskChain ( )

Retrieves the 2nd task's chain.

Returns
a reference to the 2nd task's chain.

Definition at line 873 of file iKinIpOpt.cpp.

◆ get_ctrlPose()

unsigned int iCub::iKin::iKinIpOptMin::get_ctrlPose ( ) const
inline

Returns the state of Pose control settings.

Returns
Pose control settings.

Definition at line 266 of file iKinIpOpt.h.

◆ get_posePriority()

std::string iCub::iKin::iKinIpOptMin::get_posePriority ( ) const
inline

Returns the Pose priority settings.

Returns
pose priority.

Definition at line 280 of file iKinIpOpt.h.

◆ getBoundsInf()

void iKinIpOptMin::getBoundsInf ( double &  lower,
double &  upper 
)

Returns the lower and upper bounds to represent -inf and +inf.

Parameters
loweris a reference to return the lower bound.
upperis a reference to return the upper bound.

Definition at line 1011 of file iKinIpOpt.cpp.

◆ getConstrTol()

double iKinIpOptMin::getConstrTol ( ) const

Retrieves constraints tolerance.

Returns
tolerance.

Definition at line 943 of file iKinIpOpt.cpp.

◆ getLIC()

iKinLinIneqConstr& iCub::iKin::iKinIpOptMin::getLIC ( )
inline

Returns a reference to the attached Linear Inequality Constraints object.

Returns
Linear Inequality Constraints pLIC.
See also
iKinLinIneqConstr

Definition at line 296 of file iKinIpOpt.h.

◆ getMaxCpuTime()

double iKinIpOptMin::getMaxCpuTime ( ) const

Retrieves the current value of Maximum CPU seconds.

Returns
max_cpu_time.

Definition at line 909 of file iKinIpOpt.cpp.

◆ getMaxIter()

int iKinIpOptMin::getMaxIter ( ) const

Retrieves the current value of Maximum Iteration.

Returns
max_iter.

Definition at line 892 of file iKinIpOpt.cpp.

◆ getTol()

double iKinIpOptMin::getTol ( ) const

Retrieves cost function tolerance.

Returns
tolerance.

Definition at line 926 of file iKinIpOpt.cpp.

◆ set_ctrlPose()

void iKinIpOptMin::set_ctrlPose ( const unsigned int  _ctrlPose)

Sets the state of Pose control settings.

Parameters
_ctrlPoseone of the following: IKINCTRL_POSE_FULL => complete pose control. IKINCTRL_POSE_XYZ => translational part of pose controlled. IKINCTRL_POSE_ANG => rotational part of pose controlled.

Definition at line 833 of file iKinIpOpt.cpp.

◆ set_posePriority()

bool iKinIpOptMin::set_posePriority ( const std::string &  priority)

Sets the Pose priority for weighting more either position or orientation while reaching in full pose.

Parameters
prioritycan be "position" or "orientation".
Returns
true/false on success/failure.

Definition at line 843 of file iKinIpOpt.cpp.

◆ setBoundsInf()

void iKinIpOptMin::setBoundsInf ( const double  lower,
const double  upper 
)

Sets the lower and upper bounds to represent -inf and +inf.

Parameters
loweris the new lower bound.
upperis the new upper bound.

Definition at line 1019 of file iKinIpOpt.cpp.

◆ setConstrTol()

void iKinIpOptMin::setConstrTol ( const double  constr_tol)

Sets constraints tolerance.

Parameters
toltolerance.

Definition at line 935 of file iKinIpOpt.cpp.

◆ setDerivativeTest()

void iKinIpOptMin::setDerivativeTest ( const bool  enableTest,
const bool  enable2ndDer = false 
)

Enable\disable derivative test at each call to solve method (disabled at start-up by default).

Useful to check the derivatives implementation of NLP.

Parameters
enableTesttrue if derivative test shall be enabled.
enable2ndDertrue to enable second derivative test as well (false by default).

Definition at line 992 of file iKinIpOpt.cpp.

◆ setHessianOpt()

void iKinIpOptMin::setHessianOpt ( const bool  useHessian)

Selects whether to rely on exact Hessian computation or enable Quasi-Newton approximation (Hessian is enabled at start-up by default).

Parameters
useHessiantrue if Hessian computation is enabled.

Definition at line 961 of file iKinIpOpt.cpp.

◆ setMaxCpuTime()

void iKinIpOptMin::setMaxCpuTime ( const double  max_cpu_time)

Sets Maximum CPU seconds.

Parameters
max_cpu_timeexits if cpu_time>=max_cpu_time given in seconds.

Definition at line 901 of file iKinIpOpt.cpp.

◆ setMaxIter()

void iKinIpOptMin::setMaxIter ( const int  max_iter)

Sets Maximum Iteration.

Parameters
max_iterexits if iter>=max_iter (max_iter<0 (IKINCTRL_DISABLED) disables this check).

Definition at line 880 of file iKinIpOpt.cpp.

◆ setTol()

void iKinIpOptMin::setTol ( const double  tol)

Sets cost function tolerance.

Parameters
toltolerance.

Definition at line 918 of file iKinIpOpt.cpp.

◆ setUserScaling()

void iKinIpOptMin::setUserScaling ( const bool  useUserScaling,
const double  _obj_scaling,
const double  _x_scaling,
const double  _g_scaling 
)

Enables/disables user scaling factors.

Parameters
useUserScalingtrue if user scaling is enabled.
obj_scalinguser scaling factor for the objective function.
x_scalinguser scaling factor for variables.
g_scalinguser scaling factor for constraints.

Definition at line 973 of file iKinIpOpt.cpp.

◆ setVerbosity()

void iKinIpOptMin::setVerbosity ( const unsigned int  verbose)

Sets Verbosity.

Parameters
verboseis a integer number which progressively enables different levels of warning messages or status dump. The larger this value the more detailed is the output.

Definition at line 952 of file iKinIpOpt.cpp.

◆ solve() [1/3]

yarp::sig::Vector iKinIpOptMin::solve ( const yarp::sig::Vector &  q0,
yarp::sig::Vector &  xd 
)
virtual

Executes the IpOpt algorithm trying to converge on target.

Parameters
q0is the vector of initial joint angles values.
xdis the End-Effector target Pose to be attained.
Returns
estimated joint angles.

Definition at line 1066 of file iKinIpOpt.cpp.

◆ solve() [2/3]

yarp::sig::Vector iKinIpOptMin::solve ( const yarp::sig::Vector &  q0,
yarp::sig::Vector &  xd,
double  weight2ndTask,
yarp::sig::Vector &  xd_2nd,
yarp::sig::Vector &  w_2nd 
)
virtual

Executes the IpOpt algorithm trying to converge on target.

Parameters
q0is the vector of initial joint angles values.
xdis the End-Effector target Pose to be attained.
weight2ndTaskweights the second task (disabled if 0.0).
xd_2ndis the second target task traslational Pose to be attained (typically a particular elbow xyz position).
w_2ndweights each components of the distance vector xd_2nd-x_2nd. Hence, the follows holds as second task: min 1/2*norm2(((xd_i-x_i)*w_i)_i)
Returns
estimated joint angles.

Definition at line 1056 of file iKinIpOpt.cpp.

◆ solve() [3/3]

yarp::sig::Vector iKinIpOptMin::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 
)
virtual

Executes the IpOpt algorithm trying to converge on target.

Parameters
q0is the vector of initial joint angles values.
xdis the End-Effector target Pose to be attained.
weight2ndTaskweights the second task (disabled if 0.0).
xd_2ndis the second target task traslational Pose to be attained (typically a particular elbow xyz position).
w_2ndweights each components of the distance vector xd_2nd-x_2nd. Hence, the follows holds as second task: min 1/2*norm2(((xd_i-x_i)*w_i)_i)
weight3rdTaskweights the third task (disabled if 0.0).
qd_3rdis the third task joint angles target positions to be attained.
w_3rdweights each components of the distance vector qd-q. Hence, the follows holds as third task: min 1/2*norm2(((qd_i-q_i)*w_i)_i)
exit_codestores the exit code (NULL by default). Test for one of this: SUCCESS MAXITER_EXCEEDED STOP_AT_TINY_STEP STOP_AT_ACCEPTABLE_POINT LOCAL_INFEASIBILITY USER_REQUESTED_STOP FEASIBLE_POINT_FOUND DIVERGING_ITERATES RESTORATION_FAILURE ERROR_IN_STEP_COMPUTATION INVALID_NUMBER_DETECTED TOO_FEW_DEGREES_OF_FREEDOM INTERNAL_ERROR
exhaltchecks for an external request to exit (NULL by default).
iteratepointer to a callback object (NULL by default).
Returns
estimated joint angles.

Definition at line 1030 of file iKinIpOpt.cpp.

◆ specify2ndTaskEndEff()

void iKinIpOptMin::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.

Parameters
nis the ordinal number of last joint pointing at the 2nd End-Effector.

Definition at line 856 of file iKinIpOpt.cpp.

Member Data Documentation

◆ App

void* iCub::iKin::iKinIpOptMin::App
protected

Definition at line 209 of file iKinIpOpt.h.

◆ chain

iKinChain& iCub::iKin::iKinIpOptMin::chain
protected

Definition at line 211 of file iKinIpOpt.h.

◆ chain2ndTask

iKinChain iCub::iKin::iKinIpOptMin::chain2ndTask
protected

Definition at line 212 of file iKinIpOpt.h.

◆ ctrlPose

unsigned int iCub::iKin::iKinIpOptMin::ctrlPose
protected

Definition at line 217 of file iKinIpOpt.h.

◆ g_scaling

double iCub::iKin::iKinIpOptMin::g_scaling
protected

Definition at line 221 of file iKinIpOpt.h.

◆ lowerBoundInf

double iCub::iKin::iKinIpOptMin::lowerBoundInf
protected

Definition at line 222 of file iKinIpOpt.h.

◆ noLIC

iKinLinIneqConstr iCub::iKin::iKinIpOptMin::noLIC
protected

Definition at line 214 of file iKinIpOpt.h.

◆ obj_scaling

double iCub::iKin::iKinIpOptMin::obj_scaling
protected

Definition at line 219 of file iKinIpOpt.h.

◆ pLIC

iKinLinIneqConstr* iCub::iKin::iKinIpOptMin::pLIC
protected

Definition at line 215 of file iKinIpOpt.h.

◆ posePriority

std::string iCub::iKin::iKinIpOptMin::posePriority
protected

Definition at line 224 of file iKinIpOpt.h.

◆ upperBoundInf

double iCub::iKin::iKinIpOptMin::upperBoundInf
protected

Definition at line 223 of file iKinIpOpt.h.

◆ x_scaling

double iCub::iKin::iKinIpOptMin::x_scaling
protected

Definition at line 220 of file iKinIpOpt.h.


The documentation for this class was generated from the following files: