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