|
Drake
|
|
Drake
|
TamsiSolver uses the Transition-Aware Modified Semi-Implicit (TAMSI) method, [Castro et al., 2019], to solve the equations below for mechanical systems in contact with regularized friction:
q̇ = N(q) v (1) M(q) v̇ = τ + Jₙᵀ(q) fₙ(q, v) + Jₜᵀ(q) fₜ(q, v)
where v ∈ ℝⁿᵛ is the vector of generalized velocities, M(q) ∈ ℝⁿᵛˣⁿᵛ is the mass matrix, Jₙ(q) ∈ ℝⁿᶜˣⁿᵛ is the Jacobian of normal separation velocities, Jₜ(q) ∈ ℝ²ⁿᶜˣⁿᵛ is the Jacobian of tangent velocities, fₙ ∈ ℝⁿᶜ is the vector of normal contact forces, fₜ ∈ ℝ²ⁿᶜ is the vector of tangent friction forces and τ ∈ ℝⁿᵛ is a vector of generalized forces containing all other applied forces (e.g., Coriolis, gyroscopic terms, actuator forces, etc.) but contact forces. This solver assumes a compliant law for the normal forces fₙ(q, v) and therefore the functional dependence of fₙ(q, v) with q and v is stated explicitly.
Since TamsiSolver uses regularized friction, we explicitly emphasize the functional dependence of fₜ(q, v) with the generalized velocities. The functional dependence of fₜ(q, v) with the generalized positions stems from its direct dependence with the normal forces fₙ(q, v).
TamsiSolver implements two different schemes. A "one-way coupling scheme" which solves for the friction forces given the normal forces are known. That is, normal forces affect the computation of the friction forces however, the normal forces are kept constant during the solution procedure.
A "two-way coupling scheme" treats both the normal and friction forces implicitly in the generalized velocities resulting in a numerical strategy in which normal forces affect friction forces and, conversely, friction forces couple back to the computation of the normal forces.
The two-way coupled scheme provides a more stable and therefore robust solution to the problem stated in Eq. (1) with just a small increase of the computational cost compared to the one-way coupled scheme. The one-way coupled scheme is however very useful for testing and analysis of the solver.
Equation (1) is discretized in time using a variation of the first order semi-implicit Euler scheme from step s to step s+1 with time step δt as:
qˢ⁺¹ = qˢ + δt N(qˢ) vˢ⁺¹
(2) M(qˢ) vˢ⁺¹ =
M(qˢ) vˢ + δt [τˢ + Jₙᵀ(qˢ) fₙ(qˢ,vˢ) + Jₜᵀ(qˢ) fₜ(qˢ,vˢ⁺¹)]
(We are using s for step counter rather than n to avoid Unicode-induced confusion with the "normal direction" subscript n.)
Please see details in the Derivation of the one-way coupling scheme section. The equation for the generalized velocities in Eq. (2) is rewritten as:
(3) M vˢ⁺¹ = p* + δt [Jₙᵀ fₙ + Jₜᵀ fₜ(vˢ⁺¹)]
where p* = M vˢ + δt τˢ is the generalized momentum that the system would have in the absence of contact forces and, for simplicity, we have only kept the functional dependencies in generalized velocities. Notice that TamsiSolver uses a precomputed value of the normal forces. These normal forces could be available for instance if using a compliant contact approach, for which normal forces are a function of the state.
Equation (1) is discretized in time using a variation on the semi-implicit Euler scheme with time step δt as:
qˢ⁺¹ = qˢ + δt N(qˢ) vˢ⁺¹
(4) M(qˢ) vˢ⁺¹ = M(qˢ) vˢ +
δt [τˢ + Jₙᵀ(qˢ) fₙˢ⁺¹ + Jₜᵀ(qˢ) fₜ(fₙˢ⁺¹,vₜˢ⁺¹)]
This implicit scheme variation evaluates Jacobian matrices Jₙ and Jₜ as well as the kinematic mapping N(q) at the previous time step. In Eq. (4) we have fₙˢ⁺¹ = fₙ(xˢ⁺¹, vₙˢ⁺¹) with xˢ⁺¹ = x(qˢ⁺¹), the signed penetration distance (defined positive when bodies overlap) between contact point pairs and the separation velocities vₙˢ⁺¹ = Jₙ(qˢ) vˢ⁺¹ (= −ẋˢ⁺¹). Also the functional dependence of fₜ with fₙ and vₜ is highlighted in Eq. (4). More precisely, the two-way coupling scheme uses a normal force law for each contact pair of the form:
(5) fₙ(x, vₙ) = k(vₙ)₊ x₊ (6) k(vₙ) = k (1 − d vₙ)₊
where x₊ is the positive part of x (x₊ = x if x ≥ 0 and x₊ = 0 otherwise) and k and d are the stiffness and dissipation coefficients for a given contact point, respectively.
The two-way coupling scheme uses a first order approximation to the signed distance functions vector:
(7) xˢ⁺¹ ≈ xˢ − δt vₙˢ⁺¹ = xˢ − δt Jₙ(qˢ) vˢ⁺¹
where the minus sign is needed given that ẋ = dx/dt = −vₙ. This approximation is used in Eq. (5) to obtain a numerical scheme that implicitly couples normal forces through their functional dependence on the signed penetration distance. Notice that, according to Eq. (5), normal forces at each contact point are decoupled from each other. However their values are coupled given the choice of a common variable, the generalized velocity v.
Equation (7) is used into Eq. (5) to obtain an expression of the normal force in terms of the generalized velocity vˢ⁺¹ at the next time step:
(8) fₙ(xˢ⁺¹, vₙˢ⁺¹) = k (1 − d vₙˢ⁺¹)₊ xˢ⁺¹₊
= k (1 − d Jₙ(qˢ) vˢ⁺¹)₊ (xˢ − δt Jₙ(qˢ) vˢ⁺¹)₊
= fₙ(vˢ⁺¹)
Similarly, the friction forces fₜ can be written in terms of the next time step generalized velocities using vₜˢ⁺¹ = Jₜ(qˢ) vˢ⁺¹ and using Eq. (8) to substitute an expression for the normal force in terms of vˢ⁺¹. This allows to re-write the tangential forces as a function of the generalized velocities as:
(9) fₜ(fₙˢ⁺¹, vₜˢ⁺¹) = fₜ(fₙ(x(vˢ⁺¹), vₙ(vˢ⁺¹)), vₜ((vˢ⁺¹)))
= fₜ(vˢ⁺¹)
where we write x(vˢ⁺¹) = xˢ − δt Jₙ(qˢ) vˢ⁺¹. Finally, Eqs. (8) and (9) are used into Eq. (4) to obtain an expression in vˢ⁺¹:
(10) M(qˢ) vˢ⁺¹ = p* + δt [Jₙᵀ(qˢ) fₙ(vˢ⁺¹) + Jₜᵀ(qˢ) fₜ(vˢ⁺¹)]
with p* = p* = M vˢ + δt τˢ the generalized momentum that the system would have in the absence of contact forces.
TamsiSolver uses a Newton-Raphson strategy to solve Eq. (10) in the generalized velocities, limiting the iteration update with the scheme described in iteration_limiter.
TamsiSolver solves for the generalized velocity at the next time step vˢ⁺¹ with either a one-way or two-way coupled scheme as described in the previous sections. The solver uses a Newton-Raphson iteration to compute an update Δvᵏ at the k-th Newton-Raphson iteration. Once Δvᵏ is computed, the solver limits the change in the tangential velocities Δvₜᵏ = Jₜᵀ Δvᵏ using the approach described in [Uchida et al., 2015]. This approach limits the maximum angle change θ between two successive iterations in the tangential velocity. Details of our implementation are provided in [Castro et al., 2019].
In this section we provide a detailed derivation of the first order time stepping approach in Eq. (2). We start from the continuous Eq. (1):
(1) M(q) v̇ = τ + Jₙᵀ(q) fₙ(q,v) + Jₜᵀ(q) fₜ(q,v)
we can discretize Eq. (1) in time using a first order semi-implicit Euler scheme in velocities:
(11) M(qˢ) vˢ⁺¹ = M(qˢ) vˢ +
δt [τˢ⁺¹ + Jₙᵀ(qˢ) fₙ(qˢ,vˢ⁺¹) + Jₜᵀ(qˢ) fₜ(vˢ⁺¹)] + O₁(δt²)
where the equality holds strictly since we included the leading terms in O(δt²). We use τˢ⁺¹ = τ(tˢ, qˢ, vˢ⁺¹) for brevity in Eq. (11). When moving from the continuous Eq. (1) to the discrete version Eq. (11), we lost the nice property that our compliant normal forces are decoupled from the friction forces (both depend on the same unknown vˢ⁺¹ in Eq (11)). The reason is that Eq. (11) includes an integration over a small interval of size δt. To solve the discrete system in Eq. (11), we'd like to decouple the normal forces from the tangential forces again, which will require a new (though still valid) approximation. To do so we will expand in Taylor series the term fₙ(qˢ, vˢ⁺¹):
(12) fₙ(qˢ, vˢ⁺¹) = fₙ(qˢ,vˢ) + ∇ᵥfₙ(qˢ,vˢ) (vˢ⁺¹ − vˢ) + O₂(‖vˢ⁺¹ − vˢ‖²)
The difference between vˢ and vˢ⁺¹ can be written as:
(13) vˢ⁺¹ − vˢ = δtv̇ˢ + δtO₃(δt²) = O₄(δt)
Substituting vˢ⁺¹ − vˢ from Eq. (13) into Eq. (12) we arrive to:
(14) fₙ(qˢ, vˢ⁺¹) = fₙ(qˢ,vˢ) + ∇ᵥfₙ(qˢ,vˢ) O₄(δt) + O₅(δt²)
= fₙ(qˢ,vˢ) + O₆(δt)
where O₅(δt²) = O₂(‖vˢ⁺¹ − vˢ‖²) = O₂(‖O₄(δt)‖²). A similar argument for τˢ⁺¹ shows it also differs in O(δt) from τˢ = τ(tˢ, qˢ, vˢ). We can now use Eq. (14) into Eq. (11) to arrive to:
(15) M(qˢ) vˢ⁺¹ = M(qˢ) vˢ +
δt [τˢ + Jₙᵀ(qˢ) (fₙ(qˢ,vˢ) + O₆(δt)) + Jₜᵀ(qˢ) fₜ(vˢ⁺¹)] +
O₁(δt²)
which we can rewrite as:
(16) M(qˢ) vˢ⁺¹ = M(qˢ) vˢ +
δt [τˢ + Jₙᵀ(qˢ) fₙ(qˢ, vˢ) + Jₜᵀ(qˢ) fₜ(vˢ⁺¹)] + O₇(δt²)
with O₇(δt²) = δt Jₙᵀ(qˢ) O₆(δt) + O₁(δt²). That is, Eq. (16) introduces the same order of approximation as in the semi-implicit method in Eq. (11). Up to this point we have made no approximations but we instead propagated the O(⋅) leading terms. Therefore the equalities in the equations above are exact. To obtain an approximate time stepping scheme, we drop O₇(δt²) (we neglect it) in Eq. (16) to arrive to a first order scheme:
(17) M(qˢ) vˢ⁺¹ = M(qˢ) vˢ +
δt [τˢ + Jₙᵀ(qˢ) fₙ(qˢ,vˢ) + Jₜᵀ(qˢ) fₜ(vˢ⁺¹)]
Therefore, with the scheme in Eq. (17) we are able to decouple the computation of (compliant) normal forces from that of friction forces. A very important feature of this scheme however, is the explicit nature (in the velocities v) of the term associated with the normal forces (usually including dissipation in the normal direction), which will become unstable for a sufficiently large time step. However, for most applications in practice, the stability of the scheme is mostly determined by the explicit update of normal forces with positions, that is, Eq. (17) is explicit in positions through the normal forces fₙ(qˢ, vˢ). For many common applications, the explicit dependence of τˢ(tˢ, qˢ, vˢ) on the previous time step velocities vˢ determines the overall stability of the scheme, since this term can include velocity dependent contributions such as control forces and dampers. Notice that Eq. (12) introduces an expansion of fₙ with an order of approximation consistent with the first order scheme as needed. Therefore, it propagates into a O(δt²) term exactly as needed in Eq. (16).
| T | The scalar type, which must be one of the default scalars. |
#include <drake/multibody/plant/tamsi_solver.h>
Public Member Functions | |
| TamsiSolver (int nv) | |
Instantiates a solver for a problem with nv generalized velocities. More... | |
| ~TamsiSolver () | |
| void | ResizeIfNeeded (int nv) const |
Change the working size of the solver to use nv generalized velocities. More... | |
| std::unique_ptr< TamsiSolver< T > > | Clone () const |
| void | SetOneWayCoupledProblemData (EigenPtr< const MatrixX< T >> M, EigenPtr< const MatrixX< T >> Jn, EigenPtr< const MatrixX< T >> Jt, EigenPtr< const VectorX< T >> p_star, EigenPtr< const VectorX< T >> fn, EigenPtr< const VectorX< T >> mu) |
| Sets data for the problem to be solved as outlined by Eq. More... | |
| void | SetTwoWayCoupledProblemData (EigenPtr< const MatrixX< T >> M, EigenPtr< const MatrixX< T >> Jn, EigenPtr< const MatrixX< T >> Jt, EigenPtr< const VectorX< T >> p_star, EigenPtr< const VectorX< T >> fn0, EigenPtr< const VectorX< T >> stiffness, EigenPtr< const VectorX< T >> dissipation, EigenPtr< const VectorX< T >> mu) |
| Sets the problem data to solve the problem outlined in Eq. More... | |
| TamsiSolverResult | SolveWithGuess (double dt, const VectorX< T > &v_guess) const |
Given an initial guess v_guess, this method uses a Newton-Raphson iteration to find a solution for the generalized velocities satisfying either Eq. More... | |
| const TamsiSolverIterationStats & | get_iteration_statistics () const |
| Returns statistics recorded during the last call to SolveWithGuess(). More... | |
| const TamsiSolverParameters & | get_solver_parameters () const |
| Returns the current set of parameters controlling the iteration process. More... | |
| void | set_solver_parameters (const TamsiSolverParameters ¶meters) |
| Sets the parameters to be used by the solver. More... | |
Does not allow copy, move, or assignment | |
| TamsiSolver (const TamsiSolver &)=delete | |
| TamsiSolver & | operator= (const TamsiSolver &)=delete |
| TamsiSolver (TamsiSolver &&)=delete | |
| TamsiSolver & | operator= (TamsiSolver &&)=delete |
Retrieving the solution | |
This methods allow to retrieve the solution stored in the solver after the last call to SolveWithGuess(). | |
| const VectorX< T > & | get_generalized_friction_forces () const |
| Returns a constant reference to the most recent vector of generalized friction forces. More... | |
| Eigen::VectorBlock< const VectorX< T > > | get_normal_velocities () const |
| Returns a constant reference to the most recent solution vector for normal separation velocities. More... | |
| const VectorX< T > & | get_generalized_contact_forces () const |
| Returns a constant reference to the most recent vector of generalized contact forces, including both friction and normal forces. More... | |
| Eigen::VectorBlock< const VectorX< T > > | get_tangential_velocities () const |
| Returns a constant reference to the most recent vector of tangential forces. More... | |
| const VectorX< T > & | get_generalized_velocities () const |
| Returns a constant reference to the most recent vector of generalized velocities. More... | |
| Eigen::VectorBlock< const VectorX< T > > | get_normal_forces () const |
| Returns a constant reference to the most recent vector of (repulsive) forces in the normal direction. More... | |
| Eigen::VectorBlock< const VectorX< T > > | get_friction_forces () const |
| Returns a constant reference to the most recent vector of friction forces. More... | |
Friends | |
| class | TamsiSolverTester |
|
delete |
|
delete |
|
explicit |
Instantiates a solver for a problem with nv generalized velocities.
| std::exception | if nv is non-positive. |
| ~TamsiSolver | ( | ) |
| std::unique_ptr<TamsiSolver<T> > Clone | ( | ) | const |
| Eigen::VectorBlock<const VectorX<T> > get_friction_forces | ( | ) | const |
Returns a constant reference to the most recent vector of friction forces.
These friction forces are defined in accordance to the tangential velocities Jacobian Jₜ as documented in this class's documentation.
| const VectorX<T>& get_generalized_contact_forces | ( | ) | const |
Returns a constant reference to the most recent vector of generalized contact forces, including both friction and normal forces.
| const VectorX<T>& get_generalized_friction_forces | ( | ) | const |
Returns a constant reference to the most recent vector of generalized friction forces.
| const VectorX<T>& get_generalized_velocities | ( | ) | const |
Returns a constant reference to the most recent vector of generalized velocities.
| const TamsiSolverIterationStats& get_iteration_statistics | ( | ) | const |
Returns statistics recorded during the last call to SolveWithGuess().
See IterationStats for details.
| Eigen::VectorBlock<const VectorX<T> > get_normal_forces | ( | ) | const |
Returns a constant reference to the most recent vector of (repulsive) forces in the normal direction.
That is, the normal force is positive when the bodies push each other apart. Otherwise the normal force is zero, since contact forces can only be repulsive.
| Eigen::VectorBlock<const VectorX<T> > get_normal_velocities | ( | ) | const |
Returns a constant reference to the most recent solution vector for normal separation velocities.
This method returns an Eigen::VectorBlock referencing a vector of size nc.
| const TamsiSolverParameters& get_solver_parameters | ( | ) | const |
Returns the current set of parameters controlling the iteration process.
See Parameters for details.
| Eigen::VectorBlock<const VectorX<T> > get_tangential_velocities | ( | ) | const |
Returns a constant reference to the most recent vector of tangential forces.
This method returns an Eigen::VectorBlock referencing a vector of size nc.
|
delete |
|
delete |
| void ResizeIfNeeded | ( | int | nv | ) | const |
Change the working size of the solver to use nv generalized velocities.
This can be used to either shrink or grow the workspaces.
| std::exception | if nv is non-positive. |
| void set_solver_parameters | ( | const TamsiSolverParameters & | parameters | ) |
Sets the parameters to be used by the solver.
See Parameters for details.
| void SetOneWayCoupledProblemData | ( | EigenPtr< const MatrixX< T >> | M, |
| EigenPtr< const MatrixX< T >> | Jn, | ||
| EigenPtr< const MatrixX< T >> | Jt, | ||
| EigenPtr< const VectorX< T >> | p_star, | ||
| EigenPtr< const VectorX< T >> | fn, | ||
| EigenPtr< const VectorX< T >> | mu | ||
| ) |
Sets data for the problem to be solved as outlined by Eq.
(3) in this class's documentation:
(3) M v = p* + δt Jₙᵀ fₙ + δt Jₜᵀ fₜ(v)
Refer to this class's documentation for further details on the structure of the problem and the solution strategy. In the documented parameters below, nv is the number of generalized velocities and nc is the number of contact points.
| [in] | M | The mass matrix of the system, of size nv x nv. |
| [in] | Jn | The normal separation velocities Jacobian, of size nc x nv. |
| [in] | Jt | The tangential velocities Jacobian, of size 2nc x nv. |
| [in] | p_star | The generalized momentum the system would have at s + 1 if contact forces were zero. |
| [in] | fn | A vector of size nc containing the normal force at each contact point. |
| [in] | mu | A vector of size nc containing the friction coefficient at each contact point. The solver makes no distinction between static and dynamic coefficients of friction or, similarly, the solver assumes the static and dynamic coefficients of friction are the same. |
| std::exception | if any of the data pointers are nullptr. |
| std::exception | if the problem data sizes are not consistent as described above. |
| std::exception | if SetTwoWayCoupledProblemData() was ever called on this solver. |
| void SetTwoWayCoupledProblemData | ( | EigenPtr< const MatrixX< T >> | M, |
| EigenPtr< const MatrixX< T >> | Jn, | ||
| EigenPtr< const MatrixX< T >> | Jt, | ||
| EigenPtr< const VectorX< T >> | p_star, | ||
| EigenPtr< const VectorX< T >> | fn0, | ||
| EigenPtr< const VectorX< T >> | stiffness, | ||
| EigenPtr< const VectorX< T >> | dissipation, | ||
| EigenPtr< const VectorX< T >> | mu | ||
| ) |
Sets the problem data to solve the problem outlined in Eq.
(10) in this class's documentation using a two-way coupled approach:
(10) M(qˢ) vˢ⁺¹ = p* + δt [Jₙᵀ(qˢ) fₙ(vˢ⁺¹) + Jₜᵀ(qˢ) fₜ(vˢ⁺¹)]
Refer to this class's documentation for further details on the structure of the problem and the solution strategy. In the documented parameters below, nv is the number of generalized velocities and nc is the number of contact points.
| [in] | M | The mass matrix of the system, of size nv x nv. |
| [in] | Jn | The normal separation velocities Jacobian, of size nc x nv. |
| [in] | Jt | The tangential velocities Jacobian, of size 2nc x nv. |
| [in] | p_star | The generalized momentum the system would have at n + 1 if contact forces were zero. |
| [in] | fn0 | Normal force at the previous time step. Always positive since bodies cannot attract each other. |
| [in] | stiffness | A vector of size nc storing at each ith entry the stiffness coefficient for the ith contact pair. |
| [in] | dissipation | A vector of size nc storing at each ith entry the dissipation coefficient for the ith contact pair. |
| [in] | mu | A vector of size nc containing the friction coefficient at each contact point. The solver makes no distinction between static and dynamic coefficients of friction or, similarly, the solver assumes the static and dynamic coefficients of friction are the same. |
| std::exception | if any of the data pointers are nullptr. |
| std::exception | if the problem data sizes are not consistent as described above. |
| std::exception | if SetOneWayCoupledProblemData() was ever called on this solver. |
| TamsiSolverResult SolveWithGuess | ( | double | dt, |
| const VectorX< T > & | v_guess | ||
| ) | const |
Given an initial guess v_guess, this method uses a Newton-Raphson iteration to find a solution for the generalized velocities satisfying either Eq.
(3) when one-way coupling is used or Eq. (10) when two-way coupling is used. See this class's documentation for further details. To retrieve the solution, please refer to retrieving_the_solution.
this solver accessors to retrieve the last computed solution. | [in] | dt | The time step used advance the solution in time. |
| [in] | v_guess | The initial guess used in by the Newton-Raphson iteration. Typically, the previous time step velocities. |
| std::exception | if v_guess is not of size nv, the number of generalized velocities specified at construction. |
|
friend |