Blender  V3.3
Distance.cpp
Go to the documentation of this file.
1 /* SPDX-License-Identifier: LGPL-2.1-or-later
2  * Copyright 2009 Ruben Smits. */
3 
8 #include "Distance.hpp"
9 #include "kdl/kinfam_io.hpp"
10 #include <math.h>
11 #include <string.h>
12 
13 namespace iTaSC
14 {
15 // a distance constraint is characterized by 5 values: alpha, tolerance, K, yd, yddot
16 static const unsigned int distanceCacheSize = sizeof(double)*5 + sizeof(e_scalar)*6;
17 
18 Distance::Distance(double armlength, double accuracy, unsigned int maximum_iterations):
19  ConstraintSet(1,accuracy,maximum_iterations),
20  m_chiKdl(6),m_jac(6),m_cache(NULL),
21  m_distCCh(-1),m_distCTs(0)
22 {
29 
30  m_fksolver = new KDL::ChainFkSolverPos_recursive(m_chain);
31  m_jacsolver = new KDL::ChainJntToJacSolver(m_chain);
32  m_Cf(0,2)=1.0;
33  m_alpha = 1.0;
34  m_tolerance = 0.05;
35  m_maxerror = armlength/2.0;
36  m_K = 20.0;
37  m_Wy(0) = m_alpha/*/(m_tolerance*m_K)*/;
38  m_yddot = m_nextyddot = 0.0;
39  m_yd = m_nextyd = KDL::epsilon;
40  memset(&m_data, 0, sizeof(m_data));
41  // initialize the data with normally fixed values
42  m_data.id = ID_DISTANCE;
43  m_values.id = ID_DISTANCE;
44  m_values.number = 1;
45  m_values.alpha = m_alpha;
46  m_values.feedback = m_K;
47  m_values.tolerance = m_tolerance;
48  m_values.values = &m_data;
49 }
50 
52 {
53  delete m_fksolver;
54  delete m_jacsolver;
55 }
56 
57 bool Distance::computeChi(Frame& pose)
58 {
59  double dist, alpha, beta, gamma;
60  dist = pose.p.Norm();
61  Rotation basis;
62  if (dist < KDL::epsilon) {
63  // distance is almost 0, no need for initial rotation
64  m_chi(0) = 0.0;
65  m_chi(1) = 0.0;
66  } else {
67  // find the XZ angles that bring the Y axis to point to init_pose.p
68  Vector axis(pose.p/dist);
69  beta = 0.0;
70  if (fabs(axis(2)) > 1-KDL::epsilon) {
71  // direction is aligned on Z axis, just rotation on X
72  alpha = 0.0;
73  gamma = KDL::sign(axis(2))*KDL::PI/2;
74  } else {
75  alpha = -KDL::atan2(axis(0), axis(1));
76  gamma = KDL::atan2(axis(2), KDL::sqrt(KDL::sqr(axis(0))+KDL::sqr(axis(1))));
77  }
78  // rotation after first 2 joints
79  basis = Rotation::EulerZYX(alpha, beta, gamma);
80  m_chi(0) = alpha;
81  m_chi(1) = gamma;
82  }
83  m_chi(2) = dist;
84  basis = basis.Inverse()*pose.M;
85  basis.GetEulerZYX(alpha, beta, gamma);
86  // alpha = rotation on Z
87  // beta = rotation on Y
88  // gamma = rotation on X in that order
89  // it corresponds to the joint order, so just assign
90  m_chi(3) = alpha;
91  m_chi(4) = beta;
92  m_chi(5) = gamma;
93  return true;
94 }
95 
96 bool Distance::initialise(Frame& init_pose)
97 {
98  // we will initialize m_chi to values that match the pose
99  m_externalPose=init_pose;
100  computeChi(m_externalPose);
101  // get current Jf and update internal pose
102  updateJacobian();
103  return true;
104 }
105 
107 {
109  computeChi(m_externalPose);
110  updateJacobian();
111  }
112  return true;
113 }
114 
116 {
117  m_cache = _cache;
118  m_distCCh = -1;
119  if (m_cache) {
120  // create one channel for the coordinates
121  m_distCCh = m_cache->addChannel(this, "Xf", distanceCacheSize);
122  // save initial constraint in cache position 0
123  pushDist(0);
124  }
125 }
126 
127 void Distance::pushDist(CacheTS timestamp)
128 {
129  if (m_distCCh >= 0) {
130  double *item = (double*)m_cache->addCacheItem(this, m_distCCh, timestamp, NULL, distanceCacheSize);
131  if (item) {
132  *item++ = m_K;
133  *item++ = m_tolerance;
134  *item++ = m_yd;
135  *item++ = m_yddot;
136  *item++ = m_alpha;
137  memcpy(item, &m_chi[0], 6*sizeof(e_scalar));
138  }
139  m_distCTs = timestamp;
140  }
141 }
142 
143 bool Distance::popDist(CacheTS timestamp)
144 {
145  if (m_distCCh >= 0) {
146  double *item = (double*)m_cache->getPreviousCacheItem(this, m_distCCh, &timestamp);
147  if (item && timestamp != m_distCTs) {
148  m_values.feedback = m_K = *item++;
149  m_values.tolerance = m_tolerance = *item++;
150  m_yd = *item++;
151  m_yddot = *item++;
152  m_values.alpha = m_alpha = *item++;
153  memcpy(&m_chi[0], item, 6*sizeof(e_scalar));
154  m_distCTs = timestamp;
155  m_Wy(0) = m_alpha/*/(m_tolerance*m_K)*/;
156  updateJacobian();
157  }
158  return (item) ? true : false;
159  }
160  return true;
161 }
162 
163 void Distance::pushCache(const Timestamp& timestamp)
164 {
165  if (!timestamp.substep && timestamp.cache)
166  pushDist(timestamp.cacheTimestamp);
167 }
168 
169 void Distance::updateKinematics(const Timestamp& timestamp)
170 {
171  if (timestamp.interpolate) {
172  //the internal pose and Jf is already up to date (see model_update)
173  //update the desired output based on yddot
174  if (timestamp.substep) {
175  m_yd += m_yddot*timestamp.realTimestep;
176  if (m_yd < KDL::epsilon)
177  m_yd = KDL::epsilon;
178  } else {
179  m_yd = m_nextyd;
180  m_yddot = m_nextyddot;
181  }
182  }
183  pushCache(timestamp);
184 }
185 
187 {
188  for(unsigned int i=0;i<6;i++)
189  m_chiKdl[i]=m_chi[i];
190 
191  m_fksolver->JntToCart(m_chiKdl,m_internalPose);
192  m_jacsolver->JntToJac(m_chiKdl,m_jac);
193  changeRefPoint(m_jac,-m_internalPose.p,m_jac);
194  for(unsigned int i=0;i<6;i++)
195  for(unsigned int j=0;j<6;j++)
196  m_Jf(i,j)=m_jac(i,j);
197 }
198 
199 bool Distance::setControlParameters(struct ConstraintValues* _values, unsigned int _nvalues, double timestep)
200 {
201  int action = 0;
202  int i;
203  ConstraintSingleValue* _data;
204 
205  while (_nvalues > 0) {
206  if (_values->id == ID_DISTANCE) {
207  if ((_values->action & ACT_ALPHA) && _values->alpha >= 0.0) {
208  m_alpha = _values->alpha;
209  action |= ACT_ALPHA;
210  }
211  if ((_values->action & ACT_TOLERANCE) && _values->tolerance > KDL::epsilon) {
212  m_tolerance = _values->tolerance;
213  action |= ACT_TOLERANCE;
214  }
215  if ((_values->action & ACT_FEEDBACK) && _values->feedback > KDL::epsilon) {
216  m_K = _values->feedback;
217  action |= ACT_FEEDBACK;
218  }
219  for (_data = _values->values, i=0; i<_values->number; i++, _data++) {
220  if (_data->id == ID_DISTANCE) {
221  switch (_data->action & (ACT_VALUE|ACT_VELOCITY)) {
222  case 0:
223  // no indication, keep current values
224  break;
225  case ACT_VELOCITY:
226  // only the velocity is given estimate the new value by integration
227  _data->yd = m_yd+_data->yddot*timestep;
228  // walkthrough for negative value correction
229  case ACT_VALUE:
230  // only the value is given, estimate the velocity from previous value
231  if (_data->yd < KDL::epsilon)
232  _data->yd = KDL::epsilon;
233  m_nextyd = _data->yd;
234  // if the user sets the value, we assume future velocity is zero
235  // (until the user changes the value again)
236  m_nextyddot = (_data->action & ACT_VALUE) ? 0.0 : _data->yddot;
237  if (timestep>0.0) {
238  m_yddot = (_data->yd-m_yd)/timestep;
239  } else {
240  // allow the user to change target instantenously when this function
241  // if called from setControlParameter with timestep = 0
242  m_yddot = m_nextyddot;
243  m_yd = m_nextyd;
244  }
245  break;
246  case (ACT_VALUE|ACT_VELOCITY):
247  // the user should not set the value and velocity at the same time.
248  // In this case, we will assume that he want to set the future value
249  // and we compute the current value to match the velocity
250  if (_data->yd < KDL::epsilon)
251  _data->yd = KDL::epsilon;
252  m_yd = _data->yd - _data->yddot*timestep;
253  if (m_yd < KDL::epsilon)
254  m_yd = KDL::epsilon;
255  m_nextyd = _data->yd;
256  m_nextyddot = _data->yddot;
257  if (timestep>0.0) {
258  m_yddot = (_data->yd-m_yd)/timestep;
259  } else {
260  m_yd = m_nextyd;
261  m_yddot = m_nextyddot;
262  }
263  break;
264  }
265  }
266  }
267  }
268  _nvalues--;
269  _values++;
270  }
271  if (action & (ACT_TOLERANCE|ACT_FEEDBACK|ACT_ALPHA)) {
272  // recompute the weight
273  m_Wy(0) = m_alpha/*/(m_tolerance*m_K)*/;
274  }
275  return true;
276 }
277 
278 const ConstraintValues* Distance::getControlParameters(unsigned int* _nvalues)
279 {
280  *(double*)&m_data.y = m_chi(2);
281  *(double*)&m_data.ydot = m_ydot(0);
282  m_data.yd = m_yd;
283  m_data.yddot = m_yddot;
284  m_data.action = 0;
285  m_values.action = 0;
286  if (_nvalues)
287  *_nvalues=1;
288  return &m_values;
289 }
290 
292 {
293  bool cacheAvail = true;
294  if (!timestamp.substep) {
295  if (!timestamp.reiterate)
296  cacheAvail = popDist(timestamp.cacheTimestamp);
297  }
298  if (m_constraintCallback && (m_substep || (!timestamp.reiterate && !timestamp.substep))) {
299  // initialize first callback the application to get the current values
300  *(double*)&m_data.y = m_chi(2);
301  *(double*)&m_data.ydot = m_ydot(0);
302  m_data.yd = m_yd;
303  m_data.yddot = m_yddot;
304  m_data.action = 0;
305  m_values.action = 0;
306  if ((*m_constraintCallback)(timestamp, &m_values, 1, m_constraintParam)) {
307  setControlParameters(&m_values, 1, timestamp.realTimestep);
308  }
309  }
310  if (!cacheAvail || !timestamp.interpolate) {
311  // first position in cache: set the desired output immediately as we cannot interpolate
312  m_yd = m_nextyd;
313  m_yddot = m_nextyddot;
314  }
315  double error = m_yd-m_chi(2);
316  if (KDL::Norm(error) > m_maxerror)
317  error = KDL::sign(error)*m_maxerror;
318  m_ydot(0)=m_yddot+m_K*error;
319 }
320 
321 }
typedef double(DMatrix)[4][4]
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
Class to calculate the jacobian of a general KDL::Chain, it is used by other solvers....
int JntToJac(const JntArray &q_in, Jacobian &jac)
void addSegment(const Segment &segment)
Definition: chain.cpp:55
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:526
Rotation M
Orientation of the Frame.
Definition: frames.hpp:529
Vector p
origine of the Frame
Definition: frames.hpp:528
Frame Inverse() const
Gives back inverse transformation of a Frame.
Definition: frames.inl:431
This class encapsulates a simple joint, that is with one parameterized degree of freedom and with sca...
Definition: joint.hpp:43
@ TransY
Definition: joint.hpp:45
represents rotations in 3 dimensional space.
Definition: frames.hpp:299
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
Definition: frames.inl:641
static Rotation EulerZYX(double Alfa, double Beta, double Gamma)
Definition: frames.hpp:435
void GetEulerZYX(double &Alfa, double &Beta, double &Gamma) const
Definition: frames.hpp:450
This class encapsulates a simple segment, that is a "rigid body" (i.e., a frame and an inertia) with...
Definition: segment.hpp:46
double Norm() const
Definition: frames.cpp:115
int addChannel(const void *device, const char *name, unsigned int maxItemSize)
Definition: Cache.cpp:201
void * addCacheItem(const void *device, int channel, CacheTS timestamp, void *data, unsigned int length)
Definition: Cache.cpp:403
const void * getPreviousCacheItem(const void *device, int channel, CacheTS *timestamp)
Definition: Cache.cpp:543
ConstraintCallback m_constraintCallback
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
#define e_scalar
Definition: eigen_types.hpp:37
ccl_device_inline float2 fabs(const float2 &a)
Definition: math_float2.h:222
static void error(const char *str)
Definition: meshlaplacian.c:51
INLINE S Norm(const Rall1d< T, V, S > &value)
Definition: rall1d.h:416
double sign(double arg)
Definition: utility.h:250
const double PI
the value of pi
Definition: utility.cpp:19
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:367
void changeRefPoint(const Jacobian &src1, const Vector &base_AB, Jacobian &dest)
Definition: jacobian.cpp:87
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
Definition: utility.cpp:22
IMETHOD bool Equal(const VectorAcc &, const VectorAcc &, double=epsilon)
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
Definition: rall1d.h:429
INLINE Rall1d< T, V, S > sqr(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:351
const Frame F_identity
unsigned int CacheTS
Definition: Cache.hpp:32
static const unsigned int distanceCacheSize
Definition: Distance.cpp:16
@ ACT_TOLERANCE
@ ACT_FEEDBACK
@ ACT_VELOCITY
struct ConstraintSingleValue * values
unsigned int interpolate
Definition: Cache.hpp:44
double realTimestep
Definition: Cache.hpp:37
unsigned int cache
Definition: Cache.hpp:42
CacheTS cacheTimestamp
Definition: Cache.hpp:38
unsigned int reiterate
Definition: Cache.hpp:41
unsigned int substep
Definition: Cache.hpp:40
ccl_device_inline float beta(float x, float y)
Definition: util/math.h:775