Contacts.h 21.7 KB
Newer Older
1
/*
2 3 4
 * Original Copyright (c) 2011-2016 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
 *
 *
5
 * RDL - Robot Dynamics Library
6
 * Modifications Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
7 8 9 10
 *
 * Licensed under the zlib license. See LICENSE for more details.
 */

11 12
#ifndef RDL_CONTACTS_H
#define RDL_CONTACTS_H
Jordan Lack's avatar
Jordan Lack committed
13 14 15 16

/**
 * @file Contacts.h
 */
17

18
#include <rdl_dynamics/rdl_eigenmath.h>
Jordan Lack's avatar
Jordan Lack committed
19
#include <rdl_dynamics/rdl_mathutils.h>
20

21
namespace RobotDynamics
22
{
23 24 25 26
/** \page contacts_page External Contacts
 *
 * All functions related to contacts are specified in the \ref
 * contacts_group "Contacts Module".
27

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.
 */
167
struct ConstraintSet
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.
189
     */
190 191 192 193 194 195 196 197
    ConstraintSet Copy()
    {
        ConstraintSet result(*this);

        result.bound = false;

        return result;
    }
198

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;
    }
205

206
    /** \brief Initializes and allocates memory for the constraint set.
207
     *
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.
213
     *
214 215
     * The values of ConstraintSet::acceleration may still be
     * modified after the set is bound to the model.
216
     */
217 218 219 220
    bool bind(const Model& model);

    /** \brief Returns the number of constraints. */
    size_t size() const
221
    {
222 223
        return acceleration.size();
    }
224

225 226
    /** \brief Clears all variables in the constraint set. */
    void clear();
227

228 229
    /// Method that should be used to solve internal linear systems.
    Math::LinearSolver linear_solver;
230

231 232
    /// Whether the constraint set was bound to a model (mandatory!).
    bool bound;
233

234 235 236 237
    std::vector<std::string> name;
    std::vector<unsigned int> body;
    std::vector<Math::Vector3d> point;
    std::vector<Math::Vector3d> normal;
238

239 240 241
    /** Enforced accelerations of the contact points along the contact
     * normal. */
    Math::VectorNd acceleration;
242

243 244
    /** Actual constraint forces along the contact normals. */
    Math::VectorNd force;
245

246 247
    /** Actual constraint impulses along the contact normals. */
    Math::VectorNd impulse;
248

249 250 251
    /** The velocities we want to have along the contact normals after
     * calling ComputeContactImpulsesLagrangian */
    Math::VectorNd v_plus;
252

253
    // Variables used by the Lagrangian methods
254

255 256
    /// Workspace for the joint space inertia matrix.
    Math::MatrixNd H;
257

258 259
    /// Workspace for the coriolis forces.
    Math::VectorNd C;
260

261 262 263
    /// Workspace of the lower part of b.
    Math::VectorNd gamma;
    Math::MatrixNd G;
264

265 266
    /// Workspace for the Lagrangian left-hand-side matrix.
    Math::MatrixNd A;
267

268 269
    /// Workspace for the Lagrangian right-hand-side.
    Math::VectorNd b;
270

271 272
    /// Workspace for the Lagrangian solution.
    Math::VectorNd x;
273

274 275
    /// Workspace for the QR decomposition of the null-space method
    Eigen::HouseholderQR<Math::MatrixNd> GT_qr;
276

277 278 279 280 281
    Math::MatrixNd GT_qr_Q;
    Math::MatrixNd Y;
    Math::MatrixNd Z;
    Math::VectorNd qddot_y;
    Math::VectorNd qddot_z;
282

283
    // Variables used by the IABI methods
284

285 286
    /// Workspace for the Inverse Articulated-Body Inertia.
    Math::MatrixNd K;
287

288 289
    /// Workspace for the accelerations of due to the test forces
    Math::VectorNd a;
290

291 292
    /// Workspace for the test accelerations.
    Math::VectorNd QDDot_t;
293

294 295
    /// Workspace for the default accelerations.
    Math::VectorNd QDDot_0;
296

297 298
    /// Workspace for the test forces.
    std::vector<Math::SpatialVector> f_t;
299

300 301
    /// Workspace for the actual spatial forces.
    std::vector<Math::SpatialVector> f_ext_constraints;
