Contacts.h 21.7 KB
 Jordan Lack committed May 19, 2016 1 /*  Jordan Lack committed Jan 02, 2017 2 3 4  * Original Copyright (c) 2011-2016 Martin Felis * *  Jordan Lack committed Feb 06, 2017 5  * RDL - Robot Dynamics Library  Jordan Lack committed Jan 02, 2017 6  * Modifications Copyright (c) 2017 Jordan Lack  Jordan Lack committed May 19, 2016 7 8 9 10  * * Licensed under the zlib license. See LICENSE for more details. */  Jordan Lack committed Feb 06, 2017 11 12 #ifndef RDL_CONTACTS_H #define RDL_CONTACTS_H  Jordan Lack committed Feb 04, 2017 13 14 15 16  /** * @file Contacts.h */  Jordan Lack committed May 19, 2016 17   Jordan Lack committed Feb 07, 2017 18 #include  Jordan Lack committed Feb 07, 2017 19 #include  Jordan Lack committed May 19, 2016 20   Jordan Lack committed Feb 06, 2017 21 namespace RobotDynamics  Jordan Lack committed Dec 18, 2016 22 {  Jordan Lack committed Oct 13, 2018 23 24 25 26 /** \page contacts_page External Contacts * * All functions related to contacts are specified in the \ref * contacts_group "Contacts Module".  Jordan Lack committed Dec 18, 2016 27   Jordan Lack committed Oct 13, 2018 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166  * \defgroup contacts_group Contacts * * External contacts are handled by specification of a \link * RobotDynamics::ConstraintSet * ConstraintSet \endlink which contains all informations about the * current contacts and workspace memory. * * Separate contacts can be specified by calling * ConstraintSet::addConstraint(). After all constraints have been * specified, this \link * RobotDynamics::ConstraintSet * ConstraintSet \endlink has to be bound to the model via * ConstraintSet::bind(). This initializes workspace memory that is * later used when calling one of the contact functions, such as * ForwardDynamicsContacts() or ForwardDynamicsContactsLagrangian(). * * The values in the vectors ConstraintSet::force and * ConstraintSet::impulse contain the computed force or * impulse values for each constraint when returning from one of the * contact functions. * * \section solution_constraint_system Solution of the Constraint System * * \subsection constraint_system Linear System of the Constrained Dynamics * * External contacts are constraints that act on the model. To compute the * acceleration one has to solve a linear system of the form: \f[ \left( \begin{array}{cc} H & G^T \\ G & 0 \end{array} \right) \left( \begin{array}{c} \ddot{q} \\ - \lambda \end{array} \right) = \left( \begin{array}{c} -C + \tau \\ \gamma \end{array} \right) * \f] where \f$H\f$ is the joint space inertia matrix computed with the * CompositeRigidBodyAlgorithm(), \f$G\f$ are the point jacobians of the * contact points, \f$C\f$ the bias force (sometimes called "non-linear * effects"), and \f$\gamma\f$ the generalized acceleration independent * part of the contact point accelerations. * * \subsection collision_system Linear System of the Contact Collision * * Similarly to compute the response of the model to a contact gain one has * to solve a system of the following form: \f[ \left( \begin{array}{cc} H & G^T \\ G & 0 \end{array} \right) \left( \begin{array}{c} \dot{q}^{+} \\ \Lambda \end{array} \right) = \left( \begin{array}{c} H \dot{q}^{-} \\ v^{+} \end{array} \right) * \f] where \f$H\f$ is the joint space inertia matrix computed with the * CompositeRigidBodyAlgorithm(), \f$G\f$ are the point jacobians of the * contact points, \f$\dot{q}^{+}\f$ the generalized velocity after the * impact, \f$\Lambda\f$ the impulses at each constraint, \f$\dot{q}^{-}\f$ * the generalized velocity before the impact, and \f$v^{+}\f$ the desired * velocity of each constraint after the impact (known beforehand, usually * 0). The value of \f$v^{+}\f$ can is specified via the variable * ConstraintSet::v_plus and defaults to 0. * * \subsection solution_methods Solution Methods * * There are essentially three different approaches to solve these systems: * -# \b Direct: solve the full system to simultaneously compute * \f$\ddot{q}\f$ and \f$\lambda\f$. This may be slow for large systems * and many constraints. * -# \b Range-Space: solve first for \f$\lambda\f$ and then for * \f$\ddot{q}\f$. * -# \b Null-Space: solve furst for \f$\ddot{q}\f$ and then for * \f$\lambda\f$ * The methods are the same for the contact gaints just with different * variables on the right-hand-side. * * RDL provides methods for all approaches. The implementation for the * range-space method also exploits sparsities in the joint space inertia * matrix using a sparse structure preserving \f$L^TL\f$ decomposition as * described in Chapter 8.5 of "Rigid Body Dynamics Algorithms". * * None of the methods is generally superior to the others and each has * different trade-offs as factors such as model topology, number of * constraints, constrained bodies, numerical stability, and performance * vary and evaluation has to be made on a case-by-case basis. * * \subsection solving_constraints_dynamics Methods for Solving Constrained Dynamics * * RDL provides the following methods to compute the acceleration of a * constrained system: * * - ForwardDynamicsContactsDirect() * - ForwardDynamicsContactsRangeSpaceSparse() * - ForwardDynamicsContactsNullSpace() * * \subsection solving_constraints_collisions Methods for Computing Collisions * * RDL provides the following methods to compute the collision response on * new contact gains: * * - ComputeContactImpulsesDirect() * - ComputeContactImpulsesRangeSpaceSparse() * - ComputeContactImpulsesNullSpace() * * @{ */ struct Model; /** \brief Structure that contains both constraint information and workspace memory. * * This structure is used to reduce the amount of memory allocations that * are needed when computing constraint forces. * * The ConstraintSet has to be bound to a model using ConstraintSet::bind() * before it can be used in \link RobotDynamics::ForwardDynamicsContacts * ForwardDynamicsContacts \endlink. */  Jordan Lack committed May 11, 2019 167 struct ConstraintSet  Jordan Lack committed Oct 13, 2018 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 { ConstraintSet() : linear_solver(Math::LinearSolverColPivHouseholderQR), bound(false) { } /** \brief Adds a constraint to the constraint set. * * \param body_id the body which is affected directly by the constraint * \param body_point the point that is constrained relative to the * contact body * \param world_normal the normal along the constraint acts (in base * coordinates) * \param constraint_name a human readable name (optional, default: NULL) * \param normal_acceleration the acceleration of the contact along the normal * (optional, default: 0.) */ unsigned int addConstraint(unsigned int body_id, const Math::Vector3d& body_point, const Math::Vector3d& world_normal, const char* constraint_name = NULL, double normal_acceleration = 0.); /** \brief Copies the constraints and resets its ConstraintSet::bound * flag.  Jordan Lack committed Dec 18, 2016 189  */  Jordan Lack committed Oct 13, 2018 190 191 192 193 194 195 196 197  ConstraintSet Copy() { ConstraintSet result(*this); result.bound = false; return result; }  Jordan Lack committed Dec 18, 2016 198   Jordan Lack committed Oct 13, 2018 199 200 201 202 203 204  /** \brief Specifies which method should be used for solving undelying linear systems. */ void setSolver(Math::LinearSolver solver) { linear_solver = solver; }  Jordan Lack committed Dec 18, 2016 205   Jordan Lack committed Oct 13, 2018 206  /** \brief Initializes and allocates memory for the constraint set.  Jordan Lack committed Dec 18, 2016 207  *  Jordan Lack committed Oct 13, 2018 208 209 210 211 212  * This function allocates memory for temporary values and matrices that * are required for contact force computation. Both model and constraint * set must not be changed after a binding as the required memory is * dependent on the model size (i.e. the number of bodies and degrees of * freedom) and the number of constraints in the Constraint set.  Jordan Lack committed Dec 18, 2016 213  *  Jordan Lack committed Oct 13, 2018 214 215  * The values of ConstraintSet::acceleration may still be * modified after the set is bound to the model.  Jordan Lack committed Dec 18, 2016 216  */  Jordan Lack committed Oct 13, 2018 217 218 219 220  bool bind(const Model& model); /** \brief Returns the number of constraints. */ size_t size() const  Jordan Lack committed Dec 18, 2016 221  {  Jordan Lack committed Oct 13, 2018 222 223  return acceleration.size(); }  Jordan Lack committed Dec 18, 2016 224   Jordan Lack committed Oct 13, 2018 225 226  /** \brief Clears all variables in the constraint set. */ void clear();  Jordan Lack committed Dec 18, 2016 227   Jordan Lack committed Oct 13, 2018 228 229  /// Method that should be used to solve internal linear systems. Math::LinearSolver linear_solver;  Jordan Lack committed Jul 01, 2017 230   Jordan Lack committed Oct 13, 2018 231 232  /// Whether the constraint set was bound to a model (mandatory!). bool bound;  Jordan Lack committed Dec 18, 2016 233   Jordan Lack committed Oct 13, 2018 234 235 236 237  std::vector name; std::vector body; std::vector point; std::vector normal;  Jordan Lack committed Dec 18, 2016 238   Jordan Lack committed Oct 13, 2018 239 240 241  /** Enforced accelerations of the contact points along the contact * normal. */ Math::VectorNd acceleration;  Jordan Lack committed Jul 01, 2017 242   Jordan Lack committed Oct 13, 2018 243 244  /** Actual constraint forces along the contact normals. */ Math::VectorNd force;  Jordan Lack committed Jul 01, 2017 245   Jordan Lack committed Oct 13, 2018 246 247  /** Actual constraint impulses along the contact normals. */ Math::VectorNd impulse;  Jordan Lack committed Jul 01, 2017 248   Jordan Lack committed Oct 13, 2018 249 250 251  /** The velocities we want to have along the contact normals after * calling ComputeContactImpulsesLagrangian */ Math::VectorNd v_plus;  Jordan Lack committed Dec 18, 2016 252   Jordan Lack committed Oct 13, 2018 253  // Variables used by the Lagrangian methods  Jordan Lack committed Dec 18, 2016 254   Jordan Lack committed Oct 13, 2018 255 256  /// Workspace for the joint space inertia matrix. Math::MatrixNd H;  Jordan Lack committed Jul 01, 2017 257   Jordan Lack committed Oct 13, 2018 258 259  /// Workspace for the coriolis forces. Math::VectorNd C;  Jordan Lack committed Jul 01, 2017 260   Jordan Lack committed Oct 13, 2018 261 262 263  /// Workspace of the lower part of b. Math::VectorNd gamma; Math::MatrixNd G;  Jordan Lack committed Jul 01, 2017 264   Jordan Lack committed Oct 13, 2018 265 266  /// Workspace for the Lagrangian left-hand-side matrix. Math::MatrixNd A;  Jordan Lack committed Jul 01, 2017 267   Jordan Lack committed Oct 13, 2018 268 269  /// Workspace for the Lagrangian right-hand-side. Math::VectorNd b;  Jordan Lack committed Dec 18, 2016 270   Jordan Lack committed Oct 13, 2018 271 272  /// Workspace for the Lagrangian solution. Math::VectorNd x;  Jordan Lack committed May 19, 2016 273   Jordan Lack committed Oct 13, 2018 274 275  /// Workspace for the QR decomposition of the null-space method Eigen::HouseholderQR GT_qr;  Jordan Lack committed Dec 18, 2016 276   Jordan Lack committed Oct 13, 2018 277 278 279 280 281  Math::MatrixNd GT_qr_Q; Math::MatrixNd Y; Math::MatrixNd Z; Math::VectorNd qddot_y; Math::VectorNd qddot_z;  Jordan Lack committed Dec 18, 2016 282   Jordan Lack committed Oct 13, 2018 283  // Variables used by the IABI methods  Jordan Lack committed Jul 01, 2017 284   Jordan Lack committed Oct 13, 2018 285 286  /// Workspace for the Inverse Articulated-Body Inertia. Math::MatrixNd K;  Jordan Lack committed Jul 01, 2017 287   Jordan Lack committed Oct 13, 2018 288 289  /// Workspace for the accelerations of due to the test forces Math::VectorNd a;  Jordan Lack committed Jul 01, 2017 290   Jordan Lack committed Oct 13, 2018 291 292  /// Workspace for the test accelerations. Math::VectorNd QDDot_t;  Jordan Lack committed Jul 01, 2017 293   Jordan Lack committed Oct 13, 2018 294 295  /// Workspace for the default accelerations. Math::VectorNd QDDot_0;  Jordan Lack committed Jul 01, 2017 296   Jordan Lack committed Oct 13, 2018 297 298  /// Workspace for the test forces. std::vector f_t;  Jordan Lack committed Jul 01, 2017 299   Jordan Lack committed Oct 13, 2018 300 301  /// Workspace for the actual spatial forces. std::vector f_ext_constraints;  Jordan Lack committed Dec 18, 2016 302   Jordan Lack committed Oct 13, 2018 303 304  /// Workspace for the default point accelerations. std::vector point_accel_0;  Jordan Lack committed Jul 01, 2017 305   Jordan Lack committed Oct 13, 2018 306 307  /// Workspace for the bias force due to the test force std::vector d_pA;  Jordan Lack committed Dec 18, 2016 308   Jordan Lack committed Oct 13, 2018 309 310 311  /// Workspace for the acceleration due to the test force std::vector d_a; Math::VectorNd d_u;  Jordan Lack committed Jul 01, 2017 312   Jordan Lack committed Oct 13, 2018 313 314  /// Workspace for the inertia when applying constraint forces std::vector d_IA;  Jordan Lack committed Jul 01, 2017 315   Jordan Lack committed Oct 13, 2018 316 317  /// Workspace when applying constraint forces std::vector d_U;  Jordan Lack committed Dec 18, 2016 318   Jordan Lack committed Oct 13, 2018 319 320  /// Workspace when applying constraint forces Math::VectorNd d_d;  Jordan Lack committed Dec 18, 2016 321   Jordan Lack committed Oct 13, 2018 322 323  std::vector d_multdof3_u; };  Jordan Lack committed Dec 26, 2016 324   Jordan Lack committed Oct 13, 2018 325 326 327 328 329 330 331 332 /** @brief Computes the Jacobian for the given ConstraintSet * * @param model the model * @param Q the generalized positions of the joints * @param CS the constraint set for which the Jacobian should be computed * @param G (output) matrix where the output will be stored in * @param update_kinematics whether the kinematics of the model should be * updated from Q */  Jordan Lack committed May 11, 2019 333 void calcContactJacobian(Model& model, const Math::VectorNd& Q, const ConstraintSet& CS, Math::MatrixNd& G, bool update_kinematics = true);  Jordan Lack committed Dec 29, 2016 334   Jordan Lack committed May 11, 2019 335 void calcContactSystemVariables(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& Tau, ConstraintSet& CS);  Jordan Lack committed Dec 29, 2016 336   Jordan Lack committed Oct 13, 2018 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 /** @brief Computes forward dynamics with contact by constructing and solving the full lagrangian equation * * This method builds and solves the linear system \f[ \left( \begin{array}{cc} H & G^T \\ G & 0 \end{array} \right) \left( \begin{array}{c} \ddot{q} \\ -\lambda \end{array} \right) = \left( \begin{array}{c} -C + \tau \\ \gamma \end{array} \right) * \f] where \f$H\f$ is the joint space inertia matrix computed with the * CompositeRigidBodyAlgorithm(), \f$G\f$ are the point jacobians of the * contact points, \f$C\f$ the bias force (sometimes called "non-linear * effects"), and \f$\gamma\f$ the generalized acceleration independent * part of the contact point accelerations. * * @note So far, only constraints acting along cartesian coordinate axes * are allowed (i.e. (1, 0, 0), (0, 1, 0), and (0, 0, 1)). Also, one must * not specify redundant constraints! * * @par * * @note To increase performance group constraints body and pointwise such * that constraints acting on the same body point are sequentially in * ConstraintSet. This can save computation of point jacobians \f$G\f$. * * @param model rigid body model * @param Q state vector of the internal joints * @param QDot velocity vector of the internal joints * @param Tau actuations of the internal joints * @param CS the description of all acting constraints * @param QDDot accelerations of the internals joints (output) * * @note During execution of this function values such as * ConstraintSet::force get modified and will contain the value * of the force acting along the normal. * */  Jordan Lack committed May 11, 2019 387 void forwardDynamicsContactsDirect(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& Tau, ConstraintSet& CS,  Jordan Lack committed May 11, 2019 388  Math::VectorNd& QDDot);  Jordan Lack committed Oct 13, 2018 389   Jordan Lack committed May 11, 2019 390 void forwardDynamicsContactsRangeSpaceSparse(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& Tau, ConstraintSet& CS,  Jordan Lack committed May 11, 2019 391  Math::VectorNd& QDDot);  Jordan Lack committed Oct 13, 2018 392   Jordan Lack committed May 11, 2019 393 void forwardDynamicsContactsNullSpace(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& Tau, ConstraintSet& CS,  Jordan Lack committed May 11, 2019 394  Math::VectorNd& QDDot);  Jordan Lack committed Oct 13, 2018 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457  /** @brief Computes forward dynamics that accounts for active contacts in ConstraintSet. * * The method used here is the one described by Kokkevis and Metaxas in the * Paper "Practical Physics for Articulated Characters", Game Developers * Conference, 2004. * * It does this by recursively computing the inverse articulated-body inertia (IABI) \f[ \left( \begin{array}{c} \dot{v}_1 \\ \dot{v}_2 \\ \vdots \\ \dot{v}_n \end{array} \right) = \left( \begin{array}{cccc} \Phi_{1,1} & \Phi_{1,2} & \cdots & \Phi{1,n} \\ \Phi_{2,1} & \Phi_{2,2} & \cdots & \Phi{2,n} \\ \cdots & \cdots & \cdots & \vdots \\ \Phi_{n,1} & \Phi_{n,2} & \cdots & \Phi{n,n} \end{array} \right) \left( \begin{array}{c} f_1 \\ f_2 \\ \vdots \\ f_n \end{array} \right) + \left( \begin{array}{c} \phi_1 \\ \phi_2 \\ \vdots \\ \phi_n \end{array} \right). \f] * \f$\Phi_{i,j}\f$ which is then used to build and solve a system of the form: Here \f$n\f$ is the number of constraints and the method for building the system uses the Articulated Body Algorithm to efficiently compute entries of the system. The values \f$\dot{v}_i\f$ are the constraint accelerations, \f$f_i\f$ the constraint forces, and \f$\phi_i\f$ are the constraint bias forces. * * @param model rigid body model * @param Q state vector of the internal joints * @param QDot velocity vector of the internal joints * @param Tau actuations of the internal joints * @param CS a list of all contact points * @param QDDot accelerations of the internals joints (output) * * @note During execution of this function values such as * ConstraintSet::force get modified and will contain the value * of the force acting along the normal. * * @todo Allow for external forces */  Jordan Lack committed May 11, 2019 458 void forwardDynamicsContactsKokkevis(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& Tau, ConstraintSet& CS,  Jordan Lack committed May 11, 2019 459  Math::VectorNd& QDDot);  Jordan Lack committed Oct 13, 2018 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505 506  /** @brief Computes contact gain by constructing and solving the full lagrangian equation * * This method builds and solves the linear system \f[ \left( \begin{array}{cc} H & G^T \\ G & 0 \end{array} \right) \left( \begin{array}{c} \dot{q}^{+} \\ \Lambda \end{array} \right) = \left( \begin{array}{c} H \dot{q}^{-} \\ v^{+} \end{array} \right) * \f] where \f$H\f$ is the joint space inertia matrix computed with the * CompositeRigidBodyAlgorithm(), \f$G\f$ are the point jacobians of the * contact points, \f$\dot{q}^{+}\f$ the generalized velocity after the * impact, \f$\Lambda\f$ the impulses at each constraint, \f$\dot{q}^{-}\f$ * the generalized velocity before the impact, and \f$v^{+}\f$ the desired * velocity of each constraint after the impact (known beforehand, usually * 0). The value of \f$v^{+}\f$ can is specified via the variable * ConstraintSet::v_plus and defaults to 0. * * @note So far, only constraints acting along cartesian coordinate axes * are allowed (i.e. (1, 0, 0), (0, 1, 0), and (0, 0, 1)). Also, one must * not specify redundant constraints! * * @par * * @note To increase performance group constraints body and pointwise such * that constraints acting on the same body point are sequentially in * ConstraintSet. This can save computation of point Jacobians \f$G\f$. * @param model rigid body model * @param Q state vector of the internal joints * @param QDotMinus velocity vector of the internal joints before the impact * @param CS the set of active constraints * @param QDotPlus velocities of the internals joints after the impact (output) */  Jordan Lack committed May 11, 2019 507 void computeContactImpulsesDirect(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDotMinus, ConstraintSet& CS, Math::VectorNd& QDotPlus);  Jordan Lack committed Oct 13, 2018 508 509 510  /** @brief Resolves contact gain using solveContactSystemRangeSpaceSparse() */  Jordan Lack committed May 11, 2019 511 void computeContactImpulsesRangeSpaceSparse(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDotMinus, ConstraintSet& CS, Math::VectorNd& QDotPlus);  Jordan Lack committed Oct 13, 2018 512 513 514  /** @brief Resolves contact gain using solveContactSystemNullSpace() */  Jordan Lack committed May 11, 2019 515 void computeContactImpulsesNullSpace(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDotMinus, ConstraintSet& CS, Math::VectorNd& QDotPlus);  Jordan Lack committed Oct 13, 2018 516 517 518 519 520 521 522 523 524 525 526 527 528 529 530 531 532  /** @brief Solves the full contact system directly, i.e. simultaneously for contact forces and joint accelerations. * * This solves a \f$(n_\textit{dof} + * n_c) \times (n_\textit{dof} + n_c\f$ linear system. * * @param H the joint space inertia matrix * @param G the constraint jacobian * @param c the \f$\mathbb{R}^{n_\textit{dof}}\f$ vector of the upper part of the right hand side of the system * @param gamma the \f$\mathbb{R}^{n_c}\f$ vector of the lower part of the right hand side of the system * @param qddot result: joint accelerations * @param lambda result: constraint forces * @param A work-space for the matrix of the linear system * @param b work-space for the right-hand-side of the linear system * @param x work-space for the solution of the linear system * @param linear_solver of solver that should be used to solve the system */  Jordan Lack committed May 11, 2019 533 void solveContactSystemDirect(Math::MatrixNd& H, const Math::MatrixNd& G, const Math::VectorNd& c, const Math::VectorNd& gamma, Math::VectorNd& qddot,  Jordan Lack committed May 11, 2019 534  Math::VectorNd& lambda, Math::MatrixNd& A, Math::VectorNd& b, Math::VectorNd& x, Math::LinearSolver& linear_solver);  Jordan Lack committed Oct 13, 2018 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552  /** @brief Solves the contact system by first solving for the the joint accelerations and then the contact forces using a sparse matrix decomposition of the joint space inertia matrix. * * This method exploits the branch-induced sparsity by the structure * preserving \f$L^TL \f$ decomposition described in RDL, Section 6.5. * * @param model * @param H the joint space inertia matrix * @param G the constraint jacobian * @param c the \f$\mathbb{R}^{n_\textit{dof}}\f$ vector of the upper part of the right hand side of the system * @param gamma the \f$\mathbb{R}^{n_c}\f$ vector of the lower part of the right hand side of the system * @param qddot result: joint accelerations * @param lambda result: constraint forces * @param K work-space for the matrix of the constraint force linear system * @param a work-space for the right-hand-side of the constraint force linear system * @param linear_solver type of solver that should be used to solve the constraint force system */  Jordan Lack committed May 11, 2019 553 void solveContactSystemRangeSpaceSparse(Model& model, Math::MatrixNd& H, const Math::MatrixNd& G, const Math::VectorNd& c, const Math::VectorNd& gamma,  Jordan Lack committed May 11, 2019 554  Math::VectorNd& qddot, Math::VectorNd& lambda, Math::MatrixNd& K, Math::VectorNd& a, Math::LinearSolver linear_solver);  Jordan Lack committed Oct 13, 2018 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574  /** @brief Solves the contact system by first solving for the joint accelerations and then for the constraint forces. * * This methods requires a \f$n_\textit{dof} \times n_\textit{dof}\f$ * matrix of the form \f$\left[ \ Y \ | Z \ \right]\f$ with the property * \f$GZ = 0\f$ that can be computed using a QR decomposition (e.g. see * code for ForwardDynamicsContactsNullSpace()). * * @param H the joint space inertia matrix * @param G the constraint jacobian * @param c the \f$\mathbb{R}^{n_\textit{dof}}\f$ vector of the upper part of the right hand side of the system * @param gamma the \f$\mathbb{R}^{n_c}\f$ vector of the lower part of the right hand side of the system * @param qddot result: joint accelerations * @param lambda result: constraint forces * @param Y basis for the range-space of the constraints * @param Z basis for the null-space of the constraints * @param qddot_y work-space of size \f$\mathbb{R}^{n_\textit{dof}}\f$ * @param qddot_z work-space of size \f$\mathbb{R}^{n_\textit{dof}}\f$ * @param linear_solver type of solver that should be used to solve the system */  Jordan Lack committed May 11, 2019 575 void solveContactSystemNullSpace(Math::MatrixNd& H, const Math::MatrixNd& G, const Math::VectorNd& c, const Math::VectorNd& gamma, Math::VectorNd& qddot,  Jordan Lack committed May 11, 2019 576 577  Math::VectorNd& lambda, Math::MatrixNd& Y, Math::MatrixNd& Z, Math::VectorNd& qddot_y, Math::VectorNd& qddot_z, Math::LinearSolver& linear_solver);  Jordan Lack committed Oct 13, 2018 578 579  /** @} */  Jordan Lack committed Feb 06, 2017 580 } /* namespace RobotDynamics */  Jordan Lack committed May 19, 2016 581   Jordan Lack committed Feb 06, 2017 582 /* RDL_CONTACTS_H */  Jordan Lack committed Oct 13, 2018 583 #endif // ifndef RDL_CONTACTS_H