Merge of itasc branch. Project files, scons and cmake should be working. Makefile...
[blender.git] / intern / itasc / Distance.hpp
1 /* $Id: Distance.hpp 19905 2009-04-23 13:29:54Z ben2610 $
2  * Distance.hpp
3  *
4  *  Created on: Jan 30, 2009
5  *      Author: rsmits
6  */
7
8 #ifndef DISTANCE_HPP_
9 #define DISTANCE_HPP_
10
11 #include "ConstraintSet.hpp"
12 #include "kdl/chain.hpp"
13 #include "kdl/chainfksolverpos_recursive.hpp"
14 #include "kdl/chainjnttojacsolver.hpp"
15
16 namespace iTaSC
17 {
18
19 class Distance: public iTaSC::ConstraintSet
20 {
21 protected:
22     virtual void updateKinematics(const Timestamp& timestamp);
23     virtual void pushCache(const Timestamp& timestamp);
24     virtual void updateJacobian();
25     virtual bool initialise(Frame& init_pose);
26         virtual void initCache(Cache *_cache);
27     virtual void updateControlOutput(const Timestamp& timestamp);
28         virtual bool closeLoop();
29
30 public:
31         enum ID {
32                 ID_DISTANCE=1,
33         };
34     Distance(double armlength=1.0, double accuracy=1e-6, unsigned int maximum_iterations=100);
35     virtual ~Distance();
36
37         virtual bool setControlParameters(struct ConstraintValues* _values, unsigned int _nvalues, double timestep);
38         virtual const ConstraintValues* getControlParameters(unsigned int* _nvalues);
39
40 private:
41         bool computeChi(Frame& pose);
42     KDL::Chain m_chain;
43     KDL::ChainFkSolverPos_recursive* m_fksolver;
44     KDL::ChainJntToJacSolver* m_jacsolver;
45     KDL::JntArray m_chiKdl;
46     KDL::Jacobian m_jac;
47         struct ConstraintSingleValue m_data;
48         struct ConstraintValues m_values;
49         Cache* m_cache;
50         int m_distCCh;
51         CacheTS m_distCTs;
52         double m_maxerror;
53
54         void pushDist(CacheTS timestamp);
55         bool popDist(CacheTS timestamp);
56
57     double m_alpha,m_yddot,m_yd,m_nextyd,m_nextyddot,m_K,m_tolerance;
58 };
59
60 }
61
62 #endif /* DISTANCE_HPP_ */