302

303 304
    /// Workspace for the default point accelerations.
    std::vector<Math::Vector3d> point_accel_0;
305

306 307
    /// Workspace for the bias force due to the test force
    std::vector<Math::SpatialVector> d_pA;
308

309 310 311
    /// Workspace for the acceleration due to the test force
    std::vector<Math::SpatialVector> d_a;
    Math::VectorNd d_u;
312

313 314
    /// Workspace for the inertia when applying constraint forces
    std::vector<Math::SpatialMatrix> d_IA;
315

316 317
    /// Workspace when applying constraint forces
    std::vector<Math::SpatialVector> d_U;
318

319 320
    /// Workspace when applying constraint forces
    Math::VectorNd d_d;
321

322 323
    std::vector<Math::Vector3d> d_multdof3_u;
};
324

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
 */
333
void calcContactJacobian(Model& model, const Math::VectorNd& Q, const ConstraintSet& CS, Math::MatrixNd& G, bool update_kinematics = true);
334

335
void calcContactSystemVariables(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& Tau, ConstraintSet& CS);
336

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.
 *
 */
387
void forwardDynamicsContactsDirect(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& Tau, ConstraintSet& CS,
Jordan Lack's avatar
Jordan Lack committed
388
                                   Math::VectorNd& QDDot);
389

390
void forwardDynamicsContactsRangeSpaceSparse(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& Tau, ConstraintSet& CS,
Jordan Lack's avatar
Jordan Lack committed
391
                                             Math::VectorNd& QDDot);
392

393
void forwardDynamicsContactsNullSpace(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& Tau, ConstraintSet& CS,
Jordan Lack's avatar
Jordan Lack committed
394
                                      Math::VectorNd& QDDot);
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
 */
458
void forwardDynamicsContactsKokkevis(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& Tau, ConstraintSet& CS,
Jordan Lack's avatar
Jordan Lack committed
459
                                     Math::VectorNd& QDDot);
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)
 */
507
void computeContactImpulsesDirect(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDotMinus, ConstraintSet& CS, Math::VectorNd& QDotPlus);
508 509 510

/** @brief Resolves contact gain using solveContactSystemRangeSpaceSparse()
 */
Jordan Lack's avatar
Jordan Lack committed
511
void computeContactImpulsesRangeSpaceSparse(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDotMinus, ConstraintSet& CS, Math::VectorNd& QDotPlus);
512 513 514

/** @brief Resolves contact gain using solveContactSystemNullSpace()
 */
515
void computeContactImpulsesNullSpace(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDotMinus, ConstraintSet& CS, Math::VectorNd& QDotPlus);
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
 */
533
void solveContactSystemDirect(Math::MatrixNd& H, const Math::MatrixNd& G, const Math::VectorNd& c, const Math::VectorNd& gamma, Math::VectorNd& qddot,
Jordan Lack's avatar
Jordan Lack committed
534
                              Math::VectorNd& lambda, Math::MatrixNd& A, Math::VectorNd& b, Math::VectorNd& x, Math::LinearSolver& linear_solver);
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
 */
553
void solveContactSystemRangeSpaceSparse(Model& model, Math::MatrixNd& H, const Math::MatrixNd& G, const Math::VectorNd& c, const Math::VectorNd& gamma,
Jordan Lack's avatar
Jordan Lack committed
554
                                        Math::VectorNd& qddot, Math::VectorNd& lambda, Math::MatrixNd& K, Math::VectorNd& a, Math::LinearSolver linear_solver);
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
 */
575
void solveContactSystemNullSpace(Math::MatrixNd& H, const Math::MatrixNd& G, const Math::VectorNd& c, const Math::VectorNd& gamma, Math::VectorNd& qddot,
Jordan Lack's avatar
Jordan Lack committed
576 577
                                 Math::VectorNd& lambda, Math::MatrixNd& Y, Math::MatrixNd& Z, Math::VectorNd& qddot_y, Math::VectorNd& qddot_z,
                                 Math::LinearSolver& linear_solver);
578 579

/** @} */
580
} /* namespace RobotDynamics */
581

582
/* RDL_CONTACTS_H */
583
#endif  // ifndef RDL_CONTACTS_H