doxygen: intern/itasc tagged
[blender.git] / intern / itasc / Distance.cpp
1 /** \file itasc/Distance.cpp
2  *  \ingroup itasc
3  */
4 /* $Id$
5  * Distance.cpp
6  *
7  *  Created on: Jan 30, 2009
8  *      Author: rsmits
9  */
10
11 #include "Distance.hpp"
12 #include "kdl/kinfam_io.hpp"
13 #include <math.h>
14 #include <string.h>
15
16 namespace iTaSC
17 {
18 // a distance constraint is characterized by 5 values: alpha, tolerance, K, yd, yddot
19 static const unsigned int distanceCacheSize = sizeof(double)*5 + sizeof(e_scalar)*6;
20
21 Distance::Distance(double armlength, double accuracy, unsigned int maximum_iterations):
22     ConstraintSet(1,accuracy,maximum_iterations),
23     m_chiKdl(6),m_jac(6),m_cache(NULL),
24         m_distCCh(-1),m_distCTs(0)
25 {
26     m_chain.addSegment(Segment(Joint(Joint::RotZ)));
27     m_chain.addSegment(Segment(Joint(Joint::RotX)));
28     m_chain.addSegment(Segment(Joint(Joint::TransY)));
29     m_chain.addSegment(Segment(Joint(Joint::RotZ)));
30     m_chain.addSegment(Segment(Joint(Joint::RotY)));
31     m_chain.addSegment(Segment(Joint(Joint::RotX)));
32
33         m_fksolver = new KDL::ChainFkSolverPos_recursive(m_chain);
34         m_jacsolver = new KDL::ChainJntToJacSolver(m_chain);
35     m_Cf(0,2)=1.0;
36         m_alpha = 1.0;
37         m_tolerance = 0.05;
38         m_maxerror = armlength/2.0;
39         m_K = 20.0;
40         m_Wy(0) = m_alpha/*/(m_tolerance*m_K)*/;
41         m_yddot = m_nextyddot = 0.0;
42         m_yd = m_nextyd = KDL::epsilon;
43         memset(&m_data, 0, sizeof(m_data));
44         // initialize the data with normally fixed values
45         m_data.id = ID_DISTANCE;
46         m_values.id = ID_DISTANCE;
47         m_values.number = 1;
48         m_values.alpha = m_alpha;
49         m_values.feedback = m_K;
50         m_values.tolerance = m_tolerance;
51         m_values.values = &m_data;
52 }
53
54 Distance::~Distance()
55 {
56     delete m_fksolver;
57     delete m_jacsolver;
58 }
59
60 bool Distance::computeChi(Frame& pose)
61 {
62         double dist, alpha, beta, gamma;
63         dist = pose.p.Norm();
64         Rotation basis;
65         if (dist < KDL::epsilon) {
66                 // distance is almost 0, no need for initial rotation
67                 m_chi(0) = 0.0;
68                 m_chi(1) = 0.0;
69         } else {
70                 // find the XZ angles that bring the Y axis to point to init_pose.p
71                 Vector axis(pose.p/dist);
72                 beta = 0.0;
73                 if (fabs(axis(2)) > 1-KDL::epsilon) {
74                         // direction is aligned on Z axis, just rotation on X
75                         alpha = 0.0;
76                         gamma = KDL::sign(axis(2))*KDL::PI/2;
77                 } else {
78                         alpha = -KDL::atan2(axis(0), axis(1));
79                         gamma = KDL::atan2(axis(2), KDL::sqrt(KDL::sqr(axis(0))+KDL::sqr(axis(1))));
80                 }
81                 // rotation after first 2 joints
82                 basis = Rotation::EulerZYX(alpha, beta, gamma);
83                 m_chi(0) = alpha;
84                 m_chi(1) = gamma;
85         }
86         m_chi(2) = dist;
87         basis = basis.Inverse()*pose.M;
88         basis.GetEulerZYX(alpha, beta, gamma);
89         // alpha = rotation on Z
90         // beta = rotation on Y
91         // gamma = rotation on X in that order
92         // it corresponds to the joint order, so just assign
93         m_chi(3) = alpha;
94         m_chi(4) = beta;
95         m_chi(5) = gamma;
96         return true;
97 }
98
99 bool Distance::initialise(Frame& init_pose)
100 {
101         // we will initialize m_chi to values that match the pose
102     m_externalPose=init_pose;
103         computeChi(m_externalPose);
104         // get current Jf and update internal pose
105     updateJacobian();
106         return true;
107 }
108
109 bool Distance::closeLoop()
110 {
111         if (!Equal(m_internalPose.Inverse()*m_externalPose,F_identity,m_threshold)){
112                 computeChi(m_externalPose);
113                 updateJacobian();
114         }
115         return true;
116 }
117
118 void Distance::initCache(Cache *_cache)
119 {
120         m_cache = _cache;
121         m_distCCh = -1;
122         if (m_cache) {
123                 // create one channel for the coordinates
124                 m_distCCh = m_cache->addChannel(this, "Xf", distanceCacheSize);
125                 // save initial constraint in cache position 0
126                 pushDist(0);
127         }
128 }
129
130 void Distance::pushDist(CacheTS timestamp)
131 {
132         if (m_distCCh >= 0) {
133                 double *item = (double*)m_cache->addCacheItem(this, m_distCCh, timestamp, NULL, distanceCacheSize);
134                 if (item) {
135                         *item++ = m_K;
136                         *item++ = m_tolerance;
137                         *item++ = m_yd;
138                         *item++ = m_yddot;
139                         *item++ = m_alpha;
140                         memcpy(item, &m_chi[0], 6*sizeof(e_scalar));
141                 }
142                 m_distCTs = timestamp;
143         }
144 }
145
146 bool Distance::popDist(CacheTS timestamp)
147 {
148         if (m_distCCh >= 0) {
149                 double *item = (double*)m_cache->getPreviousCacheItem(this, m_distCCh, &timestamp);
150                 if (item && timestamp != m_distCTs) {
151                         m_values.feedback = m_K = *item++;
152                         m_values.tolerance = m_tolerance = *item++;
153                         m_yd = *item++;
154                         m_yddot = *item++;
155                         m_values.alpha = m_alpha = *item++;
156                         memcpy(&m_chi[0], item, 6*sizeof(e_scalar));
157                         m_distCTs = timestamp;
158                         m_Wy(0) = m_alpha/*/(m_tolerance*m_K)*/;
159                         updateJacobian();
160                 }
161                 return (item) ? true : false;
162         }
163         return true;
164 }
165
166 void Distance::pushCache(const Timestamp& timestamp)
167 {
168         if (!timestamp.substep && timestamp.cache)
169                 pushDist(timestamp.cacheTimestamp);
170 }
171
172 void Distance::updateKinematics(const Timestamp& timestamp)
173 {
174         if (timestamp.interpolate) {
175                 //the internal pose and Jf is already up to date (see model_update)
176                 //update the desired output based on yddot
177                 if (timestamp.substep) {
178                         m_yd += m_yddot*timestamp.realTimestep;
179                         if (m_yd < KDL::epsilon)
180                                 m_yd = KDL::epsilon;
181                 } else {
182                         m_yd = m_nextyd;
183                         m_yddot = m_nextyddot;
184                 }
185         }
186         pushCache(timestamp);
187 }
188
189 void Distance::updateJacobian()
190 {
191     for(unsigned int i=0;i<6;i++)
192         m_chiKdl(i)=m_chi(i);
193
194     m_fksolver->JntToCart(m_chiKdl,m_internalPose);
195     m_jacsolver->JntToJac(m_chiKdl,m_jac);
196     changeRefPoint(m_jac,-m_internalPose.p,m_jac);
197     for(unsigned int i=0;i<6;i++)
198         for(unsigned int j=0;j<6;j++)
199             m_Jf(i,j)=m_jac(i,j);
200 }
201
202 bool Distance::setControlParameters(struct ConstraintValues* _values, unsigned int _nvalues, double timestep)
203 {
204         int action = 0;
205         int i;
206         ConstraintSingleValue* _data;
207
208         while (_nvalues > 0) {
209                 if (_values->id == ID_DISTANCE) {
210                         if ((_values->action & ACT_ALPHA) && _values->alpha >= 0.0) {
211                                 m_alpha = _values->alpha;
212                                 action |= ACT_ALPHA;
213                         }
214                         if ((_values->action & ACT_TOLERANCE) && _values->tolerance > KDL::epsilon) {
215                                 m_tolerance = _values->tolerance;
216                                 action |= ACT_TOLERANCE;
217                         }
218                         if ((_values->action & ACT_FEEDBACK) && _values->feedback > KDL::epsilon) {
219                                 m_K = _values->feedback;
220                                 action |= ACT_FEEDBACK;
221                         }
222                         for (_data = _values->values, i=0; i<_values->number; i++, _data++) {
223                                 if (_data->id == ID_DISTANCE) {
224                                         switch (_data->action & (ACT_VALUE|ACT_VELOCITY)) {
225                                         case 0:
226                                                 // no indication, keep current values
227                                                 break;
228                                         case ACT_VELOCITY:
229                                                 // only the velocity is given estimate the new value by integration
230                                                 _data->yd = m_yd+_data->yddot*timestep;
231                                                 // walkthrough for negative value correction
232                                         case ACT_VALUE:
233                                                 // only the value is given, estimate the velocity from previous value
234                                                 if (_data->yd < KDL::epsilon)
235                                                         _data->yd = KDL::epsilon;
236                                                 m_nextyd = _data->yd;
237                                                 // if the user sets the value, we assume future velocity is zero
238                                                 // (until the user changes the value again)
239                                                 m_nextyddot = (_data->action & ACT_VALUE) ? 0.0 : _data->yddot;
240                                                 if (timestep>0.0) {
241                                                         m_yddot = (_data->yd-m_yd)/timestep;
242                                                 } else {
243                                                         // allow the user to change target instantenously when this function
244                                                         // if called from setControlParameter with timestep = 0
245                                                         m_yddot = m_nextyddot;
246                                                         m_yd = m_nextyd;
247                                                 }
248                                                 break;
249                                         case (ACT_VALUE|ACT_VELOCITY):
250                                                 // the user should not set the value and velocity at the same time.
251                                                 // In this case, we will assume that he want to set the future value
252                                                 // and we compute the current value to match the velocity
253                                                 if (_data->yd < KDL::epsilon)
254                                                         _data->yd = KDL::epsilon;
255                                                 m_yd = _data->yd - _data->yddot*timestep;
256                                                 if (m_yd < KDL::epsilon)
257                                                         m_yd = KDL::epsilon;
258                                                 m_nextyd = _data->yd;
259                                                 m_nextyddot = _data->yddot;
260                                                 if (timestep>0.0) {
261                                                         m_yddot = (_data->yd-m_yd)/timestep;
262                                                 } else {
263                                                         m_yd = m_nextyd;
264                                                         m_yddot = m_nextyddot;
265                                                 }
266                                                 break;
267                                         }
268                                 }
269                         }
270                 }
271                 _nvalues--;
272                 _values++;
273         }
274         if (action & (ACT_TOLERANCE|ACT_FEEDBACK|ACT_ALPHA)) {
275                 // recompute the weight
276                 m_Wy(0) = m_alpha/*/(m_tolerance*m_K)*/;
277         }
278         return true;
279 }
280
281 const ConstraintValues* Distance::getControlParameters(unsigned int* _nvalues)
282 {
283         *(double*)&m_data.y = m_chi(2);
284         *(double*)&m_data.ydot = m_ydot(0);
285         m_data.yd = m_yd;
286         m_data.yddot = m_yddot;
287         m_data.action = 0;
288         m_values.action = 0;
289         if (_nvalues) 
290                 *_nvalues=1; 
291         return &m_values; 
292 }
293
294 void Distance::updateControlOutput(const Timestamp& timestamp)
295 {
296         bool cacheAvail = true;
297         if (!timestamp.substep) {
298                 if (!timestamp.reiterate)
299                         cacheAvail = popDist(timestamp.cacheTimestamp);
300         }
301         if (m_constraintCallback && (m_substep || (!timestamp.reiterate && !timestamp.substep))) {
302                 // initialize first callback the application to get the current values
303                 *(double*)&m_data.y = m_chi(2);
304                 *(double*)&m_data.ydot = m_ydot(0);
305                 m_data.yd = m_yd;
306                 m_data.yddot = m_yddot;
307                 m_data.action = 0;
308                 m_values.action = 0;
309                 if ((*m_constraintCallback)(timestamp, &m_values, 1, m_constraintParam)) {
310                         setControlParameters(&m_values, 1, timestamp.realTimestep);
311                 }
312         }
313         if (!cacheAvail || !timestamp.interpolate) {
314                 // first position in cache: set the desired output immediately as we cannot interpolate
315                 m_yd = m_nextyd;
316                 m_yddot = m_nextyddot;
317         }
318         double error = m_yd-m_chi(2);
319         if (KDL::Norm(error) > m_maxerror)
320                 error = KDL::sign(error)*m_maxerror;
321     m_ydot(0)=m_yddot+m_K*error;
322 }
323
324 }