Four Bar Linkage example

examples/multibody/four_bar/BUILD.bazel

@@ -0,0 +1,39 @@
+# -*- python -*-
+# This file contains rules for Bazel; see drake/doc/bazel.rst.
+
+load(
+    "@drake//tools/skylark:drake_cc.bzl",
+    "drake_cc_binary",
+)
+load("//tools/lint:lint.bzl", "add_lint_tests")
+
+filegroup(
+    name = "models",
+    srcs = glob([
+        "**/*.sdf",
+    ]),
+)
+
+drake_cc_binary(
+    name = "passive_simulation",
+    srcs = ["passive_simulation.cc"],
+    add_test_rule = 1,
+    data = [":models"],
+    test_rule_args = [
+        "--simulation_time=0.1",
+    ],
+    deps = [
+        "//common:find_resource",
+        "//geometry:geometry_visualization",
+        "//multibody/parsing",
+        "//multibody/plant",
+        "//multibody/tree",
+        "//systems/analysis:simulator",
+        "//systems/analysis:simulator_gflags",
+        "//systems/analysis:simulator_print_stats",
+        "//systems/framework:diagram",
+        "@gflags",
+    ],
+)
+
+add_lint_tests()

examples/multibody/four_bar/README.md

@@ -0,0 +1,199 @@
+# Four-Bar Linkage Example
+This planar four-bar linkage demonstrates how to use a bushing to
+approximate a closed kinematic chain. It loads an SDF model from the
+file "four_bar.sdf" into MultiBodyPlant. It handles the closed kinematic
+chain by replacing one of the four-bar's revolute (pin) joints with a
+bushing ([drake::multibody::LinearBushingRollPitchYaw](https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_linear_bushing_roll_pitch_yaw.html))
+whose force stiffness and damping values were approximated as discussed below.
+An alternative way to close this four-bar's kinematic chain is to "cut"
+one of the four-bar's rigid links in half and join those halves with a
+bushing that has both force and torque stiffness/damping. Note: the links
+in this example are constrained to rigid motion in the world X-Z
+plane (bushing X-Y plane) by the 3 revolute joints specified in the
+SDF. Therefore it is not necessary for the bushing to have force
+stiffness/damping along the joint axis.
+
+To run with default flags:
+
+```
+bazel run //examples/multibody/four_bar:passive_simulation
+```
+
+You should see the four-bar model oscillating passively with a small initial
+velocity.
+
+To change the initial velocity of `joint_WA`, q̇A in radians/second :
+```
+bazel run //examples/multibody/four_bar:passive_simulation -- --initial_velocity=<desired_velocity>
+```
+
+You can also apply a constant torque, 𝐓ᴀ, to `joint_WA` with a command line
+argument:
+```
+bazel run //examples/multibody/four_bar:passive_simulation -- --applied_torque=<desired_torque>
+```
+The torque is applied constantly to the joint actuator with no feedback. Thus,
+ if set high enough, you will see the system become unstable. 
+
+You can change the bushing parameters from the command line to observe their
+effect on
+the modeled joint. For instance, change `force_stiffness` to 300:
+ ```
+bazel run //examples/multibody/four_bar:passive_simulation -- --force_stiffness=300
+```
+And observe a gradual displacement between link *B* and link *C*.
+
+Change `force_damping` to 0:
+ ```
+bazel run //examples/multibody/four_bar:passive_simulation -- --force_damping=0
+```
+And observe the joint oscillating.
+
+Try setting `applied_torque` to 1000 and watch how the large forces interact
+with the bushing stiffness.
+
+
+## Four-bar linkage model
+
+The figure below shows a planar four-bar linkage consisting of 
+frictionless-pin-connected uniform rigid links *A, B, C* and ground-link *W*.
+- Link *A* connects to *W* and *B* at points *A*ₒ and *B*ₒ
+- Link *B* connects to *A* and *C* at points *B*ₒ and *B*c
+- Link *C* connects to *W* and *B* at points *C*ₒ and *C*ʙ
+
+Right-handed orthogonal unit vectors **Âᵢ B̂ᵢ Ĉᵢ Ŵᵢ** 
+(*i = x,y,z*) are fixed in *A, B, C, W,* with:
+- **Â**𝐱 directed from *A*ₒ to *B*ₒ
+- **B̂**𝐱 directed from *B*ₒ to *B*c
+- **Ĉ**𝐱 directed from *C*ₒ to *C*ʙ
+- **Ŵ**𝐳 vertically-upward.
+- **Â**𝐲 = **B̂**𝐲 = **Ĉ**𝐲 = **Ŵ**𝐲 parallel to pin axes
+
+| Diagram of the four bar model described above. |
+| :---: |
+| ![FourBarLinkageSchematic](images/FourBarLinkageSchematic.png)    |
+|  |
+
+|                 Quantity                   |       Symbol      |   Value   |
+|--------------------------------------------|-------------------|-----------|
+| Distance between *W*ₒ and *C*ₒ             |         𝐋ᴡ        |    2 m    |
+| Lengths of links *A, B, C*                 |        *L*        |    4 m    |
+| Masses of *A, B, C*                        |        *m*        |   20 kg   |
+| Earth’s gravitational acceleration         |        *g*        | 9.8 m/s²  |
+|                                            |                   |           |
+| **Ŵ**𝐲 measure of motor torque on *A*      |         𝐓ᴀ        | Specified |
+| Angle from **Ŵ**𝐱 to **Â**𝐱 with a +**Ŵ**𝐲 sense |    𝐪ᴀ    | Variable |
+| Angle from **Â**𝐱 to **B̂**𝐱 with a +**Â**𝐲 sense |    𝐪ʙ    | Variable |
+| Angle from **Ŵ**𝐱 to **Ĉ**𝐱 with a +**Ŵ**𝐲 sense |    𝐪ᴄ    | Variable |
+|                                            |               |           |
+| "Coupler-point" *P*'s position from *B*ₒ   |     2 **B̂**𝐱 - 2 **B̂**𝐳 |
+
+With 𝐓ᴀ = 0, the equilibrium values for the angles are:
+𝐪ᴀ ≈ 75.52°, 𝐪ʙ ≈ 104.48°, 𝐪ᴄ ≈ 104.48°.
+
+## Starting Configuration
+
+The SDF defines all of the links with their x axes parallel to the world x
+axis for convenience of measuring the angles in the state of the system
+with respect to a fixed axis. Below we derive a valid initial configuration
+of the three angles 𝐪ᴀ, 𝐪ʙ, and 𝐪ᴄ.
+
+| Derivation of starting configuration |
+| :---: |
+| ![FourBarLinkageSchematic](images/FourBarLinkageGeometry.png)    |
+| |
+
+Due to equal link lengths, the initial condition (static equilibrium) 
+forms an isosceles trapezoid and initial values can be determined from
+trigonometry. 𝐪ᴀ is one angle of a right triangle with its adjacent
+side measuring 1 m and its hypotenuse measuring 4 m.  Hence, initially
+𝐪ᴀ = tan⁻¹(√15) ≈ 1.318 radians ≈ 75.52°.
+
+Because link *B* is parallel to **Ŵ**𝐱, 𝐪ᴀ and 𝐪ʙ are supplementary,
+hence the initial value is 𝐪ʙ = π - 𝐪ᴀ ≈ 1.823 radians ≈  104.48°.
+Similarly, 𝐪ᴀ and 𝐪ᴄ are supplementary, so initially 𝐪ᴄ = 𝐪ʙ. 
+
+# Modeling the revolute joint between links B and C with a bushing
+
+In this example, we replace the pin joint at point **Bc** (see diagram)
+that connects links *B* and *C* with a
+[drake::multibody::LinearBushingRollPitchYaw](https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_linear_bushing_roll_pitch_yaw.html)
+(there are many other uses of a bushing).  We model a z-axis revolute joint by
+setting torque stiffness constant k₂ = 0 and  torque damping constant d₂ = 0.
+We chose the z-axis (Yaw) to avoid a singularity associated with "gimbal lock".
+Two frames (one attached to *B* called `Bc_Bushing` with origin at point
+**Bc** and one attached to *C* called `Cb_Bushing` with origin at point
+**Cb**) are oriented so their z-axes are perpedicular to the planar
+four-bar linkage.
+
+## Estimating bushing parameters
+Joints are normally modeled with hard constraints except in their motion
+direction, and three of the four revolute joints here are indeed modeled
+that way. However, in order to close the kinematic loop we have to use a
+bushing as a "penalty method" substitute for hard constraints. That is, because
+the bushing is compliant it will violate the constraint to some degree. The
+stiffer we make it, the more precisely it will enforce the constraint but
+the more difficult the problem will be to solve numerically. We want to
+choose stiffness k and damping d for the bushing to balance those
+considerations. First, consider your tolerance for constraint errors -- if
+the joint allows deviations of 1mm (say) would that be OK for your application?
+Similarly, would angular errors of 1 degree (say) be tolerable? We will give
+a procedure below for estimating a reasonable value of k to achieve a
+specified translational and rotational tolerance. Also, we need to choose
+d to damp out oscillations caused by the stiff spring in a "reasonable" time.
+Consider a time scale you would consider negligible. Perhaps a settling time
+of 1ms (say) would be ignorable for your robot arm, which presumably has
+much larger time constants for important behaviors. We will give a
+procedure here for obtaining a reasonable d from k and your settling
+time tolerance. For a more detailed discussion on choosing bushing parameters
+for a variety of its uses, see [drake::multibody::LinearBushingRollPitchYaw](https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_linear_bushing_roll_pitch_yaw.html).
+
+### Estimate force stiffness [kx ky kz] from loading/displacement
+The bushing's force stiffness constants [kx ky kz] can be
+approximated via various methods (or a combination thereof).
+For example, one could specify a maximum bushing displacement in a
+direction (e.g.,  xₘₐₓ), estimate a maximum directional load (Fx) that
+combines gravity forces, applied forces, inertia forces (centripetal,
+Coriolus, gyroscopic), and then calculate kx ≈ Fx /  xₘₐₓ.
+
+### Estimate force stiffness [kx ky kz] constants from mass and ωₙ
+The bushing's force stiffness constants [kx ky kz] can be
+approximated via a related linear constant-coefficient 2ⁿᵈ-order ODE:
+
+|  |  |
+| ----- | ---- |
+|  m ẍ +     dₓ ẋ +  kₓ x = 0  |  or alternatively as |
+|    ẍ + 2 ζ ωₙ ẋ + ωₙ² x = 0  |  where ωₙ = √(kₓ/m),  ζ = dx / (2 √(m kₓ)) |
+
+Values for kₓ can be determined by choosing a characteristic mass m
+(which may be directionally dependent) and then choosing ωₙ > 0
+(speed of response). Rearranging ωₙ = √(kₓ/m) produces kₓ = m ωₙ².
+One way to choose ωₙ is to choose a settling time tₛ which
+approximates the desired time for stretch x to settle to within 1% (0.01)
+of an equilibrium solution, and choose a damping ratio ζ (e.g., ζ = 1,
+critical damping), then calculate ωₙ = -ln(0.01) / (ζ tₛ) ≈ 4.6 / (ζ tₛ).
+For the included example code, a characteristic mass m = 20 kg was chosen
+with tₛ = 0.12 and ζ = 1 (critical damping). Thus
+ωₙ = -ln(0.01) / 0.12 ≈ 38.38 and kₓ = (20)*(38.38)² ≈ 30000.
+
+### Estimate force damping [dx dy dz] from mass and stiffness 
+Once m and kₓ have been chosen, damping dₓ can be estimated by picking a
+damping ratio ζ (e.g., ζ ≈ 1, critical damping), then dₓ ≈ 2 ζ √(m kx).
+For our example dₓ ≈ 2·√(20·30000) ≈ 1500.
+
+### Estimating torque stiffness [k₀ k₁ k₂] and damping [d₀ d₁ d₂]
+The bushing in this planar example replaces a revolute joint. The links are
+constrained to planar motion by the existing joints, hence no
+torque stiffness nor torque damping is needed.  An alternative way to
+deal with this four-bar's closed kinematic loop is to "cut" one of the
+four-bar's rigid links in half and join those halves with a bushing
+that has both force and torque stiffness/damping.  If this technique
+is used, torque stiffness is needed.  One way to approximate torque
+stiffness is with concepts similar to the force stiffness above.
+For example, the bushing's torque stiffness k₀ could be calculated
+by specifing a maximum bushing angular displacement θₘₐₓ, estimating
+a maximum moment load Mx and calculating k₀ = Mx / θₘₐₓ.
+Alternatively, a value for k₀ can be determined by choosing a
+characteristic moment of inertia I₀ (which is directionally dependent)
+and then choosing ωₙ (e.g., from setting time), then using k₀ ≈ I₀ ωₙ².
+With k₀ available and a damping ratio ζ chosen, d₀ ≈ 2 ζ √(I₀ k₀).