iKinIpOpt.h

Go to the documentation of this file.

/*
 * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT)
 * Copyright (C) 2006-2010 RobotCub Consortium
 * All rights reserved.
 *
 * This software may be modified and distributed under the terms
 * of the BSD-3-Clause license. See the accompanying LICENSE file for
 * details.
*/
 
#ifndef __IKINIPOPT_H__
#define __IKINIPOPT_H__
 
#include <iCub/iKin/iKinInv.h>
 
 
namespace iCub
{
 
namespace iKin
{
 
class iKinIterateCallback
{
private:
    // Copy constructor: not implemented.
    iKinIterateCallback(const iKinIterateCallback&);
    // Assignment operator: not implemented.
    iKinIterateCallback &operator=(const iKinIterateCallback&);
 
public:
    iKinIterateCallback() { }
 
    virtual void exec(const yarp::sig::Vector &xd, const yarp::sig::Vector &q) = 0;
};
 
 
class iKinLinIneqConstr
{
protected:
    yarp::sig::Matrix C;
    yarp::sig::Vector uB;
    yarp::sig::Vector lB;
 
    double lowerBoundInf;
    double upperBoundInf;
    bool   active;
 
    virtual void clone(const iKinLinIneqConstr *obj);
 
public:
    iKinLinIneqConstr();
 
    iKinLinIneqConstr(const double _lowerBoundInf, const double _upperBoundInf);
 
    iKinLinIneqConstr(const iKinLinIneqConstr &obj);
 
    virtual iKinLinIneqConstr &operator=(const iKinLinIneqConstr &obj);
 
    yarp::sig::Matrix &getC() { return C; }
 
    yarp::sig::Vector &getuB() { return uB; }
 
    yarp::sig::Vector &getlB() { return lB; }
 
    double &getLowerBoundInf() { return lowerBoundInf; }
 
    double &getUpperBoundInf() { return upperBoundInf; }
 
    bool isActive() { return active; }
 
    void setActive(bool _active) { active=_active; }
 
    virtual void update(void*) { }
};
 
 
class iCubAdditionalArmConstraints : public iKinLinIneqConstr
{
protected:
    double     shou_m, shou_n;
    double     elb_m,  elb_n;
    
    iKinChain *chain;
    double     hw_version;
 
    void clone(const iKinLinIneqConstr *obj);
 
public:
    iCubAdditionalArmConstraints(iCubArm &arm);
 
    void update(void*);
};
 
 
class iKinIpOptMin
{
private:
    // Default constructor: not implemented.
    iKinIpOptMin();
    // Copy constructor: not implemented.
    iKinIpOptMin(const iKinIpOptMin&);
    // Assignment operator: not implemented.
    iKinIpOptMin &operator=(const iKinIpOptMin&);    
 
protected:
    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;
 
public:
    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);
 
    void set_ctrlPose(const unsigned int _ctrlPose);
 
    unsigned int get_ctrlPose() const { return ctrlPose; }
 
    bool set_posePriority(const std::string &priority);
 
    std::string get_posePriority() const { return posePriority; }
 
    void attachLIC(iKinLinIneqConstr &lic) { pLIC=&lic; }
 
    iKinLinIneqConstr &getLIC() { return *pLIC; }
 
    void specify2ndTaskEndEff(const unsigned int n);
 
    iKinChain &get2ndTaskChain();
 
    void setMaxIter(const int max_iter);
 
    int getMaxIter() const;
 
    void setMaxCpuTime(const double max_cpu_time);
 
    double getMaxCpuTime() const;
 
    void setTol(const double tol);
 
    double getTol() const;
 
    void setConstrTol(const double constr_tol);
 
    double getConstrTol() const;
 
    void setVerbosity(const unsigned int verbose);
 
    void setHessianOpt(const bool useHessian);
 
    void setUserScaling(const bool useUserScaling, const double _obj_scaling,
                        const double _x_scaling, const double _g_scaling);
 
    void setDerivativeTest(const bool enableTest, const bool enable2ndDer=false);
 
    void getBoundsInf(double &lower, double &upper);
 
    void setBoundsInf(const double lower, const double upper);
 
    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);
 
    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);
 
    virtual yarp::sig::Vector solve(const yarp::sig::Vector &q0, yarp::sig::Vector &xd);
 
    virtual ~iKinIpOptMin();
};
 
}
 
}
 
#endif