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 19 /*! \file SecondOrder.hpp 20 SecondOrderDS class - Second Order Non Linear Dynamical Systems. 21 */ 22 23 #ifndef SECONDORDERDS_H 24 #define SECONDORDERDS_H 25 26 #include "DynamicalSystem.hpp" 27 #include "BoundaryCondition.hpp" 28 #include "SiconosConst.hpp" 29 30 /** Second Order non linear dynamical systems - \f$M(q,z) \dot v = F(v, q, t, z) + p \f$ 31 32 This class defines and computes a generic ndof-dimensional 33 second order Non Linear Dynamical System of the form : 34 35 \rst 36 .. math:: 37 38 M(q,z) \\dot v = F(v, q, t, z) + p \\ 39 \\dot q = G(q,v) 40 41 \endrst 42 43 where 44 45 - \f$q \in R^{ndof} \f$ is the set of the coordinates, 46 - \f$ \\dot q =v \in R^{ndof} \f$ the velocity, 47 - \f$ \ddot q =\\dot v \in R^{ndof} \f$ the acceleration, i. e. the second 48 time derivative of the generalized coordinates. 49 - \f$ p \in R^{ndof} \f$ the reaction forces due to the Non Smooth 50 Interaction. 51 - \f$ M(q) \in R^{ndof \times ndof} \f$ is the inertia term (access : mass() method). 52 - \f$ F(\\dot q , q , t) \in R^{ndof} \f$ are the forces (access forces() method). 53 - \f$ z \in R^{zSize}\f$ is a vector of arbitrary algebraic variables, some sort of discrete state. 54 55 q[i] is the derivative number i of q. 56 Thus: q[0]=\f$ q \f$, global coordinates, q[1]=\f$ \\dot q\f$, velocity, q[2]=\f$ \ddot q \f$, acceleration. 57 58 The following operators (and their jacobians) can be plugged, in the usual way (see User Guide, 'User-defined plugins') 59 60 - \f$M(q)\f$ (computeMass()) 61 - \f$F(v , q , t, z)\f$ (computeF()) 62 3 63 If required (e.g. for Event-Driven like simulation), formulation as a first-order system is also available, and writes: 64 65 - \f$ n= 2 ndof \f$ 66 - \f$ x = \left[\begin{array}{c}q \\ \\dot q\end{array}\right]\f$ 67 - rhs given by: 68 69 \rst 70 71 .. math:: 72 73 \\dot x = \left[\begin{array}{c} 74 \\dot q\\ 75 \ddot q = M^{-1}(q)\left[F(v, q , t, z) + p \right]\\ 76 \end{array}\right] 77 78 \endrst 79 80 - jacobian of the rhs, with respect to x 81 82 \rst 83 84 .. math:: 85 86 \nabla_{x}rhs(x,t) = \left[\begin{array}{cc} 87 0 & I \\ 88 \nabla_{q}(M^{-1}(q)F(v, q , t, z)) & \nabla_{\\dot q}(M^{-1}(q)F(v, q , t, z)) \\ 89 \end{array}\right] 90 91 \endrst 92 93 with the input due to the non smooth law: 94 95 \rst 96 97 .. math:: 98 99 \left[\begin{array}{c} 100 0 \\ 101 p \end{array}\right] 102 103 \endrst 104 105 In that case, use the following methods: 106 - initRhs() to allocate/initialize memory for these new operators, 107 - rhs() to get the rhs vector 108 - computeRhs(), computeJacobianRhsx() ..., to update the content of rhs, its jacobians ... 109 110 */ 111 class SecondOrderDS : public DynamicalSystem 112 { 113 114 protected: 115 /* serialization hooks */ 116 ACCEPT_SERIALIZATION(SecondOrderDS); 117 118 // -- MEMBERS -- 119 120 /** number of degrees of freedom of the system */ 121 unsigned int _ndof; 122 123 /** mass of the system */ 124 SP::SiconosMatrix _mass; 125 126 /** true if the mass matrix is constant */ 127 bool _hasConstantMass = false; 128 129 /** inverse or factorization of the mass of the system */ 130 SP::SimpleMatrix _inverseMass; 131 132 /** "Reaction", generalized forces or imuplses due to the non smooth law 133 * The index corresponds to the kinematic 134 * level of the corresponding constraints. It mainly depends on what the simulation 135 * part want to store, but some rules have to be followed. For instance : 136 * - for the constraints at the acceleration level, _p[2] stores the reaction forces, 137 * - for the constraints at the veocity level, _p[1] stores the (discrete) reaction impulse 138 * - for the constraints at the position level, _p[0] stores the multiplier for a constraint 139 * in position 140 */ 141 std::vector<SP::SiconosVector> _p; 142 143 /** Initial position */ 144 SP::SiconosVector _q0; 145 146 /** memory of previous generalized forces due to constraints */ 147 VectorOfMemories _pMemory; 148 149 /** Boundary condition applied to a dynamical system*/ 150 SP::BoundaryCondition _boundaryConditions; 151 152 /** Reaction to an applied boundary condition */ 153 SP::SiconosVector _reactionToBoundaryConditions; 154 155 // /** Default constructor */ SecondOrderDS()156 SecondOrderDS():DynamicalSystem(Type::SecondOrderDS) {}; 157 158 /** minimal constructor, from state dimension 159 result in \f$ \dot x = r \f$ 160 * \param dimension size of the system (n) 161 */ SecondOrderDS(unsigned int dimension,unsigned int ndof)162 SecondOrderDS(unsigned int dimension, unsigned int ndof):DynamicalSystem(dimension), 163 _ndof(ndof), _hasConstantMass(true) 164 {}; 165 166 public: 167 168 /** destructor */ ~SecondOrderDS()169 virtual ~SecondOrderDS() {}; 170 171 /** get p 172 * \param level unsigned int, required level for p, default = 2 173 * \return pointer on a SiconosVector 174 */ p(unsigned int level=2) const175 inline SP::SiconosVector p(unsigned int level = 2) const 176 { 177 return _p[level]; 178 } 179 180 /** get mass matrix (pointer link) 181 * \return SP::SiconosMatrix 182 */ mass() const183 inline SP::SiconosMatrix mass() const 184 { 185 return _mass; 186 } 187 188 /** get (pointer) inverse or LU-factorization of the mass, 189 * used for LU-forward-backward computation 190 * \return pointer SP::SimpleMatrix 191 */ inverseMass() const192 inline SP::SimpleMatrix inverseMass() const 193 { 194 return _inverseMass; 195 } 196 197 /** set mass to pointer newPtr 198 * \param newPtr a plugged matrix SP 199 */ setMassPtr(SP::SimpleMatrix newPtr)200 inline void setMassPtr(SP::SimpleMatrix newPtr) 201 { 202 _mass = newPtr; 203 _hasConstantMass = true; 204 } 205 206 207 /*! @name Right-hand side computation */ 208 //@{ 209 210 /** reset the state to the initial state */ 211 virtual void resetToInitialState()=0; 212 213 /** allocate (if needed) and compute rhs and its jacobian. 214 * \param time of initialization 215 */ 216 virtual void initRhs(double time)=0; 217 218 /** set nonsmooth input to zero 219 * \param level input-level to be initialized. 220 */ 221 virtual void initializeNonSmoothInput(unsigned int level) =0 ; 222 223 /** update right-hand side for the current state 224 * \param time of interest 225 */ 226 virtual void computeRhs(double time) =0; 227 228 /** update \f$\nabla_x rhs\f$ for the current state 229 * \param time of interest 230 */ 231 virtual void computeJacobianRhsx(double time) =0; 232 233 /** reset non-smooth part of the rhs (i.e. p), for all 'levels' */ 234 virtual void resetAllNonSmoothParts() =0; 235 236 /** set nonsmooth part of the rhs (i.e. p) to zero for a given level 237 * \param level 238 */ 239 virtual void resetNonSmoothPart(unsigned int level) =0; 240 241 /** set the value of the right-hand side, \f$ \dot x \f$ 242 * \param newValue SiconosVector 243 */ setRhs(const SiconosVector & newValue)244 virtual void setRhs(const SiconosVector& newValue) 245 { 246 THROW_EXCEPTION("SecondOrderDS - setRhs call is forbidden for 2nd order systems."); 247 } 248 249 /** set right-hand side, \f$ \dot x \f$ (pointer link) 250 * \param newPtr SP::SiconosVector 251 */ setRhsPtr(SP::SiconosVector newPtr)252 virtual void setRhsPtr(SP::SiconosVector newPtr) 253 { 254 THROW_EXCEPTION("SecondOrderDS - setRhsPtr call is forbidden for 2nd order systems."); 255 } 256 257 /** function to compute \f$F(v,q,t,z)\f$ for the current state 258 * \param time the current time 259 */ 260 //virtual void computeForces(double time); 261 262 /** Compute \f$F(v,q,t,z)\f$ 263 * \param time the current time 264 * \param q SP::SiconosVector: pointers on q 265 * \param velocity SP::SiconosVector: pointers on velocity 266 */ 267 virtual void computeForces(double time, 268 SP::SiconosVector q, 269 SP::SiconosVector velocity) =0; 270 271 /** Compute \f$\nabla_qF(v,q,t,z)\f$ for current \f$q,v\f$ 272 Default function to compute forces 273 * \param time the current time 274 */ 275 virtual void computeJacobianqForces(double time) =0; 276 277 /** Compute \f$\nabla_{\dot q}F(v,q,t,z)\f$ for current \f$q,v\f$ 278 * \param time the current time 279 */ 280 virtual void computeJacobianvForces(double time)=0; 281 282 ///@} 283 284 /*! @name Attributes access 285 @{ */ 286 287 /** return the number of degrees of freedom of the system 288 * \return an unsigned int. 289 */ dimension() const290 inline unsigned int dimension() const 291 { 292 return _ndof; 293 } 294 /** generalized coordinates of the system (vector of size dimension()) 295 * \return pointer on a SiconosVector 296 */ 297 virtual SP::SiconosVector q() const =0; 298 299 /** set value of generalized coordinates vector (copy) 300 * \param newValue 301 */ 302 virtual void setQ(const SiconosVector& newValue) =0; 303 304 /** set value of generalized coordinates vector (pointer link) 305 * \param newPtr 306 */ 307 virtual void setQPtr(SP::SiconosVector newPtr) =0; 308 309 /** get initial state (pointer link) 310 * \return pointer on a SiconosVector 311 */ q0() const312 SP::SiconosVector q0() const 313 { 314 return _q0; 315 } 316 317 318 /** set initial state (copy) 319 * \param newValue 320 */ 321 virtual void setQ0(const SiconosVector& newValue) =0; 322 323 /** set initial state (pointer link) 324 * \param newPtr 325 */ 326 virtual void setQ0Ptr(SP::SiconosVector newPtr) =0; 327 328 /** get velocity vector (pointer link) 329 * \return pointer on a SiconosVector 330 */ 331 virtual SP::SiconosVector velocity() const =0; 332 333 /** set velocity vector (copy) 334 * \param newValue 335 */ 336 virtual void setVelocity(const SiconosVector& newValue) =0; 337 338 /** set velocity vector (pointer link) 339 * \param newPtr 340 */ 341 virtual void setVelocityPtr(SP::SiconosVector newPtr) =0; 342 343 /** get initial velocity (pointer) 344 * \return pointer on a SiconosVector 345 */ 346 virtual SP::SiconosVector velocity0() const =0; 347 /** set initial velocity (copy) 348 * \param newValue 349 */ 350 virtual void setVelocity0(const SiconosVector& newValue) =0; 351 352 /** set initial velocity (pointer link) 353 * \param newPtr 354 */ 355 virtual void setVelocity0Ptr(SP::SiconosVector newPtr) =0; 356 357 /** get acceleration (pointer link) 358 * \return pointer on a SiconosVector 359 */ 360 virtual SP::SiconosVector acceleration() const =0; 361 362 /** get \f$ F(v,q,t,z)\f$ (pointer link) 363 * \return pointer on a SiconosVector 364 */ 365 virtual SP::SiconosVector forces() const =0; 366 367 /** get \f$ \nabla_qF(v,q,t,z)\f$ (pointer link) 368 * \return pointer on a SiconosMatrix 369 */ 370 virtual SP::SiconosMatrix jacobianqForces() const =0; 371 372 /** get \f$ \nabla_{\dot q}F(v,q,t,z)\f$ (pointer link) 373 * \return pointer on a SiconosMatrix 374 */ 375 virtual SP::SiconosMatrix jacobianvForces() const =0; 376 377 ///@} 378 379 /*! @name Memory vectors management 380 @{ */ 381 382 /** get all the values of the state vector q stored in memory. 383 * note: not const due to SchatzmanPaoliOSI::initializeWorkVectorsForDS 384 * \return a memory 385 */ 386 virtual const SiconosMemory& qMemory() =0; 387 388 /** get all the values of the state vector velocity stored in memory. 389 * note: not const due to SchatzmanPaoliOSI::initializeWorkVectorsForDS 390 * \return a memory 391 */ 392 virtual const SiconosMemory& velocityMemory() =0; 393 394 395 /** get forces in memory buff 396 * \return pointer on a SiconosMemory 397 */ 398 virtual const SiconosMemory& forcesMemory() =0; 399 400 /** initialize the SiconosMemory objects with a positive size. 401 * \param size the size of the SiconosMemory. must be >= 0 402 */ 403 virtual void initMemory(unsigned int size) =0; 404 405 /** push the current values of x, q and r in the stored previous values 406 * xMemory, qMemory, rMemory, 407 * \todo Modify the function swapIn Memory with the new Object Memory 408 */ 409 virtual void swapInMemory() =0; 410 411 ///@} 412 413 /** default function to compute the mass 414 */ 415 virtual void computeMass() =0; 416 417 /** function to compute the mass 418 * \param position value used to evaluate the mass matrix 419 */ 420 virtual void computeMass(SP::SiconosVector position) =0; 421 422 /**default function to update the plugins functions using a new time: 423 * \param time the current time 424 */ 425 virtual void updatePlugins(double time) =0; 426 427 ///@} 428 429 /** print the data of the dynamical system on the standard output 430 */ 431 virtual void display(bool brief = true) const =0; 432 433 /** set Boundary Conditions 434 * \param newbd BoundaryConditions 435 */ 436 virtual void setBoundaryConditions(SP::BoundaryCondition newbd); 437 438 /** get Boundary Conditions 439 * \return SP::BoundaryCondition pointer on a BoundaryConditions 440 */ boundaryConditions()441 inline SP::BoundaryCondition boundaryConditions() 442 { 443 return _boundaryConditions; 444 }; 445 446 /** set Reaction to Boundary Conditions 447 * \param newrbd BoundaryConditions pointer 448 */ setReactionToBoundaryConditions(SP::SiconosVector newrbd)449 inline void setReactionToBoundaryConditions(SP::SiconosVector newrbd) 450 { 451 _reactionToBoundaryConditions = newrbd; 452 }; 453 454 /** get Reaction to Boundary Conditions 455 * \return pointer on a BoundaryConditions 456 */ reactionToBoundaryConditions()457 inline SP::SiconosVector reactionToBoundaryConditions() 458 { 459 return _reactionToBoundaryConditions; 460 }; 461 462 463 /** Allocate memory for the lu factorization of the mass of the system. 464 Useful for some integrators with system inversion involving the mass 465 */ 466 virtual void init_inverse_mass() =0; 467 468 /** Update the content of the lu factorization of the mass of the system, 469 if required. 470 */ 471 virtual void update_inverse_mass() =0; 472 473 /** Allocate memory for forces and its jacobian. 474 */ 475 virtual void init_forces()=0; 476 477 ///@} 478 479 ACCEPT_STD_VISITORS(); 480 481 }; 482 483 //TYPEDEF_SPTR(SecondOrderDS) 484 485 #endif // LAGRANGIANDS_H 486