Fix OSX compilation problem with malloc.h in itasc
[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 <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 {
23     m_chain.addSegment(Segment(Joint(Joint::RotZ)));
24     m_chain.addSegment(Segment(Joint(Joint::RotX)));
25     m_chain.addSegment(Segment(Joint(Joint::TransY)));
26     m_chain.addSegment(Segment(Joint(Joint::RotZ)));
27     m_chain.addSegment(Segment(Joint(Joint::RotY)));
28     m_chain.addSegment(Segment(Joint(Joint::RotX)));
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
51 Distance::~Distance()
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
106 bool Distance::closeLoop()
107 {
108         if (!Equal(m_internalPose.Inverse()*m_externalPose,F_identity,m_threshold)){
109                 computeChi(m_externalPose);
110                 updateJacobian();
111         }
112         return true;
113 }
114
115 void Distance::initCache(Cache *_cache)
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
186 void Distance::updateJacobian()
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
291 void Distance::updateControlOutput(const Timestamp& timestamp)
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 }