From d3aa15dba478f6b696c8a77c0ea80163f74c478f Mon Sep 17 00:00:00 2001 From: amcastro-tri Date: Tue, 21 May 2024 10:49:24 -0400 Subject: [PATCH] Moves 'multibody/constraint' into a private location. Remove old (wrong) references in our Doxygen documentation. --- doc/doxygen_cxx/doxygen.h | 5 - examples/rod2d/BUILD.bazel | 28 +- .../rod2d/README.md | 287 +++++++++--------- .../rod2d}/constraint_problem_data.h | 0 .../rod2d}/constraint_solver.cc | 2 +- .../rod2d}/constraint_solver.h | 2 +- .../rod2d}/images/colliding-boxes.png | Bin examples/rod2d/rod2d.h | 4 +- .../rod2d}/test/constraint_solver_test.cc | 2 +- examples/rod2d/test/rod2d_test.cc | 4 +- multibody/constraint/BUILD.bazel | 55 ---- tools/install/libdrake/build_components.bzl | 1 - 12 files changed, 178 insertions(+), 212 deletions(-) rename multibody/constraint/constraint_doxygen.h => examples/rod2d/README.md (57%) rename {multibody/constraint => examples/rod2d}/constraint_problem_data.h (100%) rename {multibody/constraint => examples/rod2d}/constraint_solver.cc (75%) rename {multibody/constraint => examples/rod2d}/constraint_solver.h (99%) rename {multibody/constraint => examples/rod2d}/images/colliding-boxes.png (100%) rename {multibody/constraint => examples/rod2d}/test/constraint_solver_test.cc (99%) delete mode 100644 multibody/constraint/BUILD.bazel diff --git a/doc/doxygen_cxx/doxygen.h b/doc/doxygen_cxx/doxygen.h index 565c63a7418a..7760daf39b1a 100644 --- a/doc/doxygen_cxx/doxygen.h +++ b/doc/doxygen_cxx/doxygen.h @@ -167,11 +167,6 @@ namespace solvers { */ } // namespace solvers -/** - @defgroup constraint_overview Multibody Dynamics Constraints - @ingroup multibody -*/ - // TODO(russt): Take a thorough pass through the algorithms group // documentation, adding brief descriptions of each and tagging the relevant // algorithms throughout the code. diff --git a/examples/rod2d/BUILD.bazel b/examples/rod2d/BUILD.bazel index 1fbee40df733..46f5eeb3a6f4 100644 --- a/examples/rod2d/BUILD.bazel +++ b/examples/rod2d/BUILD.bazel @@ -8,6 +8,19 @@ load( package(default_visibility = ["//visibility:private"]) +drake_cc_library( + name = "constraint_solver", + srcs = ["constraint_solver.cc"], + hdrs = [ + "constraint_problem_data.h", + "constraint_solver.h", + ], + deps = [ + "//common:essential", + "//solvers:moby_lcp_solver", + ], +) + drake_cc_library( name = "rod2d_state_vector", srcs = ["rod2d_state_vector.cc"], @@ -45,11 +58,10 @@ drake_cc_library( hdrs = [ "rod2d.h", ], - visibility = ["//multibody/constraint:__pkg__"], deps = [ + ":constraint_solver", ":rod2d_state_vector", "//common:essential", - "//multibody/constraint", "//solvers:mathematical_program", "//systems/framework:leaf_system", ], @@ -77,14 +89,24 @@ drake_cc_library( # === test/ === +drake_cc_googletest( + name = "constraint_solver_test", + deps = [ + ":constraint_solver", + ":rod2d", + "//solvers:moby_lcp_solver", + "//solvers:unrevised_lemke_solver", + ], +) + drake_cc_googletest( name = "rod2d_test", deps = [ + ":constraint_solver", ":rod2d", "//common:essential", "//common/test_utilities:eigen_matrix_compare", "//common/test_utilities:expect_no_throw", - "//multibody/constraint", "//systems/analysis:simulator", ], ) diff --git a/multibody/constraint/constraint_doxygen.h b/examples/rod2d/README.md similarity index 57% rename from multibody/constraint/constraint_doxygen.h rename to examples/rod2d/README.md index f6b158450bcf..4f2315cd30f7 100644 --- a/multibody/constraint/constraint_doxygen.h +++ b/examples/rod2d/README.md @@ -1,21 +1,26 @@ -/** @file - Doxygen-only documentation for @ref constraint_overview */ - -/** @addtogroup constraint_overview - -This documentation describes the types of multibody constraints supported in -Drake, including specialized constraint types- namely point-based contact -constraints that allow Drake's constraint solver to readily incorporate the -Coulomb friction model. - -Drake's constraint system helps solve computational dynamics problems with +# Multibody Dynamics Constraints + +This documentation describes `ConstraintSolver`, a prototype solver for mixed +linear complementarity problems (MLCPs). In particular, problems corresponding +to rigid multi-body systems with bilateral and/or unilateral constraints as well +as polyhedral approximations of friction cone constraints. Although very feature +complete, we found out that this solver performs very badly for real robotic +applications. The reason being that even though there is good theory on the +existence and uniqueness of MLCP solutions, these problems are often badly +ill-conditioned numerically, leading to catastrophic failures in practice. +Moreover, we found out that performance does not scale well with the number of +constraints. This lack of rubustness and performance is the reason why you find +this implementation within an example and not as part of a supported main Drake +feature. + +`ConstraintSolver` helps solve computational dynamics problems with algebraic constraints, like differential algebraic equations:
 ẋ = f(x, t, λ)
 0 = g(x, t, λ)
 
