diff options
author | Benoit Bolsee <benoit.bolsee@online.be> | 2009-09-25 01:22:24 +0400 |
---|---|---|
committer | Benoit Bolsee <benoit.bolsee@online.be> | 2009-09-25 01:22:24 +0400 |
commit | 1483fafd1372a3d3e025d08634e798adb7da512f (patch) | |
tree | 9191765749e29866339f4c31d892603f5f8b334d /intern/itasc/Distance.hpp | |
parent | c995c605f640d8d688e6e58e0fe247ca83f91696 (diff) | |
parent | 222fe6b1a5d49f67177cbb762f55a0e482145f5d (diff) |
Merge of itasc branch. Project files, scons and cmake should be working. Makefile updated but not tested. Comes with Eigen2 2.0.6 C++ matrix library.
Diffstat (limited to 'intern/itasc/Distance.hpp')
-rw-r--r-- | intern/itasc/Distance.hpp | 62 |
1 files changed, 62 insertions, 0 deletions
diff --git a/intern/itasc/Distance.hpp b/intern/itasc/Distance.hpp new file mode 100644 index 00000000000..1366693743e --- /dev/null +++ b/intern/itasc/Distance.hpp @@ -0,0 +1,62 @@ +/* $Id: Distance.hpp 19905 2009-04-23 13:29:54Z ben2610 $ + * Distance.hpp + * + * Created on: Jan 30, 2009 + * Author: rsmits + */ + +#ifndef DISTANCE_HPP_ +#define DISTANCE_HPP_ + +#include "ConstraintSet.hpp" +#include "kdl/chain.hpp" +#include "kdl/chainfksolverpos_recursive.hpp" +#include "kdl/chainjnttojacsolver.hpp" + +namespace iTaSC +{ + +class Distance: 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 bool closeLoop(); + +public: + enum ID { + ID_DISTANCE=1, + }; + Distance(double armlength=1.0, double accuracy=1e-6, unsigned int maximum_iterations=100); + virtual ~Distance(); + + virtual bool setControlParameters(struct ConstraintValues* _values, unsigned int _nvalues, double timestep); + virtual const ConstraintValues* getControlParameters(unsigned int* _nvalues); + +private: + bool computeChi(Frame& pose); + KDL::Chain m_chain; + KDL::ChainFkSolverPos_recursive* m_fksolver; + KDL::ChainJntToJacSolver* m_jacsolver; + KDL::JntArray m_chiKdl; + KDL::Jacobian m_jac; + struct ConstraintSingleValue m_data; + struct ConstraintValues m_values; + Cache* m_cache; + int m_distCCh; + CacheTS m_distCTs; + double m_maxerror; + + void pushDist(CacheTS timestamp); + bool popDist(CacheTS timestamp); + + double m_alpha,m_yddot,m_yd,m_nextyd,m_nextyddot,m_K,m_tolerance; +}; + +} + +#endif /* DISTANCE_HPP_ */ |