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.

void	set_ctrlPose (const unsigned int _ctrlPose)
	Sets the state of Pose control settings.

unsigned int	get_ctrlPose () const
	Returns the state of Pose control settings.

bool	set_posePriority (const std::string &priority)
	Sets the Pose priority for weighting more either position or orientation while reaching in full pose.

std::string	get_posePriority () const
	Returns the Pose priority settings.

void	attachLIC (iKinLinIneqConstr &lic)
	Attach a iKinLinIneqConstr object in order to impose constraints of the form lB <= C*q <= uB.

iKinLinIneqConstr &	getLIC ()
	Returns a reference to the attached Linear Inequality Constraints object.

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.

iKinChain &	get2ndTaskChain ()
	Retrieves the 2nd task's chain.

void	setMaxIter (const int max_iter)
	Sets Maximum Iteration.

int	getMaxIter () const
	Retrieves the current value of Maximum Iteration.

void	setMaxCpuTime (const double max_cpu_time)
	Sets Maximum CPU seconds.

double	getMaxCpuTime () const
	Retrieves the current value of Maximum CPU seconds.

void	setTol (const double tol)
	Sets cost function tolerance.

double	getTol () const
	Retrieves cost function tolerance.

void	setConstrTol (const double constr_tol)
	Sets constraints tolerance.

double	getConstrTol () const
	Retrieves constraints tolerance.

void	setVerbosity (const unsigned int verbose)
	Sets Verbosity.

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).

void	setUserScaling (const bool useUserScaling, const double _obj_scaling, const double _x_scaling, const double _g_scaling)
	Enables/disables user scaling factors.

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).

void	getBoundsInf (double &lower, double &upper)
	Returns the lower and upper bounds to represent -inf and +inf.

void	setBoundsInf (const double lower, const double upper)
	Sets the lower and upper bounds to represent -inf and +inf.

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.

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.

virtual yarp::sig::Vector	solve (const yarp::sig::Vector &q0, yarp::sig::Vector &xd)
	Executes the IpOpt algorithm trying to converge on target.

virtual	~iKinIpOptMin ()
	Default destructor.

Protected Attributes
void *	App

iKinChain &	chain

iKinChain	chain2ndTask

iKinLinIneqConstr	noLIC

iKinLinIneqConstr *	pLIC

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 197 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

c	is the Chain object on which the control operates. Do not change Chain DOF from this point onwards!!
_ctrlPose	one 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.
tol	cost function tolerance.
constr_tol	constraints tolerance.
max_iter	exits if iter>=max_iter (max_iter<0 disables this check, IKINCTRL_DISABLED(==-1) by default).
verbose	integer 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).
useHessian	relies 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

lic	is the iKinLinIneqConstr object to attach.

See also: iKinLinIneqConstr

Definition at line 287 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 265 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 279 of file iKinIpOpt.h.

◆ getBoundsInf()

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

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

Parameters

lower	is a reference to return the lower bound.
upper	is 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 295 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

_ctrlPose one 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

priority can 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

lower	is the new lower bound.
upper	is the new upper bound.

Definition at line 1019 of file iKinIpOpt.cpp.

◆ setConstrTol()

void iKinIpOptMin::setConstrTol ( const double constr_tol )

Sets constraints tolerance.

Parameters

tol	tolerance.

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

enableTest	true if derivative test shall be enabled.
enable2ndDer	true 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

useHessian true 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_time exits 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_iter exits 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

tol	tolerance.

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

useUserScaling	true if user scaling is enabled.
obj_scaling	user scaling factor for the objective function.
x_scaling	user scaling factor for variables.
g_scaling	user scaling factor for constraints.

Definition at line 973 of file iKinIpOpt.cpp.

◆ setVerbosity()

void iKinIpOptMin::setVerbosity ( const unsigned int verbose )

Sets Verbosity.

Parameters

verbose is 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

q0	is the vector of initial joint angles values.
xd	is 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

