1 /* 2 * ConstraintSet.hpp 3 * 4 * Created on: Jan 5, 2009 5 * Author: rubensmits 6 */ 7 8 #ifndef CONSTRAINTSET_HPP_ 9 #define CONSTRAINTSET_HPP_ 10 11 #include "kdl/frames.hpp" 12 #include "eigen_types.hpp" 13 #include "Cache.hpp" 14 #include <vector> 15 16 namespace iTaSC { 17 18 enum ConstraintAction { 19 ACT_NONE= 0, 20 ACT_VALUE= 1, 21 ACT_VELOCITY= 2, 22 ACT_TOLERANCE= 4, 23 ACT_FEEDBACK= 8, 24 ACT_ALPHA= 16 25 }; 26 27 struct ConstraintSingleValue { 28 unsigned int id; // identifier of constraint value, depends on constraint 29 unsigned int action;// action performed, compbination of ACT_..., set on return 30 const double y; // actual constraint value 31 const double ydot; // actual constraint velocity 32 double yd; // current desired constraint value, changed on return 33 double yddot; // current desired constraint velocity, changed on return ConstraintSingleValueiTaSC::ConstraintSingleValue34 ConstraintSingleValue(): id(0), action(0), y(0.0), ydot(0.0) {} 35 }; 36 37 struct ConstraintValues { 38 unsigned int id; // identifier of group of constraint values, depend on constraint 39 unsigned short number; // number of constraints in list 40 unsigned short action; // action performed, ACT_..., set on return 41 double alpha; // constraint activation coefficient, should be [0..1] 42 double tolerance; // current desired tolerance on constraint, same unit than yd, changed on return 43 double feedback; // current desired feedback on error, in 1/sec, changed on return 44 struct ConstraintSingleValue* values; ConstraintValuesiTaSC::ConstraintValues45 ConstraintValues(): id(0), number(0), action(0), values(NULL) {} 46 }; 47 48 class ConstraintSet; 49 typedef bool (*ConstraintCallback)(const Timestamp& timestamp, struct ConstraintValues* const _values, unsigned int _nvalues, void* _param); 50 51 class ConstraintSet { 52 protected: 53 unsigned int m_nc; 54 e_scalar m_maxDeltaChi; 55 e_matrix m_Cf; 56 e_vector m_Wy,m_y,m_ydot; 57 e_vector6 m_chi,m_chidot,m_S,m_temp,m_tdelta; 58 e_matrix6 m_Jf,m_U,m_V,m_B,m_Jf_inv; 59 KDL::Frame m_internalPose,m_externalPose; 60 ConstraintCallback m_constraintCallback; 61 void* m_constraintParam; 62 void* m_poseParam; 63 bool m_toggle; 64 bool m_substep; 65 double m_threshold; 66 unsigned int m_maxIter; 67 68 friend class Scene; 69 virtual void modelUpdate(KDL::Frame& _external_pose,const Timestamp& timestamp); 70 virtual void updateKinematics(const Timestamp& timestamp)=0; 71 virtual void pushCache(const Timestamp& timestamp)=0; 72 virtual void updateJacobian()=0; 73 virtual void updateControlOutput(const Timestamp& timestamp)=0; 74 virtual void initCache(Cache *_cache) = 0; 75 virtual bool initialise(KDL::Frame& init_pose); 76 virtual void reset(unsigned int nc,double accuracy,unsigned int maximum_iterations); 77 virtual bool closeLoop(); 78 virtual double getMaxTimestep(double& timestep); 79 80 81 public: 82 ConstraintSet(unsigned int nc,double accuracy,unsigned int maximum_iterations); 83 ConstraintSet(); 84 virtual ~ConstraintSet(); 85 86 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 87 registerCallback(ConstraintCallback _function,void * _param)88 virtual bool registerCallback(ConstraintCallback _function, void* _param) 89 { 90 m_constraintCallback = _function; 91 m_constraintParam = _param; 92 return true; 93 } 94 getControlOutput() const95 virtual const e_vector& getControlOutput()const{return m_ydot;}; 96 virtual const ConstraintValues* getControlParameters(unsigned int* _nvalues) = 0; 97 virtual bool setControlParameters(ConstraintValues* _values, unsigned int _nvalues, double timestep=0.0) = 0; 98 bool setControlParameter(int id, ConstraintAction action, double value, double timestep=0.0); 99 getJf() const100 virtual const e_matrix6& getJf() const{return m_Jf;}; getPose() const101 virtual const KDL::Frame& getPose() const{return m_internalPose;}; getCf() const102 virtual const e_matrix& getCf() const{return m_Cf;}; 103 getWy() const104 virtual const e_vector& getWy() const {return m_Wy;}; setWy(const e_vector & Wy_in)105 virtual void setWy(const e_vector& Wy_in){m_Wy = Wy_in;}; setJointVelocity(const e_vector chidot_in)106 virtual void setJointVelocity(const e_vector chidot_in){m_chidot = chidot_in;}; 107 getNrOfConstraints()108 virtual unsigned int getNrOfConstraints(){return m_nc;}; substep(bool _substep)109 void substep(bool _substep) {m_substep=_substep;} substep()110 bool substep() {return m_substep;} 111 }; 112 113 } 114 115 #endif /* CONSTRAINTSET_HPP_ */ 116