Blender  V3.3
ConstraintSet.hpp
Go to the documentation of this file.
1 /* SPDX-License-Identifier: LGPL-2.1-or-later
2  * Copyright 2009 Ruben Smits. */
3 
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 
24  ACT_ALPHA= 16
25 };
26 
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
34  ConstraintSingleValue(): id(0), action(0), y(0.0), ydot(0.0) {}
35 };
36 
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
46 };
47 
48 class ConstraintSet;
49 typedef bool (*ConstraintCallback)(const Timestamp& timestamp, struct ConstraintValues* const _values, unsigned int _nvalues, void* _param);
50 
52 protected:
53  unsigned int m_nc;
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 
88  virtual bool registerCallback(ConstraintCallback _function, void* _param)
89  {
90  m_constraintCallback = _function;
91  m_constraintParam = _param;
92  return true;
93  }
94 
95  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 
100  virtual const e_matrix6& getJf() const{return m_Jf;};
101  virtual const KDL::Frame& getPose() const{return m_internalPose;};
102  virtual const e_matrix& getCf() const{return m_Cf;};
103 
104  virtual const e_vector& getWy() const {return m_Wy;};
105  virtual void setWy(const e_vector& Wy_in){m_Wy = Wy_in;};
106  virtual void setJointVelocity(const e_vector chidot_in){m_chidot = chidot_in;};
107 
108  virtual unsigned int getNrOfConstraints(){return m_nc;};
109  void substep(bool _substep) {m_substep=_substep;}
110  bool substep() {return m_substep;}
111 };
112 
113 }
114 
115 #endif /* CONSTRAINTSET_HPP_ */
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:526
virtual void updateJacobian()=0
virtual void setWy(const e_vector &Wy_in)
bool setControlParameter(int id, ConstraintAction action, double value, double timestep=0.0)
virtual double getMaxTimestep(double &timestep)
virtual const e_matrix6 & getJf() const
virtual bool initialise(KDL::Frame &init_pose)
unsigned int m_maxIter
virtual const e_vector & getControlOutput() const
virtual void initCache(Cache *_cache)=0
void substep(bool _substep)
virtual const e_vector & getWy() const
virtual EIGEN_MAKE_ALIGNED_OPERATOR_NEW bool registerCallback(ConstraintCallback _function, void *_param)
virtual void modelUpdate(KDL::Frame &_external_pose, const Timestamp &timestamp)
virtual void reset(unsigned int nc, double accuracy, unsigned int maximum_iterations)
virtual void updateKinematics(const Timestamp &timestamp)=0
virtual const KDL::Frame & getPose() const
virtual void setJointVelocity(const e_vector chidot_in)
virtual void updateControlOutput(const Timestamp &timestamp)=0
virtual bool closeLoop()
virtual unsigned int getNrOfConstraints()
ConstraintCallback m_constraintCallback
virtual const e_matrix & getCf() const
virtual void pushCache(const Timestamp &timestamp)=0
virtual bool setControlParameters(ConstraintValues *_values, unsigned int _nvalues, double timestep=0.0)=0
virtual const ConstraintValues * getControlParameters(unsigned int *_nvalues)=0
#define e_vector
Definition: eigen_types.hpp:38
#define e_vector6
Definition: eigen_types.hpp:46
#define e_scalar
Definition: eigen_types.hpp:37
#define e_matrix6
Definition: eigen_types.hpp:41
#define e_matrix
Definition: eigen_types.hpp:40
bool(* ConstraintCallback)(const Timestamp &timestamp, struct ConstraintValues *const _values, unsigned int _nvalues, void *_param)
@ ACT_TOLERANCE
@ ACT_FEEDBACK
@ ACT_VELOCITY
struct ConstraintSingleValue * values