ConstraintSet.hpp

Go to the documentation of this file.

/* SPDX-FileCopyrightText: 2009 Ruben Smits
 *
 * SPDX-License-Identifier: LGPL-2.1-or-later */
 
#ifndef CONSTRAINTSET_HPP_
#define CONSTRAINTSET_HPP_
 
#include "kdl/frames.hpp"
#include "eigen_types.hpp"
#include "Cache.hpp"
#include <vector>
 
namespace iTaSC {
 
enum ConstraintAction {
    ACT_NONE=       0,
    ACT_VALUE=      1,
    ACT_VELOCITY=   2,
    ACT_TOLERANCE=  4,
    ACT_FEEDBACK=   8,
    ACT_ALPHA=      16
};
 
struct ConstraintSingleValue {
    unsigned int id;    // identifier of constraint value, depends on constraint
    unsigned int action;// action performed, compbination of ACT_..., set on return
    const double y;     // actual constraint value
    const double ydot;  // actual constraint velocity
    double yd;          // current desired constraint value, changed on return
    double yddot;       // current desired constraint velocity, changed on return
    ConstraintSingleValue(): id(0), action(0), y(0.0), ydot(0.0) {}
};
 
struct ConstraintValues {
    unsigned int id;    // identifier of group of constraint values, depend on constraint
    unsigned short number;      // number of constraints in list
    unsigned short action;      // action performed, ACT_..., set on return
    double alpha;       // constraint activation coefficient, should be [0..1]
    double tolerance;   // current desired tolerance on constraint, same unit than yd, changed on return
    double feedback;    // current desired feedback on error, in 1/sec, changed on return
    struct ConstraintSingleValue* values;
    ConstraintValues(): id(0), number(0), action(0), values(NULL) {}
};
 
class ConstraintSet;
typedef bool (*ConstraintCallback)(const Timestamp& timestamp, struct ConstraintValues* const _values, unsigned int _nvalues, void* _param);
 
class ConstraintSet {
protected:
    unsigned int m_nc;
    e_scalar m_maxDeltaChi;
    e_matrix m_Cf;
    e_vector m_Wy,m_y,m_ydot;
    e_vector6 m_chi,m_chidot,m_S,m_temp,m_tdelta;
    e_matrix6 m_Jf,m_U,m_V,m_B,m_Jf_inv;
    KDL::Frame m_internalPose,m_externalPose;
    ConstraintCallback m_constraintCallback;
    void* m_constraintParam;
    void* m_poseParam;
    bool m_toggle;
    bool m_substep;
    double m_threshold;
    unsigned int m_maxIter;
 
    friend class Scene;
    virtual void modelUpdate(KDL::Frame& _external_pose,const Timestamp& timestamp);
    virtual void updateKinematics(const Timestamp& timestamp)=0;
    virtual void pushCache(const Timestamp& timestamp)=0;
    virtual void updateJacobian()=0;
    virtual void updateControlOutput(const Timestamp& timestamp)=0;
    virtual void initCache(Cache *_cache) = 0;
    virtual bool initialise(KDL::Frame& init_pose);
    virtual void reset(unsigned int nc,double accuracy,unsigned int maximum_iterations);
    virtual bool closeLoop();
    virtual double getMaxTimestep(double& timestep);
 
 
public:
    ConstraintSet(unsigned int nc,double accuracy,unsigned int maximum_iterations);
    ConstraintSet();
    virtual ~ConstraintSet();
 
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    virtual bool registerCallback(ConstraintCallback _function, void* _param)
    {
        m_constraintCallback = _function;
        m_constraintParam = _param;
        return true;
    }
 
    virtual const e_vector& getControlOutput()const{return m_ydot;};
    virtual const ConstraintValues* getControlParameters(unsigned int* _nvalues) = 0;
    virtual bool setControlParameters(ConstraintValues* _values, unsigned int _nvalues, double timestep=0.0) = 0;
    bool setControlParameter(int id, ConstraintAction action, double value, double timestep=0.0);
 
    virtual const e_matrix6& getJf() const{return m_Jf;};
    virtual const KDL::Frame& getPose() const{return m_internalPose;};
    virtual const e_matrix& getCf() const{return m_Cf;};
 
    virtual const e_vector& getWy() const {return m_Wy;};
    virtual void setWy(const e_vector& Wy_in){m_Wy = Wy_in;};
    virtual void setJointVelocity(const e_vector chidot_in){m_chidot = chidot_in;};
 
    virtual unsigned int getNrOfConstraints(){return m_nc;};
    void substep(bool _substep) {m_substep=_substep;}
    bool substep() {return m_substep;}
};
 
}
 
#endif /* CONSTRAINTSET_HPP_ */