where `g()` corresponds to one or more constraint equations and λ -is typically interpretable as "forces" that enforce the constraints. Drake's -constraint system permits solving initial value problems subject to such +is typically interpretable as "forces" that enforce the constraints. +`ConstraintSolver` permits solving initial value problems subject to such constraints and does so in a manner very different from "smoothing methods" (also known as "penalty methods"). Smoothing methods have traditionally required significant computation to maintain `g = 0` to high accuracy (and typically @@ -31,58 +36,18 @@ variables (i.e., the constraint forces) are computed to low accuracy. This discussion will provide necessary background material in: - - @ref constraint_types - - @ref constraint_stabilization - - @ref constraint_Jacobians - - @ref contact_surface_constraints - -and will delve into the constraint solver functionality in: - - - @ref discretization - - A prodigious number of variables will be referenced throughout the discussion - on constraints. Variables common to both acceleration-level constraints and - velocity-level constraints will be covered in @ref constraint_variable_defs. - - References for this discussion will be provided in @ref constraint_references. -*/ - -/** @defgroup constraint_variable_defs Variable definitions -@ingroup constraint_overview -- nb The number of bilateral constraint equations (nb ≥ 0) -- nk The number of edges in a polygonal approximation to a friction - cone (nk ≥ 4 for contacts between three-dimensional bodies, nk = 2 for - contacts between two-dimensional bodies). Note that nk = 2nr (where - nr is defined immediately below). -- nr *Half* the number of edges in a polygonal approximation to a - friction cone. (nr ≥ 2 for contacts between three-dimensional bodies, - nr = 1 for contacts between two-dimensional bodies). -- nc The number of contact surface constraint equations. -- nck The total number of edges in the polygonal approximations to the - nc friction cones corresponding to the nc point contacts. Note that - nck = 2ncr (where ncr is defined immediately below). -- ncr *Half* the total number of edges in the polygonal approximations to the - nc friction cones corresponding to the nc point contacts. -- nv The dimension of the system generalized velocity / force. -- nq The dimension of the system generalized coordinates. -- v The system's generalized velocity vector (of dimension nv), which is a - linear transformation of the time derivative of the system's - generalized coordinates. -- q The generalized coordinate vector of the system (of dimension nq). -- t The system time variable (a non-negative real number). -- nu The number of "generic" (non-contact related) unilateral constraint - equations. -- α A non-negative, real valued scalar used to correct the time derivative - of position constraint errors (i.e., "stabilize" the constraints) via - an error feedback process (Baumgarte Stabilization). -- β A non-negative, real valued scalar used to correct position constraint - errors via the same error feedback process (Baumgarte - Stabilization) that uses α. -- γ Non-negative, real valued scalar used to regularize constraints. -*/ - -/** @defgroup constraint_types Constraint types -@ingroup constraint_overview + - [Constraint types](#constraint_types) + - [Constraint stabilization](#constraint_stabilization) + - [Constraint Jacobians](#constraint_Jacobians) + - [Contact surface constraints](#contact_surface_constraints) + +A prodigious number of variables will be referenced throughout the discussion on +constraints. Variables common to both acceleration-level constraints and +velocity-level constraints will be covered in [Variable +definitions](#constraint_variable_defs). References for this discussion will be +provided in [References](#constraint_references). + +## Constraint types Constraints can be categorized as either bilateral ("two-sided" constraints, e.g., g(q) = 0) or unilateral ("one-sided" constraints, e.g., g(q) ≥ 0). @@ -119,23 +84,24 @@ constraint dependencies (e.g., t,q,v in gₐ) from variables that must be determined using the constraints (e.g., v̇ in gₐ).

