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