Blender  V3.3
Distance.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 DISTANCE_HPP_
9 #define DISTANCE_HPP_
10 
11 #include "ConstraintSet.hpp"
12 #include "kdl/chain.hpp"
15 
16 namespace iTaSC
17 {
18 
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 {
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;
44  KDL::ChainJntToJacSolver* m_jacsolver;
45  KDL::JntArray m_chiKdl;
46  KDL::Jacobian m_jac;
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_ */
ATTR_WARN_UNUSED_RESULT const BMVert const BMEdge * e
btAlignedObjectArray< btScalar > m_data
Class to calculate the jacobian of a general KDL::Chain, it is used by other solvers....
This class encapsulates a serial kinematic interconnection structure. It is build out of segments.
Definition: chain.hpp:36
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:526
virtual void initCache(Cache *_cache)
Definition: Distance.cpp:115
virtual void updateKinematics(const Timestamp &timestamp)
Definition: Distance.cpp:169
Distance(double armlength=1.0, double accuracy=1e-6, unsigned int maximum_iterations=100)
Definition: Distance.cpp:18
virtual bool setControlParameters(struct ConstraintValues *_values, unsigned int _nvalues, double timestep)
Definition: Distance.cpp:199
virtual void updateControlOutput(const Timestamp &timestamp)
Definition: Distance.cpp:291
virtual bool closeLoop()
Definition: Distance.cpp:106
virtual void updateJacobian()
Definition: Distance.cpp:186
virtual void pushCache(const Timestamp &timestamp)
Definition: Distance.cpp:163
virtual bool initialise(Frame &init_pose)
Definition: Distance.cpp:96
virtual ~Distance()
Definition: Distance.cpp:51
virtual const ConstraintValues * getControlParameters(unsigned int *_nvalues)
Definition: Distance.cpp:278
unsigned int CacheTS
Definition: Cache.hpp:32
Definition: DNA_ID.h:368