Constraints with velocity-level unknowns

+ This documentation distinguishes between equations that are posed at the position level but are differentiated once with respect to time (i.e., to reduce the Differential -Algebraic Equation or Differential Complementarity Problem index -@ref Ascher1998 "[Ascher 1998]"): -
+Algebraic Equation or Differential Complementarity Problem index [[Ascher
+1998]](#Ascher1998)):
 d/dt gₚ(t; q) = ġₚ(t, q; v)
 
vs. equations that **must** be posed at the velocity-level (i.e., nonholonomic constraints):
 gᵥ(t, q; v).
 
-Both cases yield a constraint with velocity-level unknowns, thereby -allowing the constraint to be used in a velocity-level constraint formulation -(e.g., an Index-2 DAE or Index-2 DCP). *Only the former constraint may "drift" -from zero over time*, due to truncation and discretization errors, unless -corrected. See @ref constraint_stabilization for further information. + +Both cases yield a constraint with velocity-level unknowns, thereby allowing the +constraint to be used in a velocity-level constraint formulation (e.g., an +Index-2 DAE or Index-2 DCP). *Only the former constraint may "drift" from zero +over time*, due to truncation and discretization errors, unless corrected. See +[Constraint stabilization](#constraint_stabilization) for further information.

Constraints with force and acceleration-level unknowns

A bilateral constraint equation with force and acceleration-level unknowns will @@ -169,10 +135,9 @@ Interpreting this triplet of constraint equations, two conditions become apparent: (1) when the constraint is inactive (gₐ > 0), the constraint force must be zero (λ = 0) and (2) the constraint force can only act in one direction (λ ≥ 0). This triplet is known as a *complementarity constraint*. -*/ -/** @defgroup constraint_stabilization Constraint stabilization -@ingroup constraint_overview + +## Constraint stabilization Both truncation and rounding errors can prevent constraints from being exactly satisfied. For example, consider the bilateral holonomic constraint @@ -196,7 +161,7 @@ g̈ₚ + 2αġₚ + β²gₚ = 0 for non-negative scalar α and real β (α and β can also represent diagonal matrices for greater generality). α and β, which both have units of 1/sec (i.e., the reciprocal of unit time) are described more fully in -@ref Baumgarte1972 "[Baumgarte 1972]". The use of α and β above make correcting +[[Baumgarte 1972]](#Baumgarte1972). The use of α and β above make correcting position-level (gₚ) and velocity-level (gₚ̇) holonomic constraint errors analogous to the dynamic problem of stabilizing a damped harmonic oscillator. Given that analogy, 2α is the viscous damping coefficient and β² the stiffness @@ -211,12 +176,11 @@ To eliminate constraint errors as quickly as possible, one strategy used in commercial software uses ζ=1, implying *critical damping*, and undamped angular frequency ω₀ that is high enough to correct errors rapidly but low enough to avoid computational stiffness. Picking that parameter is considered to be more -art than science (see @ref Ascher1995 "[Ascher 1995]"). Given desired ω₀ and ζ, +art than science (see [[Ascher 1995]](#Ascher1995)). Given desired ω₀ and ζ, α and β are set using the equations above. -*/ -/** @defgroup constraint_Jacobians Constraint Jacobian matrices -@ingroup constraint_overview + +## Constraint Jacobian matrices Much of the problem data necessary to account for constraints in dynamical systems refers to particular Jacobian matrices. These Jacobian matrices arise @@ -229,24 +193,20 @@ requiring us to leverage the relationship:
 q̇ = N(q)⋅v
 
using the left-invertible matrix N(q) between the time derivative of generalized -coordinates and generalized velocities (see @ref quasi_coordinates). This yields +coordinates and generalized velocities. This yields the requisite form:
 ġₚ = ∂gₚ/∂q⋅N(q)⋅v
 
Fortunately, adding new constraints defined in the form `gₚ(t,q)` does not require considering this distinction using the operator paradigm (see, e.g., -`N_mult` in @ref drake::multibody::constraint::ConstraintAccelProblemData -"ConstraintAccelProblemData" and -@ref drake::multibody::constraint::ConstraintVelProblemData -"ConstraintVelProblemData"). Since the Jacobians are described -completely by the equation `ġₚ = ∂gₚ/∂q⋅N(q)⋅v + ∂c/∂t`, one can simply -evaluate `ġₚ - ∂g/∂t` for a given `v`; no Jacobian matrix need be formed -explicitly. -*/ - -/** @defgroup contact_surface_constraints Contact surface constraints -@ingroup constraint_overview +`N_mult` in `ConstraintAccelProblemData` and `ConstraintVelProblemData`). Since +the Jacobians are described completely by the equation `ġₚ = ∂gₚ/∂q⋅N(q)⋅v + +∂c/∂t`, one can simply evaluate `ġₚ - ∂g/∂t` for a given `v`; no Jacobian +matrix need be formed explicitly. + + +## Contact surface constraints Consider two points pᵢ and pⱼ on rigid bodies i and j, respectively, and assume that at a certain configuration of the two bodies, ᶜq, the two @@ -255,7 +215,10 @@ p(ᶜq). To constrain the motion of pᵢ and pⱼ to the contact surface as the bodies move, one can introduce the constraint g(q) = n(q)ᵀ(pᵢ(q) - pⱼ(q)), where n(q) is the common surface normal expressed in the world frame. gₚ(q) is a unilateral constraint, meaning that -complementarity constraints are necessary (see @ref constraint_types):
+complementarity constraints are necessary (see [Constraint
+types](#constraint_types)):
+
+
 0 ≤ gₚ  ⊥  λ ≥ 0
 
@@ -275,15 +238,24 @@ keeps the contact force along the contact normal compressive, as is consistent with a non-adhesive contact model. A more substantial discussion on the kinematics of contact can be found in -@ref Pfeiffer1996 "[Pfeiffer 1996]", Ch. 4. +[[Pfeiffer 1996]](#Pfeiffer1996), Ch. 4. -@image html drake/multibody/constraint/images/colliding-boxes.png "Figure 1: Illustration of the interpretation of non-interpenetration constraints when two boxes are interpenetrating (right). The boxes prior to contact are shown at left, and are shown in the middle figure at the initial time of contact; the surface normal n̂ is shown in this figure as well. The bodies interpenetrate over time as the constraint becomes violated (e.g., by constraint drift). Nevertheless, n̂ is tracked over time from its initial direction (and definition relative to the blue body). σ represents the signed distance the bodies must be translated along n̂ so that they are osculating (kissing)." +![Figure 1](images/colliding-boxes.png) + +**Figure 1:** Illustration of the interpretation of non-interpenetration +constraints when two boxes are interpenetrating (right). The boxes prior to +contact are shown at left, and are shown in the middle figure at the initial +time of contact; the surface normal n̂ is shown in this figure as well. The +bodies interpenetrate over time as the constraint becomes violated (e.g., by +constraint drift). Nevertheless, n̂ is tracked over time from its initial +direction (and definition relative to the blue body). σ represents the signed +distance the bodies must be translated along n̂ so that they are osculating +(kissing).

Constraint softening

-As discussed in @ref discretization, the non-interpenetration -constraint can be softened by adding a term to the time derivative of the -equation: -
+
+The non-interpenetration constraint can be softened by adding a term to the time
+derivative of the equation:
 0 ≤ ġₚ  ⊥  λ ≥ 0
 
resulting in:
@@ -292,11 +264,13 @@ resulting in:
 for γ ≥ 0.
 
 

Constraint stabilization

-As discussed in @ref constraint_stabilization, the drift induced by solving an -Index-0 DAE or DCP (that has been reduced from an Index-3 DAE or DCP) can be -mitigated through one of several strategies, one of which is Baumgarte -Stabilization. The typical application of Baumgarte Stabilization will use the -second time derivative of the complementarity condition:
+
+As discussed in [Constraint stabilization](#constraint_stabilization), the drift
+induced by solving an Index-0 DAE or DCP (that has been reduced from an Index-3
+DAE or DCP) can be mitigated through one of several strategies, one of which is
+Baumgarte Stabilization. The typical application of Baumgarte Stabilization will
+use the second time derivative of the complementarity condition:
+
 0 ≤ g̈ₚ  ⊥  λ ≥ 0
 
Baumgarte Stabilization would be layered on top of this equation, resulting in: @@ -309,53 +283,84 @@ constraint problem data - Constraint regularization/softening is effected through `gammaN` - Constraint stabilization is effected through `kN` - - Note that *neither Baumgarte Stabilization nor constraint regularization/ - softening affects the definition of g(.)'s Jacobian* operators, `N_mult` and - either `N_minus_muQ_transpose_mult` (for - @ref drake::multibody::constraint::ConstraintAccelProblemData - "ConstraintAccelProblemData") or `N_transpose_mult` (for - @ref drake::multibody::constraint::ConstraintVelProblemData - "ConstraintVelProblemData") -*/ - -/** @defgroup discretization A stable discretization strategy -@ingroup constraint_overview + - Note that neither Baumgarte Stabilization nor constraint + regularization/softening affects the definition of g(.)'s Jacobian + operators. That is, `N_mult` and either `N_minus_muQ_transpose_mult` (for + `ConstraintAccelProblemData`) or `N_transpose_mult` (for + `ConstraintVelProblemData`) + +## Variable definitions + +- `nb` The number of bilateral constraint equations (nb ≥ 0) +- `nk` The number of edges in a polygonal approximation to a friction + cone (nk ≥ 4 for contacts between three-dimensional bodies, nk = 2 for + contacts between two-dimensional bodies). Note that nk = 2nr (where + nr is defined immediately below). +- `nr` *Half* the number of edges in a polygonal approximation to a + friction cone. (nr ≥ 2 for contacts between three-dimensional bodies, + nr = 1 for contacts between two-dimensional bodies). +- `nc` The number of contact surface constraint equations. +- `nck` The total number of edges in the polygonal approximations to the + nc friction cones corresponding to the nc point contacts. Note that + nck = 2ncr (where ncr is defined immediately below). +- `ncr` *Half* the total number of edges in the polygonal approximations to the + nc friction cones corresponding to the nc point contacts. +- `nv` The dimension of the system generalized velocity / force. +- `nq` The dimension of the system generalized coordinates. +- `v` The system's generalized velocity vector (of dimension nv), which is a + linear transformation of the time derivative of the system's + generalized coordinates. +- `q` The generalized coordinate vector of the system (of dimension nq). +- `t` The system time variable (a non-negative real number). +- `nu` The number of "generic" (non-contact related) unilateral constraint + equations. +- `α` A non-negative, real valued scalar used to correct the time derivative + of position constraint errors (i.e., "stabilize" the constraints) via + an error feedback process (Baumgarte Stabilization). +- `β` A non-negative, real valued scalar used to correct position constraint + errors via the same error feedback process (Baumgarte + Stabilization) that uses α. +- `γ` Non-negative, real valued scalar used to regularize constraints. + +## A stable discretization strategy To be written. Refer to https://github.com/RobotLocomotion/drake/pull/7055 for a preview. -*/ -/** @defgroup constraint_references References - @ingroup constraint_overview +## References - Sources referenced within the multibody constraint documentation. +Sources referenced within the multibody constraint documentation. - - @anchor Anitescu1997 [Anitescu 1997] M. Anitescu and F. Potra. Formulating +
    +
  • [Anitescu 1997] M. Anitescu and F. Potra. Formulating Dynamic Multi-Rigid-Body Contact Problems with Friction as Solvable Linear - Complementarity Problems. Nonlinear Dynamics, 14, pp. 231-247. 1997. - - @anchor Ascher1995 [Ascher 1995] U. Ascher, H. Chin, L. Petzold, and + Complementarity Problems. Nonlinear Dynamics, 14, pp. 231-247. 1997.
  • +
  • [Ascher 1995] U. Ascher, H. Chin, L. Petzold, and S. Reich. Stabilization of constrained mechanical systems with DAEs and - invariant manifolds. J. Mech. Struct. Machines, 23, pp. 135-158. 1995. - - @anchor Ascher1998 [Ascher 1998] U. Ascher and L. Petzold. Computer Methods + invariant manifolds. J. Mech. Struct. Machines, 23, pp. 135-158. 1995.
  • +
  • [Ascher 1998] U. Ascher and L. Petzold. Computer Methods for Ordinary Differential Equations and Differential Algebraic Equations. - SIAM, Philadelphia. 1998. - - @anchor Baumgarte1972 [Baumgarte 1972] J. Baumgarte. Stabilization of + SIAM, Philadelphia. 1998.
  • +
  • [Baumgarte 1972] J. Baumgarte. Stabilization of constraints and integrals of motion in dynamical systems. Comp. Math. Appl. - Mech. Engr., 1, pp. 1-16. 1972. - - @anchor Catto2011 [Catto 2011] E. Catto. Soft Constraints: Reinventing the - Spring. Game Developers Conference presentation, 2011. - - @anchor Cottle1992 [Cottle 1992] R. Cottle, J-S. Pang, and R. Stone. The - Linear Complementarity Problem. Academic Press, Boston. 1992. - - @anchor Hairer1996 [Hairer 1996] E. Hairer and G. Wanner. Solving ordinary + Mech. Engr., 1, pp. 1-16. 1972.
  • +
  • [Catto 2011] E. Catto. Soft Constraints: Reinventing the + Spring. Game Developers Conference presentation, 2011.
  • +
  • [Cottle 1992] R. Cottle, J-S. Pang, and R. Stone. The + Linear Complementarity Problem. Academic Press, Boston. 1992.
  • +
  • [Hairer 1996] E. Hairer and G. Wanner. Solving ordinary differential equations II: stiff and differential algebraic problems, 2nd ed. - Springer-Verlag, Berlin. 1996. - - @anchor Lacoursiere2007 [Lacoursiere 2007] C. Lacoursière. Ghosts and + Springer-Verlag, Berlin. 1996.
  • +
  • [Lacoursiere 2007] C. Lacoursière. Ghosts and Machines: Regularized Variational Methods for Interactive Simulations of - Multibodies with Dry Frictional Contacts. Umeå University. 2007. - - @anchor Pfeiffer1996 [Pfeiffer 1996] F. Pfeiffer and C. Glocker. Multibody - Dynamics with Unilateral Contacts, John Wiley & Sons, New York. 1996. - - @anchor Sciavicco2000 [Sciavicco 2000] L. Sciavicco and B. Siciliano. + Multibodies with Dry Frictional Contacts. Umeå University. 2007.
  • +
  • [Pfeiffer 1996] F. Pfeiffer and C. Glocker. Multibody + Dynamics with Unilateral Contacts, John Wiley & Sons, New York. 1996.
  • +
  • [Sciavicco 2000] L. Sciavicco and B. Siciliano. Modeling and Control of Robot Manipulators, 2nd ed. Springer-Verlag, London. - 2000. - */ + 2000.
  • +
+ + + diff --git a/multibody/constraint/constraint_problem_data.h b/examples/rod2d/constraint_problem_data.h similarity index 100% rename from multibody/constraint/constraint_problem_data.h rename to examples/rod2d/constraint_problem_data.h diff --git a/multibody/constraint/constraint_solver.cc b/examples/rod2d/constraint_solver.cc similarity index 75% rename from multibody/constraint/constraint_solver.cc rename to examples/rod2d/constraint_solver.cc index 147e08977b9a..fd3cf1ea6bce 100644 --- a/multibody/constraint/constraint_solver.cc +++ b/examples/rod2d/constraint_solver.cc @@ -1,4 +1,4 @@ -#include "drake/multibody/constraint/constraint_solver.h" +#include "drake/examples/rod2d/constraint_solver.h" namespace drake { namespace multibody { diff --git a/multibody/constraint/constraint_solver.h b/examples/rod2d/constraint_solver.h similarity index 99% rename from multibody/constraint/constraint_solver.h rename to examples/rod2d/constraint_solver.h index 5cee4ab5d9c4..dcb4239d2b62 100644 --- a/multibody/constraint/constraint_solver.h +++ b/examples/rod2d/constraint_solver.h @@ -9,7 +9,7 @@ #include "drake/common/fmt_eigen.h" #include "drake/common/text_logging.h" -#include "drake/multibody/constraint/constraint_problem_data.h" +#include "drake/examples/rod2d/constraint_problem_data.h" #include "drake/solvers/moby_lcp_solver.h" namespace drake { diff --git a/multibody/constraint/images/colliding-boxes.png b/examples/rod2d/images/colliding-boxes.png similarity index 100% rename from multibody/constraint/images/colliding-boxes.png rename to examples/rod2d/images/colliding-boxes.png diff --git a/examples/rod2d/rod2d.h b/examples/rod2d/rod2d.h index 4454946705aa..614a6bdf46ae 100644 --- a/examples/rod2d/rod2d.h +++ b/examples/rod2d/rod2d.h @@ -4,9 +4,9 @@ #include #include +#include "drake/examples/rod2d/constraint_problem_data.h" +#include "drake/examples/rod2d/constraint_solver.h" #include "drake/examples/rod2d/rod2d_state_vector.h" -#include "drake/multibody/constraint/constraint_problem_data.h" -#include "drake/multibody/constraint/constraint_solver.h" #include "drake/solvers/moby_lcp_solver.h" #include "drake/systems/framework/leaf_system.h" diff --git a/multibody/constraint/test/constraint_solver_test.cc b/examples/rod2d/test/constraint_solver_test.cc similarity index 99% rename from multibody/constraint/test/constraint_solver_test.cc rename to examples/rod2d/test/constraint_solver_test.cc index e1bff7099210..4f57e69cee07 100644 --- a/multibody/constraint/test/constraint_solver_test.cc +++ b/examples/rod2d/test/constraint_solver_test.cc @@ -1,4 +1,4 @@ -#include "drake/multibody/constraint/constraint_solver.h" +#include "drake/examples/rod2d/constraint_solver.h" #include #include diff --git a/examples/rod2d/test/rod2d_test.cc b/examples/rod2d/test/rod2d_test.cc index a223a78444ef..0c1fecf52a07 100644 --- a/examples/rod2d/test/rod2d_test.cc +++ b/examples/rod2d/test/rod2d_test.cc @@ -6,8 +6,8 @@ #include "drake/common/test_utilities/eigen_matrix_compare.h" #include "drake/common/test_utilities/expect_no_throw.h" -#include "drake/multibody/constraint/constraint_problem_data.h" -#include "drake/multibody/constraint/constraint_solver.h" +#include "drake/examples/rod2d/constraint_problem_data.h" +#include "drake/examples/rod2d/constraint_solver.h" #include "drake/systems/analysis/simulator.h" using drake::multibody::constraint::ConstraintAccelProblemData; diff --git a/multibody/constraint/BUILD.bazel b/multibody/constraint/BUILD.bazel deleted file mode 100644 index 1d3bb3da9fb1..000000000000 --- a/multibody/constraint/BUILD.bazel +++ /dev/null @@ -1,55 +0,0 @@ -load("//tools/lint:lint.bzl", "add_lint_tests") -load( - "//tools/skylark:drake_cc.bzl", - "drake_cc_binary", - "drake_cc_googletest", - "drake_cc_library", - "drake_cc_package_library", -) - -package(default_visibility = ["//visibility:public"]) - -drake_cc_package_library( - name = "constraint", - visibility = ["//visibility:public"], - deps = [ - ":constraint_problem_data", - ":constraint_solver", - ], -) - -drake_cc_library( - name = "constraint_problem_data", - hdrs = [ - "constraint_problem_data.h", - ], - deps = [ - "//common:essential", - ], -) - -drake_cc_library( - name = "constraint_solver", - srcs = ["constraint_solver.cc"], - hdrs = [ - "constraint_solver.h", - ], - deps = [ - ":constraint_problem_data", - "//solvers:moby_lcp_solver", - ], -) - -# === test/ === - -drake_cc_googletest( - name = "constraint_solver_test", - deps = [ - ":constraint_solver", - "//examples/rod2d", - "//solvers:moby_lcp_solver", - "//solvers:unrevised_lemke_solver", - ], -) - -add_lint_tests(enable_clang_format_lint = False) diff --git a/tools/install/libdrake/build_components.bzl b/tools/install/libdrake/build_components.bzl index 7ea56c2abfff..044b733ac254 100644 --- a/tools/install/libdrake/build_components.bzl +++ b/tools/install/libdrake/build_components.bzl @@ -57,7 +57,6 @@ LIBDRAKE_COMPONENTS = [ "//multibody/benchmarks/kuka_iiwa_robot", "//multibody/benchmarks/mass_damper_spring", "//multibody/benchmarks/pendulum", - "//multibody/constraint", "//multibody/contact_solvers", "//multibody/contact_solvers/sap", "//multibody/fem",