Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Four Bar Linkage example #13036

Merged
merged 8 commits into from
Apr 23, 2020
Merged
Changes from 7 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
39 changes: 39 additions & 0 deletions examples/multibody/four_bar/BUILD.bazel
Original file line number Diff line number Diff line change
@@ -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()
199 changes: 199 additions & 0 deletions examples/multibody/four_bar/README.md
Original file line number Diff line number Diff line change
@@ -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*
- ****𝐱 directed from *B*ₒ to *B*c
- ****𝐱 directed from *C*ₒ to *C*ʙ
- ****𝐳 vertically-upward.
- ****𝐲 = ****𝐲 = ****𝐲 = ****𝐲 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 ****𝐱 with a +****𝐲 sense | 𝐪ʙ | Variable |
| Angle from ****𝐱 to ****𝐱 with a +****𝐲 sense | 𝐪ᴄ | Variable |
| | | |
| "Coupler-point" *P*'s position from *B*| 2 ****𝐱 - 2 ****𝐳 |

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 kx can be determined by choosing a characteristic mass m
(which may be directionally dependent) and then choosing ωₙ > 0
(speed of response). Rearranging ωₙ = √(kx/m) produces kx = 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 ωₙ = -log(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
ωₙ = -log(0.01) / 0.12 ≈ 38.38 and kx = (20)*(38.38)² ≈ 30000.

### Estimate force damping [dx dy dz] from mass and stiffness
Once m and kx have been chosen, damping dx can be estimated by picking a
damping ratio ζ (e.g., ζ ≈ 1, critical damping), then dx ≈ 2 ζ √(m kx).
For our example dx ≈ 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₀).
313 changes: 313 additions & 0 deletions examples/multibody/four_bar/four_bar.sdf
Original file line number Diff line number Diff line change
@@ -0,0 +1,313 @@
<sdf version='1.7'>
<model name='four_bar'>
<!--
This sdf file produces a model of a planar four bar linkage.
See the README for more details on the modelling done in this file.

We replace one of the 4 revolute joints with a model revolute joint
implemented with a LinearBushingRollPitchYaw ForceElement in the code.
This SDF provides the frames at which to place the above mentioned bushing.

Links A, B, C, and (the implicit) World link complete the closed kinematic
chain.

Links A, B, C have length 4 m and mass 20 kg and in 3D are modeled
by boxes with dimensions (4.2 m, 0.1 m, 0.2 m).

For each link we define a frame: A, B, C, and W.

Wz
^
|
Wx <- - - ·

Frame W is the implicit world link with Wo its origin, Wz pointing up, Wx
pointing left, and Wy pointing out of the page. Gravity points in the -Wz
direction.

In the zero configuration, frame A is coincident with frame W. In other
words X_WA is the identity.
The Ax axis points along the link.

Frame B is defined relative to A.
The origin of the B frame measured in A, p_AB, is (-4, 0, 0)
In the zero configuration, frame B's orientation measured in A, R_AB,
is the identity.
The Bx axis points along the link.

Frame C is defined relative to W.
The origin of the C frame measured in the world, p_WC, is (-2, 0, 0).
In the zero configuration, frame C's orientation measured in the world,
R_WC, is the identity.
The Cx axis points along the link.

In pseudo code, the frames in the zero configuration can be written:
X_WA = RigidTransformd::Identity();
X_AB = RigidTransformd(Vector3d(4, 0, 0));
X_WC = RigidTransformd(Vector3d(-2, 0, 0));

For each link, we also define a revolute joint, located at the
origin of that link's frame, oriented along the world's y-axis.

joint_WA is the y-axis revolute joint between the parent world and the child A.
joint_WA's joint angle is denoted q_WA.

joint_AB is the y-axis revolute joint between the parent A and the child B.
joint_AB's joint angle is denoted q_AB.

joint_WC is the y-axis revolute joint between the parent world and the child B.
joint_WC's joint angle is denoted q_WC.

There is no joint connecting link B to link C. We will model a
revolute joint between them with a LinearBushingRollPitchYaw ForceElement in
the code. For this we define two frames:
Bc_bushing is a frame attached to link B.
Cb_bushing is a frame attached to link C.
See their definition below for the details of how they are defined.