q0	is the vector of initial joint angles values.
xd	is the End-Effector target Pose to be attained.
weight2ndTask	weights the second task (disabled if 0.0).
xd_2nd	is the second target task traslational Pose to be attained (typically a particular elbow xyz position).
w_2nd	weights each components of the distance vector xd_2nd-x_2nd. Hence, the follows holds as second task: min 1/2norm2(((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

q0	is the vector of initial joint angles values.
xd	is the End-Effector target Pose to be attained.
weight2ndTask	weights the second task (disabled if 0.0).
xd_2nd	is the second target task traslational Pose to be attained (typically a particular elbow xyz position).
w_2nd	weights each components of the distance vector xd_2nd-x_2nd. Hence, the follows holds as second task: min 1/2norm2(((xd_i-x_i)w_i)_i)
weight3rdTask	weights the third task (disabled if 0.0).
qd_3rd	is the third task joint angles target positions to be attained.
w_3rd	weights each components of the distance vector qd-q. Hence, the follows holds as third task: min 1/2norm2(((qd_i-q_i)w_i)_i)
exit_code	stores 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
exhalt	checks for an external request to exit (NULL by default).
iterate	pointer 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

n	is 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 208 of file iKinIpOpt.h.

◆ chain

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

protected

Definition at line 210 of file iKinIpOpt.h.

◆ chain2ndTask

iKinChain iCub::iKin::iKinIpOptMin::chain2ndTask

protected

Definition at line 211 of file iKinIpOpt.h.

◆ ctrlPose

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

protected

Definition at line 216 of file iKinIpOpt.h.

◆ g_scaling

double iCub::iKin::iKinIpOptMin::g_scaling

protected

Definition at line 220 of file iKinIpOpt.h.

◆ lowerBoundInf

double iCub::iKin::iKinIpOptMin::lowerBoundInf

protected

Definition at line 221 of file iKinIpOpt.h.

◆ noLIC

iKinLinIneqConstr iCub::iKin::iKinIpOptMin::noLIC

protected

Definition at line 213 of file iKinIpOpt.h.

◆ obj_scaling

double iCub::iKin::iKinIpOptMin::obj_scaling

protected

Definition at line 218 of file iKinIpOpt.h.

◆ pLIC

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

protected

Definition at line 214 of file iKinIpOpt.h.

◆ posePriority

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

protected

Definition at line 223 of file iKinIpOpt.h.

◆ upperBoundInf

double iCub::iKin::iKinIpOptMin::upperBoundInf

protected

Definition at line 222 of file iKinIpOpt.h.

◆ x_scaling

double iCub::iKin::iKinIpOptMin::x_scaling

protected

Definition at line 219 of file iKinIpOpt.h.

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

icub-main/src/libraries/iKin/include/iCub/iKin/iKinIpOpt.h
icub-main/src/libraries/iKin/src/iKinIpOpt.cpp

Public Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ iKinIpOptMin()

◆ ~iKinIpOptMin()

Member Function Documentation

◆ attachLIC()

◆ get2ndTaskChain()

◆ get_ctrlPose()

◆ get_posePriority()

◆ getBoundsInf()

◆ getConstrTol()

◆ getLIC()

◆ getMaxCpuTime()

◆ getMaxIter()

◆ getTol()

◆ set_ctrlPose()

◆ set_posePriority()

◆ setBoundsInf()

◆ setConstrTol()

◆ setDerivativeTest()

◆ setHessianOpt()

◆ setMaxCpuTime()

◆ setMaxIter()

◆ setTol()

◆ setUserScaling()

◆ setVerbosity()

◆ solve() [1/3]

◆ solve() [2/3]

◆ solve() [3/3]

◆ specify2ndTaskEndEff()

Member Data Documentation

◆ App

◆ chain

◆ chain2ndTask

◆ ctrlPose

◆ g_scaling

◆ lowerBoundInf

◆ noLIC

◆ obj_scaling

◆ pLIC

◆ posePriority

◆ upperBoundInf

◆ x_scaling