Blender V2.61 - r43446

CopyPose.hpp

Go to the documentation of this file.
00001 /*
00002  * CopyPose.h
00003  *
00004  *  Created on: Mar 17, 2009
00005  *      Author: benoit bolsee
00006  */
00007 
00008 #ifndef COPYPOSE_H_
00009 #define COPYPOSE_H_
00010 
00011 #include "ConstraintSet.hpp"
00012 namespace iTaSC{
00013 
00014 using namespace KDL;
00015 
00016 class CopyPose: public iTaSC::ConstraintSet
00017 {
00018 protected:
00019     virtual void updateKinematics(const Timestamp& timestamp);
00020     virtual void pushCache(const Timestamp& timestamp);
00021     virtual void updateJacobian();
00022     virtual bool initialise(Frame& init_pose);
00023     virtual void initCache(Cache *_cache);
00024     virtual void updateControlOutput(const Timestamp& timestamp);
00025     virtual void modelUpdate(Frame& _external_pose,const Timestamp& timestamp);
00026     virtual double getMaxTimestep(double& timestep);
00027 
00028 public:
00029     enum ID {       // constraint ID in callback and setControlParameter
00030         ID_POSITION=0,
00031         ID_POSITIONX=1,
00032         ID_POSITIONY=2,
00033         ID_POSITIONZ=3,
00034         ID_ROTATION=4,
00035         ID_ROTATIONX=5,
00036         ID_ROTATIONY=6,
00037         ID_ROTATIONZ=7,
00038     };
00039     enum CTL {      // control ID in constructor to specify which output is constrainted
00040         CTL_NONE=0x00,
00041         CTL_POSITIONX=0x01,     // the bit order is important: it matches the y output order
00042         CTL_POSITIONY=0x02,
00043         CTL_POSITIONZ=0x04,
00044         CTL_POSITION=0x07,
00045         CTL_ROTATIONX=0x08,
00046         CTL_ROTATIONY=0x10,
00047         CTL_ROTATIONZ=0x20,
00048         CTL_ROTATION=0x38,
00049         CTL_ALL=0x3F,
00050     };
00051 
00052     // use a combination of CTL_.. in control_output to specify which 
00053     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);
00054     virtual ~CopyPose();
00055 
00056     virtual bool setControlParameters(struct ConstraintValues* _values, unsigned int _nvalues, double timestep);
00057     virtual const ConstraintValues* getControlParameters(unsigned int* _nvalues);
00058 
00059 private:
00060     struct ConstraintSingleValue m_posData[3];  // index = controlled output in X,Y,Z order
00061     struct ConstraintSingleValue m_rotData[3];
00062     struct ConstraintValues m_values[2];        // index = group of controlled output, in position, rotation order
00063     Cache* m_cache;
00064     int m_poseCCh;
00065     CacheTS m_poseCTs;
00066     unsigned int m_poseCacheSize;
00067     unsigned int m_outputDynamic;   // combination of CTL_... determine which variables are dynamically controlled by the application
00068     unsigned int m_outputControl;   // combination of CTL_... determine which output are constrained
00069     unsigned int m_nvalues;     // number of elements used in m_values[]
00070     double m_maxerror;
00071 
00072     struct ControlState {
00073         int firsty;         // first y index
00074         int ny;             // number of y in output
00075         double alpha;
00076         double K;
00077         double tolerance;
00078         struct ControlValue {
00079             double yddot;
00080             double yd;
00081             double nextyd;
00082             double nextyddot;
00083         } output[3];        // inded numbex = same as m_rotData
00084     } m_rot, m_pos;
00085 
00086     void pushPose(CacheTS timestamp);
00087     bool popPose(CacheTS timestamp);
00088     int nBitsOn(unsigned int v)
00089         { int n=0; while(v) { if (v&1) n++; v>>=1; } return n; }
00090     double* restoreValues(double* item, ConstraintValues* _values, ControlState* _state, unsigned int mask);
00091     double* pushValues(double* item, ControlState* _state, unsigned int mask);
00092     void updateState(ConstraintValues* _values, ControlState* _state, unsigned int mask, double timestep);
00093     void updateValues(Vector& vel, ConstraintValues* _values, ControlState* _state, unsigned int mask);
00094     void updateOutput(Vector& vel, ControlState* _state, unsigned int mask);
00095     void interpolateOutput(ControlState* _state, unsigned int mask, const Timestamp& timestamp);
00096 
00097 };
00098 }
00099 #endif /* COPYROTATION_H_ */