blob: 02a8f9d9f2948199de1abbb82790f610d508278f (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
|
/* SPDX-License-Identifier: LGPL-2.1-or-later
* Copyright 2009 Ruben Smits. */
/** \file
* \ingroup intern_itasc
*/
#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_ */
|
