diff options
Diffstat (limited to 'intern/itasc/CopyPose.hpp')
-rw-r--r-- | intern/itasc/CopyPose.hpp | 99 |
1 files changed, 99 insertions, 0 deletions
diff --git a/intern/itasc/CopyPose.hpp b/intern/itasc/CopyPose.hpp new file mode 100644 index 00000000000..3a3f60a9f37 --- /dev/null +++ b/intern/itasc/CopyPose.hpp @@ -0,0 +1,99 @@ +/* $Id: CopyPose.hpp 20622 2009-06-04 12:47:59Z ben2610 $ + * CopyPose.h + * + * Created on: Mar 17, 2009 + * Author: benoit bolsee + */ + +#ifndef COPYPOSE_H_ +#define COPYPOSE_H_ + +#include "ConstraintSet.hpp" +namespace iTaSC{ + +using namespace KDL; + +class CopyPose: public iTaSC::ConstraintSet +{ +protected: + virtual void updateKinematics(const Timestamp& timestamp); + virtual void pushCache(const Timestamp& timestamp); + virtual void updateJacobian(); + virtual bool initialise(Frame& init_pose); + virtual void initCache(Cache *_cache); + virtual void updateControlOutput(const Timestamp& timestamp); + virtual void modelUpdate(Frame& _external_pose,const Timestamp& timestamp); + virtual double getMaxTimestep(double& timestep); + +public: + enum ID { // constraint ID in callback and setControlParameter + ID_POSITION=0, + ID_POSITIONX=1, + ID_POSITIONY=2, + ID_POSITIONZ=3, + ID_ROTATION=4, + ID_ROTATIONX=5, + ID_ROTATIONY=6, + ID_ROTATIONZ=7, + }; + enum CTL { // control ID in constructor to specify which output is constrainted + CTL_NONE=0x00, + CTL_POSITIONX=0x01, // the bit order is important: it matches the y output order + CTL_POSITIONY=0x02, + CTL_POSITIONZ=0x04, + CTL_POSITION=0x07, + CTL_ROTATIONX=0x08, + CTL_ROTATIONY=0x10, + CTL_ROTATIONZ=0x20, + CTL_ROTATION=0x38, + CTL_ALL=0x3F, + }; + + // use a combination of CTL_.. in control_output to specify which + CopyPose(unsigned int control_output=CTL_ALL, unsigned int dynamic_output=CTL_NONE, double armlength=1.0, double accuracy=1e-6, unsigned int maximum_iterations=100); + virtual ~CopyPose(); + + virtual bool setControlParameters(struct ConstraintValues* _values, unsigned int _nvalues, double timestep); + virtual const ConstraintValues* getControlParameters(unsigned int* _nvalues); + +private: + struct ConstraintSingleValue m_posData[3]; // index = controlled output in X,Y,Z order + struct ConstraintSingleValue m_rotData[3]; + struct ConstraintValues m_values[2]; // index = group of controlled output, in position, rotation order + Cache* m_cache; + int m_poseCCh; + CacheTS m_poseCTs; + unsigned int m_poseCacheSize; + unsigned int m_outputDynamic; // combination of CTL_... determine which variables are dynamically controlled by the application + unsigned int m_outputControl; // combination of CTL_... determine which output are constrained + unsigned int m_nvalues; // number of elements used in m_values[] + double m_maxerror; + + struct ControlState { + int firsty; // first y index + int ny; // number of y in output + double alpha; + double K; + double tolerance; + struct ControlValue { + double yddot; + double yd; + double nextyd; + double nextyddot; + } output[3]; // inded numbex = same as m_rotData + } m_rot, m_pos; + + void pushPose(CacheTS timestamp); + bool popPose(CacheTS timestamp); + int nBitsOn(unsigned int v) + { int n=0; while(v) { if (v&1) n++; v>>=1; } return n; } + double* restoreValues(double* item, ConstraintValues* _values, ControlState* _state, unsigned int mask); + double* pushValues(double* item, ControlState* _state, unsigned int mask); + void updateState(ConstraintValues* _values, ControlState* _state, unsigned int mask, double timestep); + void updateValues(Vector& vel, ConstraintValues* _values, ControlState* _state, unsigned int mask); + void updateOutput(Vector& vel, ControlState* _state, unsigned int mask); + void interpolateOutput(ControlState* _state, unsigned int mask, const Timestamp& timestamp); + +}; +} +#endif /* COPYROTATION_H_ */ |