1 /* Siconos is a program dedicated to modeling, simulation and control
2 * of non smooth dynamical systems.
3 *
4 * Copyright 2021 INRIA.
5 *
6 * Licensed under the Apache License, Version 2.0 (the "License");
7 * you may not use this file except in compliance with the License.
8 * You may obtain a copy of the License at
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing, software
13 * distributed under the License is distributed on an "AS IS" BASIS,
14 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 * See the License for the specific language governing permissions and
16 * limitations under the License.
17 */
18 #include "SiconosMatrixSetBlock.hpp"
19 #include "SiconosAlgebraProd.hpp"
20 #include "NewtonEulerDS.hpp"
21 #include "BlockVector.hpp"
22 #include "BlockMatrix.hpp"
23 #include <boost/math/quaternion.hpp>
24
25 #include "RotationQuaternion.hpp"
26 #include <iostream>
27
28 // #define DEBUG_STDOUT
29 // #define DEBUG_MESSAGES
30 #include "siconos_debug.h"
31
32
33
34
35 static
computeJacobianConvectedVectorInBodyFrame(double q0,double q1,double q2,double q3,SP::SimpleMatrix jacobian,SP::SiconosVector v)36 void computeJacobianConvectedVectorInBodyFrame(double q0, double q1, double q2, double q3,
37 SP::SimpleMatrix jacobian, SP::SiconosVector v)
38 {
39
40 /* This routine compute the jacobian with respect to p of R^T(p)v */
41 jacobian->zero();
42
43 double v0 = v->getValue(0);
44 double v1 = v->getValue(1);
45 double v2 = v->getValue(2);
46
47 jacobian->setValue(0,3, q0*v0+q3*v1-q2*v2);
48 jacobian->setValue(0,4, q1*v0+q2*v1+q3*v2);
49 jacobian->setValue(0,5,-q2*v0+q1*v1-q0*v2);
50 jacobian->setValue(0,6,-q3*v0+q0*v1+q1*v2);
51
52 jacobian->setValue(1,3,-q3*v0+q0*v1+q1*v2);
53 jacobian->setValue(1,4, q2*v0-q1*v1+q0*v2);
54 jacobian->setValue(1,5, q1*v0+q2*v1+q3*v2);
55 jacobian->setValue(1,6,-q0*v0-q3*v1+q2*v2);
56
57 jacobian->setValue(2,3, q2*v0-q1*v1+q0*v2);
58 jacobian->setValue(2,4, q3*v0-q0*v1-q1*v2);
59 jacobian->setValue(2,5, q0*v0+q3*v1-q2*v2);
60 jacobian->setValue(2,6, q1*v0+q2*v1+q3*v2);
61
62
63 *jacobian *=2.0;
64 }
65
66
computeT(SP::SiconosVector q,SP::SimpleMatrix T)67 void computeT(SP::SiconosVector q, SP::SimpleMatrix T)
68 {
69 DEBUG_BEGIN("computeT(SP::SiconosVector q, SP::SimpleMatrix T)\n")
70 // std::cout <<"\n NewtonEulerDS::computeT(SP::SiconosVector q)\n " <<std::endl;
71 double q0 = q->getValue(3) / 2.0;
72 double q1 = q->getValue(4) / 2.0;
73 double q2 = q->getValue(5) / 2.0;
74 double q3 = q->getValue(6) / 2.0;
75 T->setValue(3, 3, -q1);
76 T->setValue(3, 4, -q2);
77 T->setValue(3, 5, -q3);
78 T->setValue(4, 3, q0);
79 T->setValue(4, 4, -q3);
80 T->setValue(4, 5, q2);
81 T->setValue(5, 3, q3);
82 T->setValue(5, 4, q0);
83 T->setValue(5, 5, -q1);
84 T->setValue(6, 3, -q2);
85 T->setValue(6, 4, q1);
86 T->setValue(6, 5, q0);
87 DEBUG_END("computeT(SP::SiconosVector q, SP::SimpleMatrix T)\n")
88
89 }
90
91 // From a set of data; Mass filled-in directly from a siconosMatrix -
92 // This constructor leads to the minimum NewtonEuler System form: \f$ M\ddot q = p \f$
93 /*
94 Q0 : contains the center of mass coordinate, and the quaternion initial. (dim(Q0)=7)
95 Twist0 : contains the initial velocity of center of mass and the omega initial. (dim(VTwist0)=6)
96 */
NewtonEulerDS()97 NewtonEulerDS::NewtonEulerDS():
98 SecondOrderDS(13,6),
99 _hasConstantFExt(false),
100 _hasConstantMExt(false),
101 _isMextExpressedInInertialFrame(false),
102 _nullifyMGyr(false),
103 _computeJacobianFIntqByFD(true),
104 _computeJacobianFInttwistByFD(true),
105 _computeJacobianMIntqByFD(true),
106 _computeJacobianMInttwistByFD(true),
107 _epsilonFD(sqrt(std::numeric_limits< double >::epsilon()))
108 {
109 /* common code for constructors
110 * would be better to use delagation of constructors in c++11
111 */
112 _init();
113 }
114
115
NewtonEulerDS(SP::SiconosVector Q0,SP::SiconosVector Twist0,double mass,SP::SiconosMatrix inertialMatrix)116 NewtonEulerDS::NewtonEulerDS(SP::SiconosVector Q0, SP::SiconosVector Twist0,
117 double mass, SP::SiconosMatrix inertialMatrix):
118 SecondOrderDS(13,6),
119 _hasConstantFExt(false),
120 _hasConstantMExt(false),
121 _isMextExpressedInInertialFrame(false),
122 _nullifyMGyr(false),
123 _computeJacobianFIntqByFD(true),
124 _computeJacobianFInttwistByFD(true),
125 _computeJacobianMIntqByFD(true),
126 _computeJacobianMInttwistByFD(true),
127 _epsilonFD(sqrt(std::numeric_limits< double >::epsilon()))
128
129 {
130 DEBUG_BEGIN("NewtonEulerDS::NewtonEulerDS(SP::SiconosVector Q0, SP::SiconosVector Twist0,double mass, SP::SiconosMatrix inertialMatrix)\n");
131
132 /* common code for constructors
133 * would be better to use delegation of constructors in c++11
134 */
135 _init();
136
137 // Initial conditions
138 _q0 = Q0;
139 _twist0 = Twist0;
140 resetToInitialState();
141
142 _scalarMass = mass;
143 if(inertialMatrix)
144 _I = inertialMatrix;
145 updateMassMatrix();
146
147 _T->zero();
148 _T->setValue(0, 0, 1.0);
149 _T->setValue(1, 1, 1.0);
150 _T->setValue(2, 2, 1.0);
151 computeT();
152
153 DEBUG_END("NewtonEulerDS::NewtonEulerDS(SP::SiconosVector Q0, SP::SiconosVector Twist0,double mass, SP::SiconosMatrix inertialMatrix)\n");
154 }
155
156
init_forces()157 void NewtonEulerDS::init_forces()
158 {
159 // Allocate memory for forces and its jacobians.
160 // Needed only for integrators with first-order formulation.
161
162 if(!_wrench)
163 _wrench.reset(new SiconosVector(_ndof));
164 if(!_mGyr)
165 _mGyr.reset(new SiconosVector(3,0.0));
166
167 /** The follwing jacobian are always allocated since we have always
168 * Gyroscopical forces that has non linear forces
169 * This should be remove if the integration is explicit or _nullifyMGyr(false) is set to true ?
170 */
171 _jacobianMGyrtwist.reset(new SimpleMatrix(3, _ndof));
172 _jacobianWrenchTwist.reset(new SimpleMatrix(_ndof, _ndof));
173
174 }
175
176
_init()177 void NewtonEulerDS::_init()
178 {
179 // --- NEWTONEULER INHERITED CLASS MEMBERS ---
180 // -- Memory allocation for vector and matrix members --
181
182 _qDim = 7;
183 _ndof = 6;
184 _n = _qDim + _ndof;
185
186
187 _zeroPlugin();
188
189 // Current state
190 _q.reset(new SiconosVector(_qDim));
191 _twist.reset(new SiconosVector(_ndof));
192 _dotq.reset(new SiconosVector(_qDim));
193
194 /** \todo lazy Memory allocation */
195 _p.resize(3);
196 _p[1].reset(new SiconosVector(_ndof)); // Needed in NewtonEulerR
197
198
199 _mass.reset(new SimpleMatrix(_ndof, _ndof));
200 _mass->zero();
201 _T.reset(new SimpleMatrix(_qDim, _ndof));
202
203 _scalarMass = 1.;
204 _I.reset(new SimpleMatrix(3, 3));
205 _I->eye();
206 updateMassMatrix();
207
208 init_forces();
209
210 //We initialize _z with a null vector of size 1, since z is required in plug-in functions call.
211 _z.reset(new SiconosVector(1));
212
213 }
214
updateMassMatrix()215 void NewtonEulerDS::updateMassMatrix()
216 {
217 // _mass->zero();
218 // _mass->setValue(0, 0, _scalarMass);
219 // _mass->setValue(1, 1, _scalarMass);
220 // _mass->setValue(2, 2, _scalarMass);
221
222 _mass->eye();
223 * _mass *= _scalarMass;
224
225 Index dimIndex(2);
226 dimIndex[0] = 3;
227 dimIndex[1] = 3;
228 Index startIndex(4);
229 startIndex[0] = 0;
230 startIndex[1] = 0;
231 startIndex[2] = 3;
232 startIndex[3] = 3;
233 setBlock(_I, _mass, dimIndex, startIndex);
234
235 }
236
_zeroPlugin()237 void NewtonEulerDS::_zeroPlugin()
238 {
239 _pluginFExt.reset(new PluggedObject());
240 _pluginMExt.reset(new PluggedObject());
241 _pluginFInt.reset(new PluggedObject());
242 _pluginMInt.reset(new PluggedObject());
243 _pluginJacqFInt.reset(new PluggedObject());
244 _pluginJactwistFInt.reset(new PluggedObject());
245 _pluginJacqMInt.reset(new PluggedObject());
246 _pluginJactwistMInt.reset(new PluggedObject());
247 }
248
249 // Destructor
~NewtonEulerDS()250 NewtonEulerDS::~NewtonEulerDS()
251 {
252 }
253
setInertia(double ix,double iy,double iz)254 void NewtonEulerDS::setInertia(double ix, double iy, double iz)
255 {
256 _I->zero();
257
258 (*_I)(0, 0) = ix;
259 (*_I)(1, 1) = iy;
260 (*_I)(2, 2) = iz;
261
262 updateMassMatrix();
263 }
264
initializeNonSmoothInput(unsigned int level)265 void NewtonEulerDS::initializeNonSmoothInput(unsigned int level)
266 {
267 DEBUG_PRINTF("NewtonEulerDS::initializeNonSmoothInput(unsigned int level) for level = %i\n",level);
268
269 if(!_p[level])
270 {
271 if(level == 0)
272 {
273 _p[level].reset(new SiconosVector(_qDim));
274 }
275 else
276 _p[level].reset(new SiconosVector(_ndof));
277 }
278
279 #ifdef DEBUG_MESSAGES
280 DEBUG_PRINT("display() after initialization");
281 display();
282 #endif
283 }
284
285
286
initRhs(double time)287 void NewtonEulerDS::initRhs(double time)
288 {
289 DEBUG_BEGIN("NewtonEulerDS::initRhs(double time)\n");
290 // dim
291 _n = _qDim + 6;
292
293 _x0.reset(new SiconosVector(*_q0, *_twist0));
294
295 _x[0].reset(new SiconosVector(*_q, *_twist));
296
297 if(!_acceleration)
298 _acceleration.reset(new SiconosVector(6));
299
300 // Compute _dotq
301 computeT();
302 prod(*_T, *_twist, *_dotq, true);
303 _x[1].reset(new SiconosVector(*_dotq, *_acceleration));
304
305
306 // Nothing to do for the initialization of the wrench
307
308
309 // Everything concerning rhs and its jacobian is handled in initRhs and computeXXX related functions.
310 _rhsMatrices.resize(numberOfRhsMatrices);
311
312 if(!_p[2])
313 _p[2].reset(new SiconosVector(6));
314
315
316 init_inverse_mass();
317
318 computeRhs(time);
319
320 /** \warning the derivative of T w.r.t to q is neglected */
321 _rhsMatrices[jacobianXBloc00].reset(new SimpleMatrix(_qDim, _qDim, Siconos::ZERO));
322
323
324 _rhsMatrices[jacobianXBloc01].reset(new SimpleMatrix(*_T));
325 bool flag1 = false, flag2 = false;
326 if(_jacobianWrenchq)
327 {
328 // Solve MjacobianX(1,0) = jacobianFL[0]
329 computeJacobianqForces(time);
330
331 _rhsMatrices[jacobianXBloc10].reset(new SimpleMatrix(*_jacobianWrenchq));
332 _inverseMass->Solve(*_rhsMatrices[jacobianXBloc10]);
333 flag1 = true;
334 }
335
336 if(_jacobianWrenchTwist)
337 {
338 // Solve MjacobianX(1,1) = jacobianFL[1]
339 computeJacobianvForces(time);
340 _rhsMatrices[jacobianXBloc11].reset(new SimpleMatrix(*_jacobianWrenchTwist));
341 _inverseMass->Solve(*_rhsMatrices[jacobianXBloc11]);
342 flag2 = true;
343 }
344
345 if(!_rhsMatrices[zeroMatrix])
346 _rhsMatrices[zeroMatrix].reset(new SimpleMatrix(6, 6, Siconos::ZERO));
347
348 if(!_rhsMatrices[zeroMatrixqDim])
349 _rhsMatrices[zeroMatrixqDim].reset(new SimpleMatrix(6, _qDim, Siconos::ZERO));
350
351 if(flag1 && flag2)
352 _jacxRhs.reset(new BlockMatrix(_rhsMatrices[jacobianXBloc00], _rhsMatrices[jacobianXBloc01],
353 _rhsMatrices[jacobianXBloc10], _rhsMatrices[jacobianXBloc11]));
354 else if(flag1) // flag2 = false
355 _jacxRhs.reset(new BlockMatrix(_rhsMatrices[jacobianXBloc00], _rhsMatrices[jacobianXBloc01],
356 _rhsMatrices[jacobianXBloc10], _rhsMatrices[zeroMatrix]));
357 else if(flag2) // flag1 = false
358 _jacxRhs.reset(new BlockMatrix(_rhsMatrices[jacobianXBloc00], _rhsMatrices[jacobianXBloc01],
359 _rhsMatrices[zeroMatrixqDim], _rhsMatrices[jacobianXBloc11]));
360 else
361 _jacxRhs.reset(new BlockMatrix(_rhsMatrices[jacobianXBloc00], _rhsMatrices[jacobianXBloc01],
362 _rhsMatrices[zeroMatrixqDim], _rhsMatrices[zeroMatrix]));
363 DEBUG_EXPR(display(););
364 DEBUG_END("NewtonEulerDS::initRhs(double time)\n");
365 }
366
setQ(const SiconosVector & newValue)367 void NewtonEulerDS::setQ(const SiconosVector& newValue)
368 {
369 if(newValue.size() != _qDim)
370 THROW_EXCEPTION("NewtonEulerDS - setQ: inconsistent input vector size ");
371
372 if(! _q)
373 _q.reset(new SiconosVector(newValue));
374 else
375 *_q = newValue;
376 }
377
setQPtr(SP::SiconosVector newPtr)378 void NewtonEulerDS::setQPtr(SP::SiconosVector newPtr)
379 {
380 if(newPtr->size() != _qDim)
381 THROW_EXCEPTION("NewtonEulerDS - setQPtr: inconsistent input vector size ");
382 _q = newPtr;
383 }
setQ0(const SiconosVector & newValue)384 void NewtonEulerDS::setQ0(const SiconosVector& newValue)
385 {
386 if(newValue.size() != _qDim)
387 THROW_EXCEPTION("NewtonEulerDS - setQ0: inconsistent input vector size ");
388
389 if(! _q0)
390 _q0.reset(new SiconosVector(newValue));
391 else
392 *_q0 = newValue;
393 }
394
setQ0Ptr(SP::SiconosVector newPtr)395 void NewtonEulerDS::setQ0Ptr(SP::SiconosVector newPtr)
396 {
397 if(newPtr->size() != _qDim)
398 THROW_EXCEPTION("NewtonEulerDS - setQ0Ptr: inconsistent input vector size ");
399 _q0 = newPtr;
400 }
setVelocity(const SiconosVector & newValue)401 void NewtonEulerDS::setVelocity(const SiconosVector& newValue)
402 {
403 if(newValue.size() != _ndof)
404 THROW_EXCEPTION("NewtonEulerDS - setVelocity: inconsistent input vector size ");
405
406 if(! _twist)
407 _twist0.reset(new SiconosVector(newValue));
408 else
409 *_twist = newValue;
410 }
411
setVelocityPtr(SP::SiconosVector newPtr)412 void NewtonEulerDS::setVelocityPtr(SP::SiconosVector newPtr)
413 {
414 if(newPtr->size() != _ndof)
415 THROW_EXCEPTION("NewtonEulerDS - setVelocityPtr: inconsistent input vector size ");
416 _twist = newPtr;
417 }
418
setVelocity0(const SiconosVector & newValue)419 void NewtonEulerDS::setVelocity0(const SiconosVector& newValue)
420 {
421 if(newValue.size() != _ndof)
422 THROW_EXCEPTION("NewtonEulerDS - setVelocity0: inconsistent input vector size ");
423
424 if(! _twist0)
425 _twist0.reset(new SiconosVector(newValue));
426 else
427 *_twist0 = newValue;
428 }
429
setVelocity0Ptr(SP::SiconosVector newPtr)430 void NewtonEulerDS::setVelocity0Ptr(SP::SiconosVector newPtr)
431 {
432 if(newPtr->size() != _ndof)
433 THROW_EXCEPTION("NewtonEulerDS - setVelocity0Ptr: inconsistent input vector size ");
434 _twist0 = newPtr;
435 }
436
437
438
resetToInitialState()439 void NewtonEulerDS::resetToInitialState()
440 {
441 // set q and q[1] to q0 and Twist0
442 if(_q0)
443 {
444 *_q = *_q0;
445 }
446 else
447 THROW_EXCEPTION("NewtonEulerDS::resetToInitialState - initial position _q0 is null");
448
449
450 if(_twist0)
451 {
452 *_twist = *_twist0;
453 }
454 else
455 THROW_EXCEPTION("NewtonEulerDS::resetToInitialState - initial twist _twist0 is null");
456 }
457
init_inverse_mass()458 void NewtonEulerDS::init_inverse_mass()
459 {
460 if(_mass && !_inverseMass)
461 {
462 updateMassMatrix();
463 _inverseMass.reset(new SimpleMatrix(*_mass));
464 }
465 }
466
update_inverse_mass()467 void NewtonEulerDS::update_inverse_mass()
468 {
469 if(_mass && _inverseMass)
470 {
471 updateMassMatrix();
472 *_inverseMass = *_mass;
473 }
474 }
475
computeFExt(double time)476 void NewtonEulerDS::computeFExt(double time)
477 {
478 // computeFExt(time, _fExt);
479
480 if(_pluginFExt->fPtr)
481 {
482 ((FExt_NE)_pluginFExt->fPtr)(time, &(*_fExt)(0), _qDim, &(*_q0)(0)); // parameter z are assumed to be equal to q0
483 }
484
485 }
486
computeFExt(double time,SP::SiconosVector fExt)487 void NewtonEulerDS::computeFExt(double time, SP::SiconosVector fExt)
488 {
489 /* if the pointer has been set to an external vector
490 * after setting the plugin, we do not call the plugin */
491 if(_hasConstantFExt)
492 {
493 if(fExt != _fExt)
494 *fExt = *_fExt;
495 }
496 else
497 {
498 if(_pluginFExt->fPtr)
499 {
500 ((FExt_NE)_pluginFExt->fPtr)(time, &(*fExt)(0), _qDim, &(*_q0)(0)); // parameter z are assumed to be equal to q0
501 }
502 }
503 }
504
505 /** This function has been added to avoid Swig director to wrap _MExt into numpy.array
506 * when we call NewtonEulerDS::computeMExt(double time, SP::SiconosVector q, SP::SiconosVector mExt)
507 * that calls in turn computeMExt(time, q, _mExt);
508 */
509 static
computeMExt_internal(double time,bool hasConstantMExt,unsigned int qDim,SP::SiconosVector q0,SP::PluggedObject pluginMExt,SP::SiconosVector mExt_attributes,SP::SiconosVector mExt)510 void computeMExt_internal(double time, bool hasConstantMExt,
511 unsigned int qDim, SP::SiconosVector q0,
512 SP::PluggedObject pluginMExt, SP::SiconosVector mExt_attributes,
513 SP::SiconosVector mExt)
514 {
515 /* if the pointer has been set to an external vector
516 * after setting the plugin, we do not call the plugin */
517 if(hasConstantMExt)
518 {
519 if(mExt != mExt_attributes)
520 *mExt = *mExt_attributes;
521 }
522 else if(pluginMExt->fPtr)
523 ((FExt_NE)pluginMExt->fPtr)(time, &(*mExt)(0), qDim, &(*q0)(0)); // parameter z are assumed to be equal to q0
524
525 }
526
527
computeMExt(double time)528 void NewtonEulerDS::computeMExt(double time)
529 {
530 DEBUG_BEGIN("N3ewtonEulerDS::computeMExt(double time)\n");
531 computeMExt_internal(time,_hasConstantMExt,
532 _qDim, _q0,
533 _pluginMExt, _mExt, _mExt);
534 DEBUG_END("NewtonEulerDS::computeMExt(double time)\n");
535 }
536
537
538
computeMExt(double time,SP::SiconosVector mExt)539 void NewtonEulerDS::computeMExt(double time, SP::SiconosVector mExt)
540 {
541 DEBUG_BEGIN("NewtonEulerDS::computeMExt(double time, SP::SiconosVector mExt)\n");
542 computeMExt_internal(time, _hasConstantMExt,
543 _qDim, _q0, _pluginMExt, _mExt, mExt);
544 DEBUG_END("NewtonEulerDS::computeMExt(double time, SP::SiconosVector mExt)\n");
545 }
546
547
548
computeJacobianMExtqExpressedInInertialFrameByFD(double time,SP::SiconosVector q)549 void NewtonEulerDS::computeJacobianMExtqExpressedInInertialFrameByFD(double time, SP::SiconosVector q)
550 {
551
552 DEBUG_BEGIN("NewtonEulerDS::computeJacobianMExtqExpressedInInertialFrameByFD(...)\n");
553
554 /* The computation of Jacobian of R^T mExt is somehow very rough since the pertubation
555 * that we apply to q that gives qeps does not provide a unit quaternion. The rotation
556 * is computed assuming that the quaternion is unit (see quaternionRotate(double q0, double
557 * q1, double q2, double q3, SP::SiconosVector v)).
558 */
559
560 SP::SiconosVector mExt(new SiconosVector(3));
561 computeMExt(time, mExt);
562 if(_isMextExpressedInInertialFrame)
563 ::changeFrameAbsToBody(q,mExt);
564 DEBUG_EXPR(q->display());
565 DEBUG_EXPR(mExt->display(););
566
567 double mExt0 = mExt->getValue(0);
568 double mExt1 = mExt->getValue(1);
569 double mExt2 = mExt->getValue(2);
570
571 SP::SiconosVector qeps(new SiconosVector(*q));
572 _jacobianMExtq->zero();
573 (*qeps)(3) += _epsilonFD;
574 for(int j =3; j < 7; j++)
575 {
576 computeMExt(time, mExt);
577 if(_isMextExpressedInInertialFrame)
578 ::changeFrameAbsToBody(qeps,mExt);
579 DEBUG_EXPR(mExt->display(););
580 _jacobianMExtq->setValue(0,j, (mExt->getValue(0) - mExt0)/_epsilonFD);
581 _jacobianMExtq->setValue(1,j, (mExt->getValue(1) - mExt1)/_epsilonFD);
582 _jacobianMExtq->setValue(2,j, (mExt->getValue(2) - mExt2)/_epsilonFD);
583 (*qeps)(j) -= _epsilonFD;
584 if(j<6)(*qeps)(j+1) += _epsilonFD;
585 }
586 DEBUG_EXPR(_jacobianMExtq->display(););
587 DEBUG_END("NewtonEulerDS::computeJacobianMExtqExpressedInInertialFrameByFD(...)\n");
588
589 }
590
computeJacobianMExtqExpressedInInertialFrame(double time,SP::SiconosVector q)591 void NewtonEulerDS::computeJacobianMExtqExpressedInInertialFrame(double time, SP::SiconosVector q)
592 {
593 DEBUG_BEGIN("NewtonEulerDS::computeJacobianMExtqExpressedInInertialFrame(...)\n");
594 bool isMextExpressedInInertialFrame_save = _isMextExpressedInInertialFrame;
595 _isMextExpressedInInertialFrame=false;
596 SP::SiconosVector mExt(new SiconosVector(3));
597 computeMExt(time, mExt);
598 if(_isMextExpressedInInertialFrame)
599 ::changeFrameAbsToBody(q,mExt);
600 DEBUG_EXPR(q->display());
601 DEBUG_EXPR(mExt->display());
602
603 _isMextExpressedInInertialFrame=isMextExpressedInInertialFrame_save;
604
605 double q0 = q->getValue(3);
606 double q1 = q->getValue(4);
607 double q2 = q->getValue(5);
608 double q3 = q->getValue(6);
609
610 computeJacobianConvectedVectorInBodyFrame(q0, q1, q2, q3, _jacobianMExtq, mExt);
611
612 DEBUG_EXPR(_jacobianMExtq->display());
613
614 // SP::SimpleMatrix jacobianMExtqtmp (new SimpleMatrix(*_jacobianMExtq));
615 // computeJacobianMExtqExpressedInInertialFrameByFD(time, q);
616
617 // std::cout << "################# " << (*jacobianMExtqtmp- *_jacobianMExtq).normInf() << std::endl;
618 // assert((*jacobianMExtqtmp- *_jacobianMExtq).normInf()< 1e-10);
619
620 // DEBUG_EXPR(_jacobianMExtq->display(););
621 DEBUG_END("NewtonEulerDS::computeJacobianMExtqExpressedInInertialFrame(...)\n");
622
623 }
computeFInt(double time,SP::SiconosVector q,SP::SiconosVector v)624 void NewtonEulerDS::computeFInt(double time, SP::SiconosVector q, SP::SiconosVector v)
625 {
626 computeFInt(time, q, v, _fInt);
627 }
628
computeFInt(double time,SP::SiconosVector q,SP::SiconosVector v,SP::SiconosVector fInt)629 void NewtonEulerDS::computeFInt(double time, SP::SiconosVector q, SP::SiconosVector v, SP::SiconosVector fInt)
630 {
631 if(_pluginFInt->fPtr)
632 ((FInt_NE)_pluginFInt->fPtr)(time, &(*q)(0), &(*v)(0), &(*fInt)(0), _qDim, &(*_q0)(0));// parameter z are assumed to be equal to q0
633 }
634
635
636
computeMInt(double time,SP::SiconosVector q,SP::SiconosVector v)637 void NewtonEulerDS::computeMInt(double time, SP::SiconosVector q, SP::SiconosVector v)
638 {
639 DEBUG_BEGIN("NewtonEulerDS::computeMInt(double time, SP::SiconosVector q, SP::SiconosVector v)\n");
640 computeMInt(time, q, v, _mInt);
641 DEBUG_END("NewtonEulerDS::computeMInt(double time, SP::SiconosVector q, SP::SiconosVector v)\n");
642 }
643
computeMInt(double time,SP::SiconosVector q,SP::SiconosVector v,SP::SiconosVector mInt)644 void NewtonEulerDS::computeMInt(double time, SP::SiconosVector q, SP::SiconosVector v, SP::SiconosVector mInt)
645 {
646 DEBUG_BEGIN("NewtonEulerDS::computeMInt(double time, SP::SiconosVector q, SP::SiconosVector v, SP::SiconosVector mInt)\n");
647 if(_pluginMInt->fPtr)
648 ((FInt_NE)_pluginMInt->fPtr)(time, &(*q)(0), &(*v)(0), &(*mInt)(0), _qDim, &(*_q0)(0));// parameter z are assumed to be equal to q0
649 DEBUG_END("NewtonEulerDS::computeMInt(double time, SP::SiconosVector q, SP::SiconosVector v, SP::SiconosVector mInt)\n");
650 }
651
652
computeJacobianFIntq(double time)653 void NewtonEulerDS::computeJacobianFIntq(double time)
654 {
655 computeJacobianFIntq(time, _q, _twist);
656 }
computeJacobianFIntv(double time)657 void NewtonEulerDS::computeJacobianFIntv(double time)
658 {
659 computeJacobianFIntv(time, _q, _twist);
660 }
661
computeJacobianFIntq(double time,SP::SiconosVector q,SP::SiconosVector twist)662 void NewtonEulerDS::computeJacobianFIntq(double time, SP::SiconosVector q, SP::SiconosVector twist)
663 {
664 DEBUG_PRINT("NewtonEulerDS::computeJacobianFIntq(...) starts");
665 if(_pluginJacqFInt->fPtr)
666 ((FInt_NE)_pluginJacqFInt->fPtr)(time, &(*q)(0), &(*twist)(0), &(*_jacobianFIntq)(0, 0), _qDim, &(*_q0)(0));
667 else if(_computeJacobianFIntqByFD)
668 computeJacobianFIntqByFD(time, q, twist);
669 DEBUG_EXPR(_jacobianFIntq->display(););
670 DEBUG_END("NewtonEulerDS::computeJacobianFIntq(...)");
671 }
672
computeJacobianFIntqByFD(double time,SP::SiconosVector q,SP::SiconosVector twist)673 void NewtonEulerDS::computeJacobianFIntqByFD(double time, SP::SiconosVector q, SP::SiconosVector twist)
674 {
675 DEBUG_BEGIN("NewtonEulerDS::computeJacobianFIntqByFD(...)\n");
676 SP::SiconosVector fInt(new SiconosVector(3));
677 computeFInt(time, q, twist, fInt);
678
679 double fInt0 = fInt->getValue(0);
680 double fInt1 = fInt->getValue(1);
681 double fInt2 = fInt->getValue(2);
682
683 SP::SiconosVector qeps(new SiconosVector(*q));
684 _jacobianFIntq->zero();
685 (*qeps)(0) += _epsilonFD;
686 for(int j =0; j < 7; j++)
687 {
688 computeFInt(time, qeps, twist, fInt);
689 _jacobianFIntq->setValue(0,j, (fInt->getValue(0) - fInt0)/_epsilonFD);
690 _jacobianFIntq->setValue(1,j, (fInt->getValue(1) - fInt1)/_epsilonFD);
691 _jacobianFIntq->setValue(2,j, (fInt->getValue(2) - fInt2)/_epsilonFD);
692 (*qeps)(j) -= _epsilonFD;
693 if(j<6)(*qeps)(j+1) += _epsilonFD;
694 }
695 DEBUG_END("NewtonEulerDS::computeJacobianFIntqByFD(...)\n");
696
697
698 }
699
computeJacobianFIntv(double time,SP::SiconosVector q,SP::SiconosVector twist)700 void NewtonEulerDS::computeJacobianFIntv(double time, SP::SiconosVector q, SP::SiconosVector twist)
701 {
702 if(_pluginJactwistFInt->fPtr)
703 ((FInt_NE)_pluginJactwistFInt->fPtr)(time, &(*q)(0), &(*twist)(0), &(*_jacobianFInttwist)(0, 0), _qDim, &(*_q0)(0));
704 else if(_computeJacobianFInttwistByFD)
705 computeJacobianFIntvByFD(time, q, twist);
706 }
707
computeJacobianFIntvByFD(double time,SP::SiconosVector q,SP::SiconosVector twist)708 void NewtonEulerDS::computeJacobianFIntvByFD(double time, SP::SiconosVector q, SP::SiconosVector twist)
709 {
710 DEBUG_BEGIN("NewtonEulerDS::computeJacobianFIntvByFD(...)\n");
711 SP::SiconosVector fInt(new SiconosVector(3));
712 computeFInt(time, q, twist, fInt);
713
714 double fInt0 = fInt->getValue(0);
715 double fInt1 = fInt->getValue(1);
716 double fInt2 = fInt->getValue(2);
717
718 SP::SiconosVector veps(new SiconosVector(*twist));
719 _jacobianFInttwist->zero();
720
721 (*veps)(0) += _epsilonFD;
722 for(int j =0; j < 6; j++)
723 {
724 computeFInt(time, q, veps, fInt);
725 _jacobianFInttwist->setValue(0,j, (fInt->getValue(0) - fInt0)/_epsilonFD);
726 _jacobianFInttwist->setValue(1,j, (fInt->getValue(1) - fInt1)/_epsilonFD);
727 _jacobianFInttwist->setValue(2,j, (fInt->getValue(2) - fInt2)/_epsilonFD);
728 (*veps)(j) -= _epsilonFD;
729 if(j<5)(*veps)(j+1) += _epsilonFD;
730 }
731
732 DEBUG_END("NewtonEulerDS::computeJacobianFIntvByFD(...)\n");
733
734
735 }
computeJacobianMGyrtwistByFD(double time,SP::SiconosVector q,SP::SiconosVector twist)736 void NewtonEulerDS::computeJacobianMGyrtwistByFD(double time, SP::SiconosVector q, SP::SiconosVector twist)
737 {
738 DEBUG_BEGIN("NewtonEulerDS::computeJacobianMGyrvByFD(...)\n");
739 SP::SiconosVector mGyr(new SiconosVector(3));
740 computeMGyr(twist, mGyr);
741
742 double mGyr0 = mGyr->getValue(0);
743 double mGyr1 = mGyr->getValue(1);
744 double mGyr2 = mGyr->getValue(2);
745
746 SP::SiconosVector veps(new SiconosVector(*twist));
747 _jacobianMGyrtwist->zero();
748
749
750 (*veps)(0) += _epsilonFD;
751 for(int j =0; j < 6; j++)
752 {
753 computeMGyr(veps, mGyr);
754 _jacobianMGyrtwist->setValue(3,j, (mGyr->getValue(0) - mGyr0)/_epsilonFD);
755 _jacobianMGyrtwist->setValue(4,j, (mGyr->getValue(1) - mGyr1)/_epsilonFD);
756 _jacobianMGyrtwist->setValue(5,j, (mGyr->getValue(2) - mGyr2)/_epsilonFD);
757 (*veps)(j) -= _epsilonFD;
758 if(j<5)(*veps)(j+1) += _epsilonFD;
759 }
760 DEBUG_EXPR(_jacobianMGyrtwist->display());
761 DEBUG_END("NewtonEulerDS::computeJacobianMGyrvByFD(...)\n");
762
763
764 }
computeJacobianMIntq(double time)765 void NewtonEulerDS::computeJacobianMIntq(double time)
766 {
767 computeJacobianMIntq(time, _q, _twist);
768 }
computeJacobianMIntv(double time)769 void NewtonEulerDS::computeJacobianMIntv(double time)
770 {
771 computeJacobianMIntv(time, _q, _twist);
772 }
773
computeJacobianMIntq(double time,SP::SiconosVector q,SP::SiconosVector twist)774 void NewtonEulerDS::computeJacobianMIntq(double time, SP::SiconosVector q, SP::SiconosVector twist)
775 {
776 DEBUG_PRINT("NewtonEulerDS::computeJacobianMIntq(...) starts");
777 if(_pluginJacqMInt->fPtr)
778 ((FInt_NE)_pluginJacqMInt->fPtr)(time, &(*q)(0), &(*twist)(0), &(*_jacobianMIntq)(0, 0), _qDim, &(*_q0)(0));
779 else if(_computeJacobianMIntqByFD)
780 computeJacobianMIntqByFD(time, q, twist);
781 DEBUG_EXPR(_jacobianMIntq->display());
782 DEBUG_PRINT("NewtonEulerDS::computeJacobianMIntq(...) ends");
783
784 }
785
computeJacobianMIntqByFD(double time,SP::SiconosVector q,SP::SiconosVector twist)786 void NewtonEulerDS::computeJacobianMIntqByFD(double time, SP::SiconosVector q, SP::SiconosVector twist)
787 {
788 DEBUG_PRINT("NewtonEulerDS::computeJacobianMIntqByFD(...) starts\n");
789
790 SP::SiconosVector mInt(new SiconosVector(3));
791 computeMInt(time, q, twist, mInt);
792 double mInt0 = mInt->getValue(0);
793 double mInt1 = mInt->getValue(1);
794 double mInt2 = mInt->getValue(2);
795
796 SP::SiconosVector qeps(new SiconosVector(*q));
797
798 (*qeps)(0) += _epsilonFD;
799 for(int j =0; j < 7; j++)
800 {
801 computeMInt(time, qeps, twist, mInt);
802 _jacobianMIntq->setValue(0,j, (mInt->getValue(0) - mInt0)/_epsilonFD);
803 _jacobianMIntq->setValue(1,j, (mInt->getValue(1) - mInt1)/_epsilonFD);
804 _jacobianMIntq->setValue(2,j, (mInt->getValue(2) - mInt2)/_epsilonFD);
805 (*qeps)(j) -= _epsilonFD;
806 if(j<6)(*qeps)(j+1) += _epsilonFD;
807 }
808 DEBUG_PRINT("NewtonEulerDS::computeJacobianMIntqByFD(...) ends\n");
809 }
810
computeJacobianMIntv(double time,SP::SiconosVector q,SP::SiconosVector twist)811 void NewtonEulerDS::computeJacobianMIntv(double time, SP::SiconosVector q, SP::SiconosVector twist)
812 {
813 if(_pluginJactwistMInt->fPtr)
814 ((FInt_NE)_pluginJactwistMInt->fPtr)(time, &(*q)(0), &(*twist)(0), &(*_jacobianMInttwist)(0, 0), _qDim, &(*_q0)(0));
815 else if(_computeJacobianMInttwistByFD)
816 computeJacobianMIntvByFD(time, q, twist);
817 }
818
computeJacobianMIntvByFD(double time,SP::SiconosVector q,SP::SiconosVector twist)819 void NewtonEulerDS::computeJacobianMIntvByFD(double time, SP::SiconosVector q, SP::SiconosVector twist)
820 {
821 DEBUG_PRINT("NewtonEulerDS::computeJacobianMIntvByFD(...) starts\n");
822
823 SP::SiconosVector mInt(new SiconosVector(3));
824 computeMInt(time, q, twist, mInt);
825 double mInt0 = mInt->getValue(0);
826 double mInt1 = mInt->getValue(1);
827 double mInt2 = mInt->getValue(2);
828
829 SP::SiconosVector veps(new SiconosVector(*twist));
830
831 (*veps)(0) += _epsilonFD;
832 for(int j =0; j < 6; j++)
833 {
834 computeMInt(time, q, veps, mInt);
835 _jacobianMInttwist->setValue(0,j, (mInt->getValue(0) - mInt0)/_epsilonFD);
836 _jacobianMInttwist->setValue(1,j, (mInt->getValue(1) - mInt1)/_epsilonFD);
837 _jacobianMInttwist->setValue(2,j, (mInt->getValue(2) - mInt2)/_epsilonFD);
838 (*veps)(j) -= _epsilonFD;
839 if(j<5)(*veps)(j+1) += _epsilonFD;
840 }
841 DEBUG_PRINT("NewtonEulerDS::computeJacobianMIntvByFD(...) ends\n");
842 }
843
844
computeRhs(double time)845 void NewtonEulerDS::computeRhs(double time)
846 {
847 DEBUG_BEGIN("NewtonEulerDS::computeRhs(double time)");
848 *_acceleration = *(_p[2]); // Warning: r/p update is done in Interactions/Relations
849
850 computeForces(time, _q, _twist);
851 *_acceleration += *_wrench;
852 DEBUG_EXPR(_wrench->display(););
853
854 if(_inverseMass)
855 _inverseMass->Solve(*_acceleration);
856
857
858 // Compute _dotq
859 computeT();
860 prod(*_T, *_twist, *_dotq, true);
861
862 _x[1]->setBlock(0, *_dotq);
863 _x[1]->setBlock(_qDim, *_acceleration);
864
865 }
866
computeJacobianRhsx(double time)867 void NewtonEulerDS::computeJacobianRhsx(double time)
868 {
869 if(_jacobianWrenchq)
870 {
871 SP::SiconosMatrix bloc10 = _jacxRhs->block(1, 0);
872 computeJacobianqForces(time);
873 *bloc10 = *_jacobianWrenchq;
874 _inverseMass->Solve(*bloc10);
875 }
876 if(_jacobianWrenchTwist)
877 {
878 SP::SiconosMatrix bloc11 = _jacxRhs->block(1, 1);
879 computeJacobianvForces(time);
880 *bloc11 = *_jacobianWrenchTwist;
881 _inverseMass->Solve(*bloc11);
882 }
883
884 }
885
computeForces(double time)886 void NewtonEulerDS::computeForces(double time)
887 {
888 computeForces(time, _q, _twist);
889 }
890
891
892 /** This function has been added to avoid Swig director to wrap _mGyr into numpy.array
893 * when we call NewtonEulerDS::computeMGyr(SP::SiconosVector twist) that calls in turn
894 * computeMGyr(twist, _mGyr)
895 */
896 static
computeMGyr_internal(SP::SiconosMatrix I,SP::SiconosVector twist,SP::SiconosVector mGyr)897 void computeMGyr_internal(SP::SiconosMatrix I,SP::SiconosVector twist, SP::SiconosVector mGyr)
898 {
899 if(I)
900 {
901 DEBUG_EXPR(I->display());
902 DEBUG_EXPR(twist->display());
903 SiconosVector omega(3);
904 SiconosVector iomega(3);
905 omega.setValue(0, twist->getValue(3));
906 omega.setValue(1, twist->getValue(4));
907 omega.setValue(2, twist->getValue(5));
908 prod(*I, omega, iomega, true);
909 cross_product(omega, iomega, *mGyr);
910 }
911 }
computeMGyr(SP::SiconosVector twist,SP::SiconosVector mGyr)912 void NewtonEulerDS::computeMGyr(SP::SiconosVector twist, SP::SiconosVector mGyr)
913 {
914 // computation of \Omega times I \Omega (MGyr is in the l.h.s of the equation of motion)
915 DEBUG_BEGIN("NewtonEulerDS::computeMGyr(SP::SiconosVector twist, SP::SiconosVector mGyr)\n");
916
917 ::computeMGyr_internal(_I, twist, mGyr);
918
919 DEBUG_END("NewtonEulerDS::computeMGyr(SP::SiconosVector twist, SP::SiconosVector mGyr)\n");
920
921 }
computeMGyr(SP::SiconosVector twist)922 void NewtonEulerDS::computeMGyr(SP::SiconosVector twist)
923 {
924 /*computation of \Omega times I \Omega*/
925 //DEBUG_BEGIN("NewtonEulerDS::computeMGyr(SP::SiconosVector twist)\n");
926 ::computeMGyr_internal(_I, twist, _mGyr);
927 //DEBUG_END("NewtonEulerDS::computeMGyr(SP::SiconosVector twist)\n");
928
929 }
930
931
computeForces(double time,SP::SiconosVector q,SP::SiconosVector twist)932 void NewtonEulerDS::computeForces(double time, SP::SiconosVector q, SP::SiconosVector twist)
933 {
934 DEBUG_BEGIN("NewtonEulerDS::computeForces(double time, SP::SiconosVector q, SP::SiconosVector twist)\n")
935
936 if(_wrench)
937 {
938 _wrench->zero();
939
940 // External wrench
941
942 if(_fExt)
943 {
944 computeFExt(time);
945 assert(!std::isnan(_fExt->vector_sum()));
946 _wrench->setBlock(0, *_fExt);
947 }
948 if(_mExt)
949 {
950 computeMExt(time);
951 assert(!std::isnan(_mExt->vector_sum()));
952 if(_isMextExpressedInInertialFrame)
953 {
954 SP::SiconosVector mExt(std::make_shared<SiconosVector>(*_mExt));
955 ::changeFrameAbsToBody(q,mExt);
956 _wrench->setBlock(3, *mExt);
957 }
958 else
959 _wrench->setBlock(3, *_mExt);
960 }
961
962 // Internal wrench
963
964 if(_fInt)
965 {
966 computeFInt(time, q, twist);
967 assert(!std::isnan(_fInt->vector_sum()));
968 _wrench->setValue(0, _wrench->getValue(0) - _fInt->getValue(0));
969 _wrench->setValue(1, _wrench->getValue(1) - _fInt->getValue(1));
970 _wrench->setValue(2, _wrench->getValue(2) - _fInt->getValue(2));
971
972 }
973
974 if(_mInt)
975 {
976 computeMInt(time, q, twist);
977 assert(!std::isnan(_mInt->vector_sum()));
978 _wrench->setValue(3, _wrench->getValue(3) - _mInt->getValue(0));
979 _wrench->setValue(4, _wrench->getValue(4) - _mInt->getValue(1));
980 _wrench->setValue(5, _wrench->getValue(5) - _mInt->getValue(2));
981 }
982
983 // Gyroscopical effect
984 if(!_nullifyMGyr)
985 {
986 computeMGyr(twist);
987 assert(!std::isnan(_mGyr->vector_sum()));
988 _wrench->setValue(3, _wrench->getValue(3) - _mGyr->getValue(0));
989 _wrench->setValue(4, _wrench->getValue(4) - _mGyr->getValue(1));
990 _wrench->setValue(5, _wrench->getValue(5) - _mGyr->getValue(2));
991 }
992 DEBUG_EXPR(_wrench->display());
993 DEBUG_END("NewtonEulerDS::computeForces(double time, SP::SiconosVector q, SP::SiconosVector twist)\n")
994
995 }
996 else
997 {
998 THROW_EXCEPTION("NewtonEulerDS::computeForces _wrench is null");
999 }
1000 // else nothing.
1001 }
1002
computeJacobianqForces(double time)1003 void NewtonEulerDS::computeJacobianqForces(double time)
1004 {
1005 DEBUG_BEGIN("NewtonEulerDS::computeJacobianqWrench(double time) \n");
1006 if(_jacobianWrenchq)
1007 {
1008 _jacobianWrenchq->zero();
1009 if(_jacobianFIntq)
1010 {
1011 computeJacobianFIntq(time);
1012 _jacobianWrenchq->setBlock(0,0,-1.0 * *_jacobianFIntq);
1013 }
1014 if(_jacobianMIntq)
1015 {
1016 computeJacobianMIntq(time);
1017 }
1018 if(_isMextExpressedInInertialFrame && _mExt)
1019 {
1020 computeJacobianMExtqExpressedInInertialFrame(time, _q);
1021 _jacobianWrenchq->setBlock(3,0,1.0* *_jacobianMExtq);
1022 }
1023 DEBUG_EXPR(_jacobianWrenchq->display(););
1024 }
1025 else
1026 {
1027 THROW_EXCEPTION("NewtonEulerDS::computeJacobianqForces _jacobianWrenchq is null");
1028 }
1029 //else nothing.
1030 DEBUG_END("NewtonEulerDS::computeJacobianqForces(double time) \n");
1031 }
1032
computeJacobianvForces(double time)1033 void NewtonEulerDS::computeJacobianvForces(double time)
1034 {
1035 DEBUG_BEGIN("NewtonEulerDS::computeJacobiantwistForces(double time) \n");
1036 if(_jacobianWrenchTwist)
1037 {
1038 _jacobianWrenchTwist->zero();
1039 if(_jacobianFInttwist)
1040 {
1041 computeJacobianFIntv(time);
1042 _jacobianWrenchTwist->setBlock(0,0,-1.0 * *_jacobianFInttwist);
1043 }
1044 if(_jacobianMInttwist)
1045 {
1046 computeJacobianMIntv(time);
1047 _jacobianWrenchTwist->setBlock(3,0,-1.0 * *_jacobianMInttwist);
1048 }
1049 if(!_nullifyMGyr)
1050 {
1051 if(_jacobianMGyrtwist)
1052 {
1053 //computeJacobianMGyrtwistByFD(time,_q,_twist);
1054 computeJacobianMGyrtwist(time);
1055 _jacobianWrenchTwist->setBlock(3,0,-1.0 * *_jacobianMGyrtwist);
1056 }
1057 }
1058 }
1059 //else nothing.
1060 DEBUG_END("NewtonEulerDS::computeJacobiantwistForces(double time) \n");
1061 }
1062
computeJacobianMGyrtwist(double time)1063 void NewtonEulerDS::computeJacobianMGyrtwist(double time)
1064 {
1065 DEBUG_BEGIN("NewtonEulerDS::computeJacobianMGyrtwist(double time) \n");
1066 if(_jacobianMGyrtwist)
1067 {
1068 //Omega /\ I \Omega:
1069 _jacobianMGyrtwist->zero();
1070 SiconosVector omega(3);
1071 omega.setValue(0, _twist->getValue(3));
1072 omega.setValue(1, _twist->getValue(4));
1073 omega.setValue(2, _twist->getValue(5));
1074 SiconosVector Iomega(3);
1075 prod(*_I, omega, Iomega, true);
1076 SiconosVector ei(3);
1077 SiconosVector Iei(3);
1078 SiconosVector ei_Iomega(3);
1079 SiconosVector omega_Iei(3);
1080
1081 /*See equation of DevNotes.pdf, equation with label eq:NE_nablaFL1*/
1082 for(int i = 0; i < 3; i++)
1083 {
1084 ei.zero();
1085 ei.setValue(i, 1.0);
1086 prod(*_I, ei, Iei, true);
1087 cross_product(omega, Iei, omega_Iei);
1088 cross_product(ei, Iomega, ei_Iomega);
1089 for(int j = 0; j < 3; j++)
1090 _jacobianMGyrtwist->setValue(j, 3 + i, ei_Iomega.getValue(j) + omega_Iei.getValue(j));
1091 }
1092 // Check if Jacobian is valid. Warning to the transpose operation in
1093 // _jacobianMGyrtwist->setValue(3 + j, 3 + i, ei_Iomega.getValue(j) + omega_Iei.getValue(j));
1094 }
1095 //else nothing.
1096 DEBUG_EXPR(_jacobianMGyrtwist->display());
1097 // _jacobianMGyrtwist->display();
1098 // SP::SimpleMatrix jacobianMGyrtmp (new SimpleMatrix(*_jacobianMGyrtwist));
1099 // computeJacobianMGyrtwistByFD(time, _q, _twist);
1100 // jacobianMGyrtmp->display();
1101 // std::cout << "################# " << (*jacobianMGyrtmp - *_jacobianMGyrtwist).normInf() << std::endl;
1102 // assert((*jacobianMGyrtmp - *_jacobianMGyrtwist).normInf()< 1e-10);
1103 DEBUG_END("NewtonEulerDS::computeJacobianMGyrtwist(double time) \n");
1104 }
1105
1106
display(bool brief) const1107 void NewtonEulerDS::display(bool brief) const
1108 {
1109 std::cout << "=====> NewtonEuler System display (number: " << _number << ")." <<std::endl;
1110 std::cout << "- _ndof : " << _ndof <<std::endl;
1111 std::cout << "- _qDim : " << _qDim <<std::endl;
1112 std::cout << "- _n : " << _n <<std::endl;
1113 std::cout << "- q " <<std::endl;
1114 if(_q) _q->display();
1115 else std::cout << "-> nullptr" <<std::endl;
1116 std::cout << "- q0 " <<std::endl;
1117 if(_q0) _q0->display();
1118 std::cout << "- twist " <<std::endl;
1119 if(_twist) _twist->display();
1120 else std::cout << "-> nullptr" <<std::endl;
1121 std::cout << "- twist0 " <<std::endl;
1122 if(_twist0) _twist0->display();
1123 else std::cout << "-> nullptr" <<std::endl;
1124 std::cout << "- dotq " <<std::endl;
1125 if(_dotq) _dotq->display();
1126 else std::cout << "-> nullptr" <<std::endl;
1127 std::cout << "- p[0] " <<std::endl;
1128 if(_p[0]) _p[0]->display();
1129 else std::cout << "-> nullptr" <<std::endl;
1130 std::cout << "- p[1] " <<std::endl;
1131 if(_p[1]) _p[1]->display();
1132 else std::cout << "-> nullptr" <<std::endl;
1133 std::cout << "- p[2] " <<std::endl;
1134 if(_p[2]) _p[2]->display();
1135 else std::cout << "-> nullptr" <<std::endl;
1136 std::cout << "mass :" << _scalarMass <<std::endl;
1137 std::cout << "Inertia :" <<std::endl;
1138 if(_I) _I->display();
1139 else std::cout << "-> nullptr" <<std::endl;
1140 std::cout << "===================================== " <<std::endl;
1141 }
1142
1143 // --- Functions for memory handling ---
initMemory(unsigned int steps)1144 void NewtonEulerDS::initMemory(unsigned int steps)
1145 {
1146 DynamicalSystem::initMemory(steps);
1147
1148 if(steps == 0)
1149 std::cout << "Warning : NewtonEulerDS::initMemory with size equal to zero" <<std::endl;
1150 else
1151 {
1152 _qMemory.setMemorySize(steps, _qDim);
1153 _twistMemory.setMemorySize(steps, _ndof);
1154 _forcesMemory.setMemorySize(steps, _ndof);
1155 _dotqMemory.setMemorySize(steps, _qDim);
1156 // swapInMemory(); Useless, done in osi->initializeWorkVectorsForDS
1157 }
1158 }
1159
swapInMemory()1160 void NewtonEulerDS::swapInMemory()
1161 {
1162 // _xMemory->swap(_x[0]);
1163 _qMemory.swap(*_q);
1164 _twistMemory.swap(*_twist);
1165 _dotqMemory.swap(*_dotq);
1166 _forcesMemory.swap(*_wrench);
1167 }
1168
resetAllNonSmoothParts()1169 void NewtonEulerDS::resetAllNonSmoothParts()
1170 {
1171 if(_p[1])
1172 _p[1]->zero();
1173 else
1174 _p[1].reset(new SiconosVector(_ndof));
1175 }
resetNonSmoothPart(unsigned int level)1176 void NewtonEulerDS::resetNonSmoothPart(unsigned int level)
1177 {
1178 if(_p[level])
1179 _p[level]->zero();
1180 }
1181
1182
computeT()1183 void NewtonEulerDS::computeT()
1184 {
1185 ::computeT(_q,_T);
1186 }
1187
1188
1189
computeTdot()1190 void NewtonEulerDS::computeTdot()
1191 {
1192 if(!_Tdot)
1193 {
1194 _Tdot.reset(new SimpleMatrix(_qDim, _ndof));
1195 _Tdot->zero();
1196 }
1197
1198 ::computeT(_dotq,_Tdot);
1199 }
1200
1201
normalizeq()1202 void NewtonEulerDS::normalizeq()
1203 {
1204 ::normalizeq(_q);
1205 }
1206
setComputeJacobianFIntqFunction(const std::string & pluginPath,const std::string & functionName)1207 void NewtonEulerDS::setComputeJacobianFIntqFunction(const std::string& pluginPath, const std::string& functionName)
1208 {
1209 // Plugin::setFunction(&computeJacobianFIntqPtr, pluginPath,functionName);
1210 _pluginJacqFInt->setComputeFunction(pluginPath, functionName);
1211 if(!_jacobianFIntq)
1212 _jacobianFIntq.reset(new SimpleMatrix(3, _qDim));
1213 if(!_jacobianWrenchq)
1214 _jacobianWrenchq.reset(new SimpleMatrix(_ndof, _qDim));
1215 _computeJacobianFIntqByFD = false;
1216 }
setComputeJacobianFIntvFunction(const std::string & pluginPath,const std::string & functionName)1217 void NewtonEulerDS::setComputeJacobianFIntvFunction(const std::string& pluginPath, const std::string& functionName)
1218 {
1219 // Plugin::setFunction(&computeJacobianFIntvPtr, pluginPath,functionName);
1220 _pluginJactwistFInt->setComputeFunction(pluginPath, functionName);
1221 if(!_jacobianFInttwist)
1222 _jacobianFInttwist.reset(new SimpleMatrix(3, _ndof));
1223 _computeJacobianFInttwistByFD = false;
1224 }
setComputeJacobianFIntqFunction(FInt_NE fct)1225 void NewtonEulerDS::setComputeJacobianFIntqFunction(FInt_NE fct)
1226 {
1227 _pluginJacqFInt->setComputeFunction((void *)fct);
1228 if(!_jacobianFIntq)
1229 _jacobianFIntq.reset(new SimpleMatrix(3, _qDim));
1230 if(!_jacobianWrenchq)
1231 _jacobianWrenchq.reset(new SimpleMatrix(_ndof, _qDim));
1232 _computeJacobianFIntqByFD = false;
1233 }
setComputeJacobianFIntvFunction(FInt_NE fct)1234 void NewtonEulerDS::setComputeJacobianFIntvFunction(FInt_NE fct)
1235 {
1236 _pluginJactwistFInt->setComputeFunction((void *)fct);
1237 if(!_jacobianFInttwist)
1238 _jacobianFInttwist.reset(new SimpleMatrix(3, _ndof));
1239 if(!_jacobianWrenchTwist)
1240 _jacobianWrenchTwist.reset(new SimpleMatrix(_ndof, _ndof));
1241 _computeJacobianFInttwistByFD = false;
1242 }
1243
setComputeJacobianMIntqFunction(const std::string & pluginPath,const std::string & functionName)1244 void NewtonEulerDS::setComputeJacobianMIntqFunction(const std::string& pluginPath, const std::string& functionName)
1245 {
1246 _pluginJacqMInt->setComputeFunction(pluginPath, functionName);
1247 if(!_jacobianMIntq)
1248 _jacobianMIntq.reset(new SimpleMatrix(3, _qDim));
1249 if(!_jacobianWrenchq)
1250 _jacobianWrenchq.reset(new SimpleMatrix(_ndof, _qDim));
1251 _computeJacobianMIntqByFD=false;
1252 }
setComputeJacobianMIntvFunction(const std::string & pluginPath,const std::string & functionName)1253 void NewtonEulerDS::setComputeJacobianMIntvFunction(const std::string& pluginPath, const std::string& functionName)
1254 {
1255 _pluginJactwistMInt->setComputeFunction(pluginPath, functionName);
1256 if(!_jacobianMInttwist)
1257 _jacobianMInttwist.reset(new SimpleMatrix(3, _ndof));
1258 if(!_jacobianWrenchTwist)
1259 _jacobianWrenchTwist.reset(new SimpleMatrix(_ndof, _ndof));
1260 _computeJacobianMInttwistByFD=false;
1261 }
setComputeJacobianMIntqFunction(FInt_NE fct)1262 void NewtonEulerDS::setComputeJacobianMIntqFunction(FInt_NE fct)
1263 {
1264 _pluginJacqMInt->setComputeFunction((void *)fct);
1265 if(!_jacobianMIntq)
1266 _jacobianMIntq.reset(new SimpleMatrix(3, _qDim));
1267 if(!_jacobianWrenchq)
1268 _jacobianWrenchq.reset(new SimpleMatrix(_ndof, _qDim));
1269 _computeJacobianMIntqByFD = false;
1270 }
setComputeJacobianMIntvFunction(FInt_NE fct)1271 void NewtonEulerDS::setComputeJacobianMIntvFunction(FInt_NE fct)
1272 {
1273 _pluginJactwistMInt->setComputeFunction((void *)fct);
1274 if(!_jacobianMInttwist)
1275 _jacobianMInttwist.reset(new SimpleMatrix(3, _ndof));
1276 if(!_jacobianWrenchTwist)
1277 _jacobianWrenchTwist.reset(new SimpleMatrix(_ndof, _ndof));
1278 _computeJacobianMInttwistByFD=false;
1279 }
1280
1281
computeKineticEnergy()1282 double NewtonEulerDS::computeKineticEnergy()
1283 {
1284 DEBUG_BEGIN("NewtonEulerDS::computeKineticEnergy()\n");
1285 assert(_twist);
1286 assert(_mass);
1287 DEBUG_EXPR(_twist->display());
1288 DEBUG_EXPR(_mass->display());
1289
1290 SiconosVector tmp(6);
1291 prod(*_mass, *_twist, tmp, true);
1292 double K =0.5*inner_prod(tmp,*_twist);
1293
1294 DEBUG_PRINTF("Kinetic Energy = %e\n", K);
1295 DEBUG_END("NewtonEulerDS::computeKineticEnergy()\n");
1296 return K;
1297 }
1298
linearVelocity(bool absoluteRef) const1299 SP::SiconosVector NewtonEulerDS::linearVelocity(bool absoluteRef) const
1300 {
1301 // Short-cut: return the _twist 6-vector without modification, first
1302 // 3 components are the expected linear velocity.
1303 if(absoluteRef)
1304 return _twist;
1305
1306 SP::SiconosVector v(std::make_shared<SiconosVector>(3));
1307 linearVelocity(absoluteRef, *v);
1308 return v;
1309 }
1310
linearVelocity(bool absoluteRef,SiconosVector & v) const1311 void NewtonEulerDS::linearVelocity(bool absoluteRef, SiconosVector &v) const
1312 {
1313 v(0) = (*_twist)(0);
1314 v(1) = (*_twist)(1);
1315 v(2) = (*_twist)(2);
1316
1317 /* See _twist: linear velocity is in absolute frame */
1318 if(!absoluteRef)
1319 changeFrameAbsToBody(*_q, v);
1320 }
1321
angularVelocity(bool absoluteRef) const1322 SP::SiconosVector NewtonEulerDS::angularVelocity(bool absoluteRef) const
1323 {
1324 SP::SiconosVector w(std::make_shared<SiconosVector>(3));
1325 angularVelocity(absoluteRef, *w);
1326 return w;
1327 }
1328
angularVelocity(bool absoluteRef,SiconosVector & w) const1329 void NewtonEulerDS::angularVelocity(bool absoluteRef, SiconosVector &w) const
1330 {
1331 w(0) = (*_twist)(3);
1332 w(1) = (*_twist)(4);
1333 w(2) = (*_twist)(5);
1334
1335 /* See _twist: angular velocity is in relative frame */
1336 if(absoluteRef)
1337 changeFrameBodyToAbs(*_q, w);
1338 }
1339
computeExtForceAtPos(SP::SiconosVector q,bool isMextExpressedInInertialFrame,SP::SiconosVector force,bool forceAbsRef,SP::SiconosVector pos,bool posAbsRef,SP::SiconosVector fExt,SP::SiconosVector mExt,bool accumulate)1340 void computeExtForceAtPos(SP::SiconosVector q, bool isMextExpressedInInertialFrame,
1341 SP::SiconosVector force, bool forceAbsRef,
1342 SP::SiconosVector pos, bool posAbsRef,
1343 SP::SiconosVector fExt, SP::SiconosVector mExt,
1344 bool accumulate)
1345 {
1346 assert(!!fExt && fExt->size() == 3);
1347 assert(!!force && force->size() == 3);
1348 if(pos)
1349 assert(!!mExt && mExt->size() == 3);
1350
1351 SiconosVector abs_frc(*force), local_frc(*force);
1352
1353 if(forceAbsRef)
1354 {
1355 if(pos)
1356 changeFrameAbsToBody(*q, local_frc);
1357 }
1358 else
1359 changeFrameBodyToAbs(*q, abs_frc);
1360
1361 if(pos)
1362 {
1363 assert(!!mExt && mExt->size() >= 3);
1364 SiconosVector moment(3);
1365 if(posAbsRef)
1366 {
1367 SiconosVector local_pos(*pos);
1368 local_pos(0) -= (*q)(0);
1369 local_pos(1) -= (*q)(1);
1370 local_pos(2) -= (*q)(2);
1371 changeFrameAbsToBody(*q, local_pos);
1372 cross_product(local_pos, local_frc, moment);
1373 }
1374 else
1375 {
1376 cross_product(*pos, local_frc, moment);
1377 }
1378
1379 if(isMextExpressedInInertialFrame)
1380 changeFrameBodyToAbs(*q, moment);
1381
1382 if(accumulate)
1383 *mExt = *mExt + moment;
1384 else
1385 *mExt = moment;
1386 }
1387
1388 if(accumulate)
1389 *fExt += *fExt + abs_frc;
1390 else
1391 *fExt = abs_frc;
1392 }
1393
addExtForceAtPos(SP::SiconosVector force,bool forceAbsRef,SP::SiconosVector pos,bool posAbsRef)1394 void NewtonEulerDS::addExtForceAtPos(SP::SiconosVector force, bool forceAbsRef,
1395 SP::SiconosVector pos, bool posAbsRef)
1396 {
1397 assert(!!_fExt && _fExt->size() == 3);
1398 assert(!!force && force->size() == 3);
1399 if(pos)
1400 assert(!!_mExt && _mExt->size() == 3);
1401
1402 computeExtForceAtPos(_q, _isMextExpressedInInertialFrame,
1403 force, forceAbsRef,
1404 pos, posAbsRef,
1405 _fExt, _mExt, true);
1406 }
1407