In the default configuration (i.e. q_WA = q_AB = q_WC = 0) all of the links
lie flat along the Wx axis. Note this is not a valid configuration that
satisfies loop closure. However, they are defined in such a way to make
this SDF readable and so at least q_WA and q_WC are measured relative to a
fixed axis of the world frame. Please see the README for a detailed
diagram with configuration angles that satisfy the loop closure equation.
-->
<link name='A'>
<!-- The frame A measured in the world frame, X_WA is the identity. This
link is also called 'Crank' in the diagram in README.md -->
<pose>0 0 0 0 0 0</pose>
<inertial>
<!-- Acm (A's center of mass) is located at the midpoint of the 4 meter long link. -->
<pose>2 0 0 0 0 0</pose>
<mass>20</mass>
<inertia>
<!-- Inertia tensor for a solid cuboid of dimensions
(l, w, h) = (4.2 m, 0.1 m, 0.2 m) and mass 20 kg-->
<ixx>0.08333333333333334</ixx>
<iyy>29.46666666666666</iyy>
<izz>29.416666666666668</izz>
<ixy>0</ixy><ixz>0</ixz><iyz>0</iyz>
</inertia>
</inertial>
<!-- A skinny red box along the Ax axis -->
<visual name='A_visual'>
<pose>2 0 0 0 0 0</pose>
<geometry>
<box>
<size>4.2 0.1 0.2</size>
</box>
</geometry>
<material>
<diffuse>1 0 0 1</diffuse>
</material>
</visual>
<!-- A visual representing the revolute joint between the world and
link A. -->
<visual name='A_pivot_visual'>
<!-- Rotation around Ax by 90° = π/2 radians ≈ 1.57079632679 -->
<pose>0 0 0 1.57079632679 0 0</pose>
<geometry>
<cylinder>
<radius>0.01</radius>
<length>0.15</length>
</cylinder>
</geometry>
<material>
<diffuse>0 1 0 1</diffuse>
</material>
</visual>
</link>

<link name='B'>
<!-- The frame B measured in A, X_AB, is a translation along the Ax
axis, and a small translation along the Ay (revolute) axis for visual
purposes. This link is also called 'Coupler' in the diagram in README.md -->
<pose relative_to="A">4 0.1 0 0 0 0</pose>
<inertial>
<!-- Bcm (B's center of mass) is located at the midpoint of the 4 meter long link. -->
<pose>2 0 0 0 0 0</pose>
<mass>20</mass>
<inertia>
<!-- Inertia tensor for a solid cuboid of dimensions
(l, w, h) = (4.2 m, 0.1 m, 0.2 m) and mass 20 kg-->
<ixx>0.08333333333333334</ixx>
<iyy>29.46666666666666</iyy>
<izz>29.416666666666668</izz>
<ixy>0</ixy><ixz>0</ixz><iyz>0</iyz>
</inertia>
</inertial>
<!-- A skinny blue box along the Bx axis -->
<visual name='B_visual'>
<pose>2 0 0 0 0 0</pose>
<geometry>
<box><size>4.2 0.1 0.2</size></box>
</geometry>
<material>
<diffuse>0 0 1 1</diffuse>
</material>
</visual>
<!-- A visual representing the revolute joint between
link A and link B. -->
<visual name='B_pivot_visual'>
<!-- Rotation around Bx by 90° = π/2 radians ≈ 1.57079632679 -->
<pose>0 -0.05 0 1.57079632679 0 0</pose>
<geometry>
<cylinder>
<radius>0.01</radius>
<length>0.25</length>
</cylinder>
</geometry>
<material>
<diffuse>0 1 0 1</diffuse>
</material>
</visual>
<!-- A visual representing the 'Coupler-Point' P -->
<visual name='CouplerPoint'>
<!-- Translation down Bx by 2 and down Bz by -2 -->
<pose relative_to="B">2 0 -2 0 0 0</pose>
<geometry>
<sphere>
<radius>0.1</radius>
</sphere>
</geometry>
<material>
<diffuse>0 0 1 1</diffuse>
</material>
</visual>
</link>

<link name='C'>
<!-- The frame C measured in W, X_WC, is a translation along the Wx
axis, and a small translation along the Wy (revolute) axis for visual
purposes. This link is also called 'Rocker' in the diagram in README.md -->
<pose>-2 0.2 0 0 0 0</pose>
<inertial>
<!-- Ccm (C's center of mass) is located at the midpoint of the 4 meter long link. -->
<pose>2 0 0 0 0 0</pose>
<mass>20</mass>
<inertia>
<!-- Inertia tensor for a solid cuboid of dimensions
(l, w, h) = (4.2 m, 0.1 m, 0.2 m) and mass 20 kg-->
<ixx>0.08333333333333334</ixx>
<iyy>29.46666666666666</iyy>
<izz>29.416666666666668</izz>
<ixy>0</ixy><ixz>0</ixz><iyz>0</iyz>
</inertia>
</inertial>
<!-- A skinny yellow box along the Bx axis -->
<visual name='C_visual'>
<pose>2 0 0 0 0 0</pose>
<geometry>
<box><size>4.2 0.1 0.2</size></box>
</geometry>
<material>
<diffuse>1 1 0 1</diffuse>
</material>
</visual>
<!-- A visual representing the revolute joint between the world
and link C. -->
<visual name='C_pivot_visual'>
<!-- Rotation around Cx by 90° = π/2 radians ≈ 1.57079632679 -->
<pose>0 0 0 1.57079632679 0 0</pose>
<geometry>
<cylinder>
<radius>0.01</radius>
<length>0.15</length>
</cylinder>
</geometry>
<material>
<diffuse>0 1 0 1</diffuse>
</material>
</visual>
</link>

<joint name='joint_WA' type='revolute'>
<child>A</child>
<parent>world</parent>
<axis>
<!--
This revolute joint's initial angle is
tan⁻¹(√15) ≈ 1.318116071652818 ≈ 75.52°.
See the README for more details.
-->
<initial_position>1.318116071652818</initial_position>
<xyz>0 1 0</xyz>
<limit>
<!-- No limit provided. sdformat defaults to -1, which we parse as
an actuated joint. -->
</limit>
<dynamics>
<damping>0.0</damping>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>

<joint name='joint_AB' type='revolute'>
<child>B</child>
<parent>A</parent>
<axis>
<!--
This revolute joint's initial angle is
π − tan⁻¹(√15) ≈ 1.8234765819369751 ≈ 104.48°.
See the README for more details.
-->
<initial_position>1.8234765819369751</initial_position>
<xyz>0 1 0</xyz>
<limit>
<!-- An effort of 0 is parsed as an unactuated joint. -->
<effort>0</effort>
</limit>
<dynamics>
<damping>0.0</damping>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>

<joint name='joint_WC' type='revolute'>
<child>C</child>
<parent>world</parent>
<axis>
<!--
This revolute joint's initial angle is
π − tan⁻¹(√15) ≈ 1.8234765819369751 ≈ 104.48°.
See the README for more details.
-->
<initial_position>1.8234765819369751</initial_position>
<xyz>0 1 0</xyz>
<limit>
<!-- An effort of 0 is parsed as an unactuated joint. -->
<effort>0</effort>
</limit>
<dynamics>
<damping>0.0</damping>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>

<!--
A frame at the end of the B link, down the Bx axis. The frame is
rotated so that the z of Bc_bushing is aligned with the By axis. This is
done so the LinearBushingRollPitchYaw ForceElement freely rotates around
its z axis (Yaw) to avoid gimbal lock on the y axis (Pitch).
-->
<frame name="Bc_bushing" attached_to="B">
<!-- This frame is rotated relative from link B, around Bx, by -90° = -π/2 radians ≈ -1.57079632679 -->
<pose relative_to="B">4 0 0 -1.57079632679 0 0</pose>
</frame>

<!--
A frame at the end of the C link, down the Cx axis. The frame is
rotated so that the z of Cb_bushing is aligned with the Cy axis. This is
done so the LinearBushingRollPitchYaw ForceElement freely rotates around
its z axis (Yaw) to avoid gimbal lock on the y axis (Pitch).
-->
<frame name="Cb_bushing" attached_to="C">
<!-- This frame is rotated relative from link C, around Cx, by -90° = -π/2 radians ≈ -1.57079632679 -->
<pose relative_to="C">4 0 0 -1.57079632679 0 0</pose>
</frame>

</model>
</sdf>
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
188 changes: 188 additions & 0 deletions examples/multibody/four_bar/passive_simulation.cc
Original file line number Diff line number Diff line change
@@ -0,0 +1,188 @@
/* @file
A four bar linkage demo demonstrating the use of a linear bushing as
a way to model a kinematic loop. It shows:
- How to model a four bar linkage in SDF.
- Use the `multibody::Parser` to load a model from an SDF file into a
MultibodyPlant.
- Model a revolute joint with a `multibody::LinearBushingRollPitchYaw` to
model a closed kinematic chain.
Refer to README.md for more details.
*/
#include <gflags/gflags.h>

#include "drake/common/find_resource.h"
#include "drake/geometry/geometry_visualization.h"
#include "drake/multibody/parsing/parser.h"
#include "drake/multibody/tree/linear_bushing_roll_pitch_yaw.h"
#include "drake/multibody/tree/revolute_joint.h"
#include "drake/systems/analysis/simulator.h"
#include "drake/systems/analysis/simulator_gflags.h"
#include "drake/systems/analysis/simulator_print_stats.h"
#include "drake/systems/framework/diagram_builder.h"

namespace drake {

using Eigen::Vector3d;

using geometry::SceneGraph;
using multibody::Frame;
using multibody::LinearBushingRollPitchYaw;
using multibody::MultibodyPlant;
using multibody::Parser;
using multibody::RevoluteJoint;
using systems::Context;
using systems::DiagramBuilder;
using systems::Simulator;

namespace examples {
namespace multibody {
namespace four_bar {
namespace {

DEFINE_double(simulation_time, 10.0, "Duration of the simulation in seconds.");

DEFINE_double(
force_stiffness, 30000,
"Force (translational) stiffness value for kx, ky, kz in N/m of the "
"LinearBushingRollPitchYaw ForceElement.");

DEFINE_double(
force_damping, 1500,
"Force (translational) damping value for dx, dy, dz in N·s/m of the "
"LinearBushingRollPitchYaw ForceElement.");

DEFINE_double(torque_stiffness, 30000,
"Torque (rotational) stiffness value for k₀, k₁, k₂ in N·m/rad "
"of the LinearBushingRollPitchYaw ForceElement.");

DEFINE_double(
torque_damping, 1500,
"Torque (rotational) damping value for d₀, d₁, and d₂ in N·m·s/rad of "
"the LinearBushingRollPitchYaw ForceElement.");

DEFINE_double(applied_torque, 0.0,
"Constant torque applied to joint_WA, denoted Tᴀ in the README.");

DEFINE_double(
initial_velocity, 3.0,
"Initial velocity, q̇A, of joint_WA. Default set to 3 radians/second ≈ "
"171.88 degrees/second so that the model has some motion.");

int do_main() {
// Build a generic MultibodyPlant and SceneGraph.
DiagramBuilder<double> builder;

auto [four_bar, scene_graph] = AddMultibodyPlantSceneGraph(
&builder, std::make_unique<MultibodyPlant<double>>(0.0));

// Make and add the four_bar model from an SDF model.
const std::string relative_name =
"drake/examples/multibody/four_bar/four_bar.sdf";
const std::string full_name = FindResourceOrThrow(relative_name);

Parser parser(&four_bar);
parser.AddModelFromFile(full_name);

// Get the two frames that define the bushing, namely frame Bc that is
// welded to the end of link B and frame Cb that is welded to the end of
// link C. Although the bushing allows 6 degrees-of-freedom between frames
// Bc and Cb, the stiffness and damping constants are chosen to approximate
// the connection between frames Bc and Cb as having only one rotational
// degree of freedom along the bushing's z-axis.
const Frame<double>& bc_bushing = four_bar.GetFrameByName("Bc_bushing");
const Frame<double>& cb_bushing = four_bar.GetFrameByName("Cb_bushing");

// See the README for a discussion of how these parameters were selected
// for this particular example.
const double k_xyz = FLAGS_force_stiffness;
const double d_xyz = FLAGS_force_damping;
const double k_012 = FLAGS_torque_stiffness;
const double d_012 = FLAGS_torque_damping;

// See the documentation for LinearBushingRollPitchYaw.
// This particular choice of parameters models a z-axis revolute joint.
// Note: since each link is constrained to rigid motion in the world X-Z
// plane (X-Y plane of the bushing) by the revolute joints specified in the
// SDF, it is unnecessary for the bushing to have non-zero values for: k_z,
// d_z, k_0, d_0, k_1, d_1. They are left here as an example of how to
// parameterize a general z-axis revolute joint without other constraints.
const Vector3d force_stiffness_constants{k_xyz, k_xyz, k_xyz}; // N/m
const Vector3d force_damping_constants{d_xyz, d_xyz, d_xyz}; // N·s/m
const Vector3d torque_stiffness_constants{k_012, k_012, 0}; // N·m/rad
const Vector3d torque_damping_constants{d_012, d_012, 0}; // N·m·s/rad

// Add a bushing force element where the joint between link B and link C
// should be in an ideal 4-bar linkage.
four_bar.AddForceElement<LinearBushingRollPitchYaw>(
bc_bushing, cb_bushing, torque_stiffness_constants,
torque_damping_constants, force_stiffness_constants,
force_damping_constants);

// We are done defining the model. Finalize and build the diagram.
four_bar.Finalize();

ConnectDrakeVisualizer(&builder, scene_graph);
auto diagram = builder.Build();

// Create a context for this system and sub-context for the four bar system.
std::unique_ptr<Context<double>> diagram_context =
diagram->CreateDefaultContext();
Context<double>& four_bar_context =
four_bar.GetMyMutableContextFromRoot(diagram_context.get());

// A constant source for applied torque (Tᴀ) at joint_WA.
four_bar.get_actuation_input_port().FixValue(&four_bar_context,
FLAGS_applied_torque);

// Set initial conditions so the model will have some motion
const RevoluteJoint<double>& joint_WA =
four_bar.GetJointByName<RevoluteJoint>("joint_WA");
const RevoluteJoint<double>& joint_WC =
four_bar.GetJointByName<RevoluteJoint>("joint_WC");
const RevoluteJoint<double>& joint_AB =
four_bar.GetJointByName<RevoluteJoint>("joint_AB");

// See the README for an explanation of these angles.
const double qA = atan2(sqrt(15.0), 1.0); // about 75.52°
const double qB = M_PI - qA; // about 104.48°
const double qC = qB; // about 104.48°

joint_WA.set_angle(&four_bar_context, qA);
joint_AB.set_angle(&four_bar_context, qB);
joint_WC.set_angle(&four_bar_context, qC);

// Set q̇A,the rate of change in radians/second of the angle qA.
joint_WA.set_angular_rate(&four_bar_context, FLAGS_initial_velocity);

// Create a simulator and run the simulation
std::unique_ptr<Simulator<double>> simulator =
MakeSimulatorFromGflags(*diagram, std::move(diagram_context));

simulator->AdvanceTo(FLAGS_simulation_time);

// Print some useful statistics
PrintSimulatorStatistics(*simulator);

return 0;
}

} // namespace
} // namespace four_bar
} // namespace multibody
} // namespace examples
} // namespace drake

int main(int argc, char* argv[]) {
gflags::SetUsageMessage(
"A four bar linkage demo demonstrating the use of a linear bushing as "
"a way to model a kinematic loop. Launch drake-visualizer before running "
"this example.");
// Changes the default realtime rate to 1.0, so the visualization looks
// realistic. Otherwise, it finishes so fast that we can't appreciate the
// motion. Users can still change it on command-line, e.g. "
// --simulator_target_realtime_rate=0.5" to slow it down.
FLAGS_simulator_target_realtime_rate = 1.0;
gflags::ParseCommandLineFlags(&argc, &argv, true);
return drake::examples::multibody::four_bar::do_main();
}