Skip to content

Commit

Permalink
nicer docs
Browse files Browse the repository at this point in the history
  • Loading branch information
baggepinnen committed Oct 28, 2023
1 parent a326d05 commit 282a2dc
Show file tree
Hide file tree
Showing 3 changed files with 33 additions and 13 deletions.
25 changes: 13 additions & 12 deletions docs/src/examples/pendulum.md
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
# Pendulum--The "Hello World of multi-body dynamics"
This beginners tutorial will model a pendulum pivoted around the origin in the world frame. The world frame is a constant that lives inside the Multibody module, all multibody models are "grounded" in the same world.
This beginners tutorial will model a pendulum pivoted around the origin in the world frame. The world frame is a constant that lives inside the Multibody module, all multibody models are "grounded" in the same world, i.e., the `world` component must be included in all models.

![Pendulum](https://doc.modelica.org/Modelica%203.2.3/Resources/Images/Mechanics/MultiBody/Examples/Elementary/Pendulum.png)

Expand All @@ -18,21 +18,21 @@ show(stdout, MIME"text/plain"(), world)
nothing # hide
```

Unless otherwise specified, the world defaults to have a gravitational field pointing in the negative ``y`` direction and an graivational acceleration of ``9.81``.
Unless otherwise specified, the world defaults to have a gravitational field pointing in the negative ``y`` direction and a gravitational acceleration of ``9.81``.


## Modeling the pendulum
Our simple pendulum will initially consist of a [`Body`](@ref) and a [`Revolute`](@ref) joint (the pivot joint). We construct these elements by calling their constructors
Our simple pendulum will initially consist of a [`Body`](@ref) (point mass) and a [`Revolute`](@ref) joint (the pivot joint). We construct these elements by calling their constructors
```@example pendulum
@named joint = Revolute(n = [0, 0, 1], isroot = true)
@named body = Body(; m = 1, isroot = false, r_cm = [0.5, 0, 0])
nothing # hide
```
The `n` argument to [`Revolute`](@ref) denotes the rotational axis of the joint, this vector must have `norm(n) == 1`. We also indicate that the revolute joint is the root of the kinematic tree, i.e., the potential state of the joint will serve as the state variables for the system.

The [`Body`](@ref) is constructed by providing its mass, `m`, and the vector `r_cm` from its first frame, `body.frame_a`, to the center of mass. Since the world by default has the gravity field pointing along the negative ``y`` axis, we place the center of mass along the ``x``-axis to make the pendulum swing back and forth. The body is not selected as the root of the kinematic tree, since we have a joint in this system, but if we had attached the body directly to, e.g., a spring, we could set the body to be the root and avoid having to introduce an "artificial joint".
The [`Body`](@ref) is constructed by providing its mass, `m`, and the vector `r_cm` from its mounting frame, `body.frame_a`, to the center of mass. Since the world by default has the gravity field pointing along the negative ``y`` axis, we place the center of mass along the ``x``-axis to make the pendulum swing back and forth. The body is not selected as the root of the kinematic tree, since we have a joint in this system, but if we had attached the body directly to, e.g., a spring, we could set the body to be the root and avoid having to introduce an "artificial joint", which is otherwise needed in order to have at least one component that has a potential state.

To connect the components together, we create a vector of connections using the `connect` function. A joint typically has two frames, `frame_a` and `frame_b`. The first frame of the joint is attached to the world frame, and the body is attached to the second joint frame. The order of the connections is not important for ModelingToolkit, but it's good practice to follow some convention, here, we start at the world and progress outwards in the kinematic tree.
To connect the components together, we create a vector of connections using the `connect` function. A joint typically has two frames, `frame_a` and `frame_b`. In this example, the first frame of the joint is attached to the world frame, and the body is attached to the second frame of the joint, i.e., the joint allows the body to swing back and forth. The order of the connections is not important for ModelingToolkit, but it's good practice to follow some convention, here, we start at the world and progress outwards in the kinematic tree.
```@example pendulum
connections = [
connect(world.frame_b, joint.frame_a)
Expand Down Expand Up @@ -65,7 +65,7 @@ plot(sol, idxs = joint.phi, title="Pendulum")
```
The solution `sol` can be plotted directly if the Plots package is loaded. The figure indicates that the pendulum swings back and forth without any damping. To add damping as well, we could add a `Damper` from the `ModelingToolkitStandardLibrary.Mechanical.Rotational` module to the revolute joint. We do this below

## 3d Animation
## 3D Animation
Multibody.jl supports automatic 3D rendering of mechanisms, we use this feature to illustrate the result of the simulation below:

```@example pendulum
Expand All @@ -76,10 +76,10 @@ nothing # hide

![animation](pendulum.gif)

By default, the world frame is indicated using the convention x: red, y: green, z: blue.
By default, the world frame is indicated using the convention x: red, y: green, z: blue. The animation shows how the simple [`Body`](@ref) represents a point mass at a particular distance `r_cm` away from its mounting flange `frame_a`. To model a more physically motivated pendulum rod, we could have used a [`BodyShape`](@ref) component, which has two mounting flanges instead of one. The [`BodyShape`](@ref) component is shown in several of the examples available in the example sections of the documentation.

## Adding damping
To add damping to the pendulum such that the pendulum will eventually come to rest, we add a [`Damper`](@ref) to the revolute joint. The damping coefficient is given by `d`, and the damping force is proportional to the angular velocity of the joint. To add the damper to the revolute joint, we must create the joint with the keyword argument `useAxisFlange = true`, this adds two internal flanges to the joint to which you can attach components from the `ModelingToolkitStandardLibrary.Mechanical.Rotational` module. We then connect one of the flanges of the damper to the axis flange of the joint, and the other damper flange to the support flange which is rigidly attached to the world.
To add damping to the pendulum such that the pendulum will eventually come to rest, we add a [`Damper`](@ref) to the revolute joint. The damping coefficient is given by `d`, and the damping force is proportional to the angular velocity of the joint. To add the damper to the revolute joint, we must create the joint with the keyword argument `useAxisFlange = true`, this adds two internal flanges to the joint to which you can attach components from the `ModelingToolkitStandardLibrary.Mechanical.Rotational` module (1-dimensional components). We then connect one of the flanges of the damper to the axis flange of the joint, and the other damper flange to the support flange which is rigidly attached to the world.
```@example pendulum
@named damper = Rotational.Damper(d = 0.1)
@named joint = Revolute(n = [0, 0, 1], isroot = true, useAxisFlange = true)
Expand Down Expand Up @@ -119,10 +119,10 @@ connections = [connect(world.frame_b, joint.frame_a)
@named model = ODESystem(connections, t, systems = [world, joint, body, damper, spring])
ssys = structural_simplify(IRSystem(model))
prob = ODEProblem(ssys, [damper.s_rel => 1], (0, 30))
prob = ODEProblem(ssys, [damper.s_rel => 1, D(D(joint.s)) => 0], (0, 30))
sol = solve(prob, Rodas4())
plot(sol, idxs = joint.s, title="Mass-spring-damper system")
Plots.plot(sol, idxs = joint.s, title="Mass-spring-damper system")
```

As is hopefully evident from the little code snippet above, this linear pendulum model has a lot in common with the rotary pendulum. In this example, we connected both the spring and a damper to the same axis flange in the joint. This time, the components came from the `Translational` submodule of ModelingToolkitStandardLibrary rather than the `Rotational` submodule. Also here do we pass `useAxisFlange` when we create the joint to make sure that it is equipped with the flanges `support` and `axis` needed to connect the translational components.
Expand All @@ -142,7 +142,8 @@ ssys = structural_simplify(IRSystem(model))
defs = Dict(collect(multibody_spring.r_rel_0 .=> [0, 1, 0])...,
collect(root_body.r_0 .=> [0, 0, 0])...,
collect((D.(root_body.phi)) .=> [0, 0, 0])...,
collect(D.(D.(root_body.phi)) .=> [0, 0, 0])...)
collect(D.(D.(root_body.phi)) .=> [0, 0, 0])...,
collect(D.(root_body.phid) .=> [0, 0, 0])...,)
prob = ODEProblem(ssys, defs, (0, 30))
Expand All @@ -169,7 +170,7 @@ The figure above should look identical to the simulation of the mass-spring-damp
## Going 3D
The systems we have modeled so far have all been _planar_ mechanisms. We now extend this to a 3-dimensional system, the [_Furuta pendulum_](https://en.wikipedia.org/wiki/Furuta_pendulum).

This pendulum has two joints, one in the "shoulder", which is typically configured to rotate around the gravitational axis, and one in the "elbow", which is typically configured to rotate around the axis of the upper arm. The upper arm is attached to the shoulder joint, and the lower arm is attached to the elbow joint. The tip of the pendulum is attached to the lower arm.
This pendulum, sometimes referred to as a _rotary pendulum_, has two joints, one in the "shoulder", which is typically configured to rotate around the gravitational axis, and one in the "elbow", which is typically configured to rotate around the axis of the upper arm. The upper arm is attached to the shoulder joint, and the lower arm is attached to the elbow joint. The tip of the pendulum is attached to the lower arm.

```@example pendulum
using ModelingToolkit, Multibody, JuliaSimCompiler, OrdinaryDiffEq, Plots
Expand Down
19 changes: 19 additions & 0 deletions docs/src/index.md
Original file line number Diff line number Diff line change
Expand Up @@ -36,12 +36,29 @@ Pages = ["frames.jl"]
```

## Joints

A joint restricts the number of degrees of freedom (DOF) of a body. For example, a free floating body has 6 DOF, but if it is attached to a [`Revolute`](@ref) joint, the joint restricts all but one rotational degree of freedom (a revolute joint acts like a hinge). Similarily, a [`Prismatic`](@ref) joint restricts all but one translational degree of freedom (a prismatic joint acts like a slider).

A [`Spherical`](@ref) joints restricts all translational degrees of freedom, but allows all rotational degrees of freedom. It thus transmits no torque.

Some joints offer the option to add 1-dimensional components to them by providing the keyword `useAxisFlange = true`. This allows us to add, e.g., springs, dampers, sensors, and actuators to the joint.

```@autodocs
Modules = [Multibody]
Pages = ["joints.jl"]
```

## Components

The perhaps most fundamental component is a [`Body`](@ref), this component has a single flange, `frame_a`, which is used to connect the body to other components. This component has a mass, a vector `r_cm` from `frame_a` to the center of mass, and a moment of inertia tensor `I` in the center of mass. The body can be thought of as a point mass with a moment of inertia tensor.

A mass with a shape can be modeled using a [`BodyShape`](@ref). The primary difference between a [`Body`](@ref) and a [`BodyShape`](@ref) is that the latter has an additional flange, `frame_b`, which is used to connect the body to other components. The translation between `flange_a` and `flange_b` is determined by the vector `r`. The [`BodyShape`](@ref) is suitable to model, e.g., cylinders, rods, and boxes.

A rod without a mass (just a translation), is modeled using [`FixedTranslation`](@ref).




```@autodocs
Modules = [Multibody]
Pages = ["components.jl"]
Expand All @@ -54,6 +71,8 @@ Pages = ["forces.jl"]
```

## Sensors
A sensor is an object that translates quantities in the mechanical domain into causal signals which can interact with causal components from [ModelingToolkitStandardLibrary.Blocks](https://docs.sciml.ai/ModelingToolkitStandardLibrary/stable/API/blocks/), such as control systems etc.

```@autodocs
Modules = [Multibody]
Pages = ["sensors.jl"]
Expand Down
2 changes: 1 addition & 1 deletion src/joints.jl
Original file line number Diff line number Diff line change
Expand Up @@ -609,7 +609,7 @@ end
"""
FreeMotion(; name, enforceState = true, sequence, isroot = true, w_rel_a_fixed = false, z_rel_a_fixed = false, phi = 0, phi_d = 0, phi_dd = 0, w_rel_b = 0, r_rel_a = 0, v_rel_a = 0, a_rel_a = 0)
Joint which does not constrain the motion between `frame_a` and `frame_b`. Such a joint is only meaningful if the relative distance and orientation between `frame_a` and `frame_b`, and their derivatives, shall be used as state.
Joint which _does not_ constrain the motion between `frame_a` and `frame_b`. Such a joint is only meaningful if the relative distance and orientation between `frame_a` and `frame_b`, and their derivatives, shall be used as state.
Note, that bodies such as [`Body`](@ref), [`BodyShape`](@ref), have potential state variables describing the distance and orientation, and their derivatives, between the world frame and a body fixed frame. Therefore, if these potential state variables are suited, a `FreeMotion` joint is not needed.
Expand Down

0 comments on commit 282a2dc

Please sign in to comment.