Skip to content

Commit

Permalink
follow a more julian naming convention
Browse files Browse the repository at this point in the history
  • Loading branch information
baggepinnen committed Jun 12, 2024
1 parent ac22ef7 commit 1c19687
Show file tree
Hide file tree
Showing 25 changed files with 338 additions and 320 deletions.
4 changes: 2 additions & 2 deletions docs/src/examples/free_motion.md
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
# Free motions
This example demonstrates how a free-floating [`Body`](@ref) can be simulated. The body is attached to the world through a [`FreeMotion`](@ref) joint, i.e., a joint that imposes no constraints. The joint is required to add the appropriate relative state variables between the world and the body. We choose `enforceState = true` and `isroot = true` in the [`FreeMotion`](@ref) constructor.
This example demonstrates how a free-floating [`Body`](@ref) can be simulated. The body is attached to the world through a [`FreeMotion`](@ref) joint, i.e., a joint that imposes no constraints. The joint is required to add the appropriate relative state variables between the world and the body. We choose `state = true` and `isroot = true` in the [`FreeMotion`](@ref) constructor.

```@example FREE_MOTION
using Multibody
Expand All @@ -12,7 +12,7 @@ t = Multibody.t
D = Differential(t)
world = Multibody.world
@named freeMotion = FreeMotion(enforceState = true, isroot = true)
@named freeMotion = FreeMotion(state = true, isroot = true)
@named body = Body(m = 1)
eqs = [connect(world.frame_b, freeMotion.frame_a)
Expand Down
2 changes: 1 addition & 1 deletion docs/src/examples/gearbox.md
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@ systems = @named begin
gearConstraint = GearConstraint(; ratio = 10)
cyl1 = Body(; m = 1, r_cm = [0.4, 0, 0])
cyl2 = Body(; m = 1, r_cm = [0.4, 0, 0])
torque1 = Torque(resolveInFrame = :frame_b)
torque1 = Torque(resolve_frame = :frame_b)
fixed = Fixed()
inertia1 = Rotational.Inertia(J = cyl1.I_11)
idealGear = Rotational.IdealGear(ratio = 10, use_support = true)
Expand Down
4 changes: 2 additions & 2 deletions docs/src/examples/kinematic_loops.md
Original file line number Diff line number Diff line change
Expand Up @@ -22,8 +22,8 @@ D = Differential(t)
world = Multibody.world
systems = @named begin
j1 = Revolute(useAxisFlange=true) # We use an axis flange to attach a damper
j2 = Revolute(useAxisFlange=true)
j1 = Revolute(axisflange=true) # We use an axis flange to attach a damper
j2 = Revolute(axisflange=true)
j3 = Revolute()
j4 = RevolutePlanarLoopConstraint()
j5 = Revolute()
Expand Down
12 changes: 6 additions & 6 deletions docs/src/examples/pendulum.md
Original file line number Diff line number Diff line change
Expand Up @@ -79,10 +79,10 @@ nothing # hide
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 (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.
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 `axisflange = 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)
@named joint = Revolute(n = [0, 0, 1], isroot = true, axisflange = true)
connections = [connect(world.frame_b, joint.frame_a)
connect(damper.flange_b, joint.axis)
Expand Down Expand Up @@ -116,7 +116,7 @@ A mass suspended in a spring can be though of as a linear pendulum (often referr
@named body_0 = Body(; m = 1, isroot = false, r_cm = [0, 0, 0])
@named damper = Translational.Damper(d=1)
@named spring = Translational.Spring(c=10)
@named joint = Prismatic(n = [0, 1, 0], useAxisFlange = true)
@named joint = Prismatic(n = [0, 1, 0], axisflange = true)
connections = [connect(world.frame_b, joint.frame_a)
connect(damper.flange_b, spring.flange_b, joint.axis)
Expand All @@ -132,7 +132,7 @@ sol = solve(prob, Rodas4())
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.
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 `axisflange` when we create the joint to make sure that it is equipped with the flanges `support` and `axis` needed to connect the translational components.

```@example pendulum
Multibody.render(model, sol; z = -5, filename = "linear_pend.gif", framerate=24)
Expand Down Expand Up @@ -197,8 +197,8 @@ W(args...; kwargs...) = Multibody.world
@mtkmodel FurutaPendulum begin
@components begin
world = W()
shoulder_joint = Revolute(n = [0, 1, 0], isroot = true, useAxisFlange = true)
elbow_joint = Revolute(n = [0, 0, 1], isroot = true, useAxisFlange = true, phi0=0.1)
shoulder_joint = Revolute(n = [0, 1, 0], isroot = true, axisflange = true)
elbow_joint = Revolute(n = [0, 0, 1], isroot = true, axisflange = true, phi0=0.1)
upper_arm = BodyShape(; m = 0.1, isroot = false, r = [0, 0, 0.6], radius=0.05)
lower_arm = BodyShape(; m = 0.1, isroot = false, r = [0, 0.6, 0], radius=0.05)
tip = Body(; m = 0.3, isroot = false)
Expand Down
2 changes: 1 addition & 1 deletion docs/src/examples/sensors.md
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@ sol = solve(prob, Rodas4())
plot(sol, idxs = [collect(forcesensor.force.u); collect(joint.frame_a.f)])
```

Note how the force sensor measures a force that appears to equal the cut-force in the joint in magnitude, but the orientation appears to differ. Frame cut forces and toques are resolved in the world frame by default, while the force sensor measures the force in the frame of the sensor. We can choose which frame to resolve the measurements in by using hte keyword argument `@named forcesensor = CutForce(; resolveInFrame = :world)`. If we do this, the traces in the plot above will overlap.
Note how the force sensor measures a force that appears to equal the cut-force in the joint in magnitude, but the orientation appears to differ. Frame cut forces and toques are resolved in the world frame by default, while the force sensor measures the force in the frame of the sensor. We can choose which frame to resolve the measurements in by using hte keyword argument `@named forcesensor = CutForce(; resolve_frame = :world)`. If we do this, the traces in the plot above will overlap.

Since the torque sensor measures a torque in a revolute joint, it should measure zero torque in this case, no torque is transmitted through the revolute joint since the rotational axis is perpendicular to the gravitational force:
```@example sensor
Expand Down
2 changes: 1 addition & 1 deletion docs/src/examples/spherical_pendulum.md
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@ D = Differential(t)
world = Multibody.world
systems = @named begin
joint = Spherical(enforceState=true, isroot=true, phi = 1)
joint = Spherical(state=true, isroot=true, phi = 1)
bar = FixedTranslation(r = [0, -1, 0])
body = Body(; m = 1, isroot = false)
end
Expand Down
2 changes: 1 addition & 1 deletion docs/src/examples/spring_damper_system.md
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@ world = Multibody.world
body2 = Body(; m = 1, isroot = false, r_cm = [0.0, -0.2, 0]) # This is not root since there is a joint parallel to the spring leading to this body
bar1 = FixedTranslation(r = [0.3, 0, 0])
bar2 = FixedTranslation(r = [0.6, 0, 0])
p2 = Prismatic(n = [0, -1, 0], s0 = 0.1, useAxisFlange = true, isroot = true)
p2 = Prismatic(n = [0, -1, 0], s0 = 0.1, axisflange = true, isroot = true)
spring2 = Multibody.Spring(c = 30, s_unstretched = 0.1)
spring1 = Multibody.Spring(c = 30, s_unstretched = 0.1)
damper1 = Multibody.Damper(d = 2)
Expand Down
4 changes: 2 additions & 2 deletions docs/src/examples/spring_mass_system.md
Original file line number Diff line number Diff line change
Expand Up @@ -22,14 +22,14 @@ D = Differential(t)
world = Multibody.world
systems = @named begin
p1 = Prismatic(n = [0, -1, 0], s0 = 0.1, useAxisFlange = true)
p1 = Prismatic(n = [0, -1, 0], s0 = 0.1, axisflange = true)
spring1 = TranslationalModelica.Spring(c=30, s_rel0 = 0.1)
spring2 = Multibody.Spring(c = 30, s_unstretched = 0.1)
body1 = Body(m = 1, r_cm = [0, 0, 0])
bar1 = FixedTranslation(r = [0.3, 0, 0])
bar2 = FixedTranslation(r = [0.3, 0, 0])
body2 = Body(m = 1, r_cm = [0, 0, 0])
p2 = Prismatic(n = [0, -1, 0], s0 = 0.1, useAxisFlange = true)
p2 = Prismatic(n = [0, -1, 0], s0 = 0.1, axisflange = true)
end
eqs = [
Expand Down
6 changes: 3 additions & 3 deletions docs/src/examples/three_springs.md
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@ This example mirrors that of the [modelica three-springs](https://doc.modelica.o



The example connects three springs together in a single point. The springs are all massless and do normally not have any state variables, but we can insist on one of the springs being stateful, in this case, we must tell the lower spring component to act as root by setting `fixedRotationAtFrame_a = fixedRotationAtFrame_b = true`.
The example connects three springs together in a single point. The springs are all massless and do normally not have any state variables, but we can insist on one of the springs being stateful, in this case, we must tell the lower spring component to act as root by setting `fixed_rotation_at_frame_a = fixed_rotation_at_frame_b = true`.

```@example spring_mass_system
using Multibody
Expand All @@ -28,7 +28,7 @@ systems = @named begin
spring1 = Multibody.Spring(c = 20, m = 0, s_unstretched = 0.1,
r_rel_0 = [-0.2, -0.2, 0.2])
spring2 = Multibody.Spring(c = 40, m = 0, s_unstretched = 0.1,
fixedRotationAtFrame_a = true, fixedRotationAtFrame_b = true)
fixed_rotation_at_frame_a = true, fixed_rotation_at_frame_b = true)
spring3 = Multibody.Spring(c = 20, m = 0, s_unstretched = 0.1)
end
eqs = [connect(world.frame_b, bar1.frame_a)
Expand All @@ -42,7 +42,7 @@ eqs = [connect(world.frame_b, bar1.frame_a)
@named model = ODESystem(eqs, t, systems = [world; systems])
ssys = structural_simplify(IRSystem(model))
prob = ODEProblem(ssys, [
D.(spring3.lineForce.r_rel_0) .=> 0;
D.(spring3.lineforce.r_rel_0) .=> 0;
], (0, 10))
sol = solve(prob, Rodas4(), u0=prob.u0 .+ 1e-1*randn(length(prob.u0)))
Expand Down
2 changes: 1 addition & 1 deletion docs/src/index.md
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ A joint restricts the number of degrees of freedom (DOF) of a body. For example,

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.
Some joints offer the option to add 1-dimensional components to them by providing the keyword `axisflange = true`. This allows us to add, e.g., springs, dampers, sensors, and actuators to the joint.

```@autodocs
Modules = [Multibody]
Expand Down
6 changes: 3 additions & 3 deletions examples/JuliaSim_logo.jl
Original file line number Diff line number Diff line change
Expand Up @@ -14,9 +14,9 @@ radius_large = length_scale*0.3
@mtkmodel Logo begin
@components begin
world = World()
revl = Revolute(; radius = radius_large, color=JULIASIM_PURPLE, useAxisFlange=true)
revl2 = Revolute(; radius = radius_large, color=JULIASIM_PURPLE, useAxisFlange=true)
revr = Revolute(; radius = radius_small, color=JULIASIM_PURPLE, useAxisFlange=true)
revl = Revolute(; radius = radius_large, color=JULIASIM_PURPLE, axisflange=true)
revl2 = Revolute(; radius = radius_large, color=JULIASIM_PURPLE, axisflange=true)
revr = Revolute(; radius = radius_small, color=JULIASIM_PURPLE, axisflange=true)
bodyl = Body(m=1, radius = radius_small, color=JULIASIM_PURPLE)
bodyr = Body(m=1, radius = radius_large, color=JULIASIM_PURPLE)
bar_top = FixedTranslation(r=length_scale*[1, 0.05, 0], radius=length_scale*0.025, color=JULIASIM_PURPLE)
Expand Down
4 changes: 2 additions & 2 deletions examples/fourbar.jl
Original file line number Diff line number Diff line change
Expand Up @@ -90,8 +90,8 @@ isinteractive() && plot(sol)
## Now close the loop
# We must also replace one joint with a RevolutePlanarLoopConstraint
systems = @named begin
j1 = Revolute(useAxisFlange=true)
j2 = Revolute(useAxisFlange=true)
j1 = Revolute(axisflange=true)
j2 = Revolute(axisflange=true)
j3 = Revolute()
# j4 = Revolute()
# j2 = RevolutePlanarLoopConstraint()
Expand Down
1 change: 1 addition & 0 deletions src/Multibody.jl
Original file line number Diff line number Diff line change
Expand Up @@ -144,6 +144,7 @@ include("forces.jl")
export PartialCutForceBaseSensor, BasicCutForce, BasicCutTorque, CutTorque, CutForce
include("sensors.jl")

export point_to_point
include("robot/path_planning.jl")
include("robot/robot_components.jl")
include("robot/FullRobot.jl")
Expand Down
30 changes: 15 additions & 15 deletions src/components.jl
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ end
@parameters g=9.81 [description = "gravitational acceleration of world"]
O = ori(frame_b)
eqs = Equation[collect(frame_b.r_0) .~ 0;
O ~ nullRotation()
O ~ nullrotation()
# vec(D(O).R .~ 0); # QUESTION: not sure if I should have to add this, should only have 12 equations according to modelica paper
]
ODESystem(eqs, t, [], [n; g]; name, systems = [frame_b])
Expand All @@ -64,7 +64,7 @@ gravity_acceleration(r) = GlobalScope(world.g) * GlobalScope.(world.n) # NOTE: T
description = "Position vector from world frame to frame_b, resolved in world frame",
] end
eqs = [collect(frame_b.r_0 .~ r)
ori(frame_b) ~ nullRotation()]
ori(frame_b) ~ nullrotation()]
compose(ODESystem(eqs, t; name), systems...)
end

Expand Down Expand Up @@ -165,13 +165,13 @@ Fixed translation followed by a fixed rotation of `frame_b` with respect to `fra

if isroot
R_rel = planar_rotation(n, angle, 0)
eqs = [ori(frame_b) ~ absoluteRotation(frame_a, R_rel);
eqs = [ori(frame_b) ~ absolute_rotation(frame_a, R_rel);
zeros(3) ~ fa + resolve1(R_rel, fb);
zeros(3) ~ taua + resolve1(R_rel, taub) - cross(r,
fa)]
else
R_rel_inv = planar_rotation(n, -angle, 0)
eqs = [ori(frame_a) ~ absoluteRotation(frame_b, R_rel_inv);
eqs = [ori(frame_a) ~ absolute_rotation(frame_b, R_rel_inv);
zeros(3) ~ fb + resolve1(R_rel_inv, fa);
zeros(3) ~ taub + resolve1(R_rel_inv, taua) +
cross(resolve1(R_rel_inv, r), fb)]
Expand Down Expand Up @@ -214,7 +214,7 @@ Representing a body with 3 translational and 3 rotational degrees-of-freedom.
radius = 0.005,
air_resistance = 0.0,
color = [1,0,0,1],
useQuaternions=false,)
quat=false,)
@variables r_0(t)[1:3]=r_0 [
state_priority = 2,
description = "Position vector from origin of world frame to origin of frame_a",
Expand Down Expand Up @@ -262,7 +262,7 @@ Representing a body with 3 translational and 3 rotational degrees-of-freedom.
dvs = [r_0;v_0;a_0;g_0;w_a;z_a;]
eqs = if isroot # isRoot

if useQuaternions
if quat
@named frame_a = Frame(varw = true)
Ra = ori(frame_a, true)
# @variables q(t)[1:4] = [0.0,0,0,1.0]
Expand All @@ -271,7 +271,7 @@ Representing a body with 3 translational and 3 rotational degrees-of-freedom.
# qw = collect(qw)
# Q = Quaternion(q, qw)
@named Q = NumQuaternion(varw=true)
ar = from_Q(Q, angularVelocity2(Q, D.(Q.Q)))
ar = from_Q(Q, angular_velocity2(Q, D.(Q.Q)))
Equation[
0 ~ orientation_constraint(Q)
Ra ~ ar
Expand All @@ -285,7 +285,7 @@ Representing a body with 3 translational and 3 rotational degrees-of-freedom.
@variables phid(t)[1:3]=phid0 [state_priority = 10]
@variables phidd(t)[1:3]=zeros(3) [state_priority = 10]
phi, phid, phidd = collect.((phi, phid, phidd))
ar = axesRotations([1, 2, 3], phi, phid)
ar = axes_rotations([1, 2, 3], phi, phid)

Ra = ori(frame_a, true)

Expand All @@ -302,9 +302,9 @@ Representing a body with 3 translational and 3 rotational degrees-of-freedom.
Ra = ori(frame_a)
# This equation is defined here and not in the Rotation component since the branch above might use another equation
Equation[
# collect(w_a .~ DRa.w); # angularVelocity2(R, D.(R)): skew(R.w) = R.T*der(transpose(R.T))
# collect(w_a .~ DRa.w); # angular_velocity2(R, D.(R)): skew(R.w) = R.T*der(transpose(R.T))
# vec(DRa.R .~ 0)
collect(w_a .~ angularVelocity2(Ra));]
collect(w_a .~ angular_velocity2(Ra));]
end

eqs = [eqs;
Expand Down Expand Up @@ -340,7 +340,7 @@ The `BodyShape` component is similar to a [`Body`](@ref), but it has two frames
"""
@component function BodyShape(; name, m = 1, r = [0, 0, 0], r_cm = 0.5*r, r_0 = 0, radius = 0.08, color=:purple, kwargs...)
systems = @named begin
frameTranslation = FixedTranslation(r = r)
translation = FixedTranslation(r = r)
body = Body(; r_cm, r_0, kwargs...)
frame_a = Frame()
frame_b = Frame()
Expand Down Expand Up @@ -374,8 +374,8 @@ The `BodyShape` component is similar to a [`Body`](@ref), but it has two frames
eqs = [r_0 .~ collect(frame_a.r_0)
v_0 .~ D.(r_0)
a_0 .~ D.(v_0)
connect(frame_a, frameTranslation.frame_a)
connect(frame_b, frameTranslation.frame_b)
connect(frame_a, translation.frame_a)
connect(frame_b, translation.frame_b)
connect(frame_a, body.frame_a)]
ODESystem(eqs, t, [r_0; v_0; a_0], pars; name, systems)
end
Expand Down Expand Up @@ -413,7 +413,7 @@ function Rope(; name, l = 1, n = 10, m = 1, c = 0, d=0, air_resistance=0, d_join
li = l / n # Segment length
mi = m / n # Segment mass

joints = [Spherical(name=Symbol("joint_$i"), isroot=true, enforceState=true, d = d_joint) for i = 1:n+1]
joints = [Spherical(name=Symbol("joint_$i"), isroot=true, state=true, d = d_joint) for i = 1:n+1]

eqs = [
connect(frame_a, joints[1].frame_a)
Expand All @@ -427,7 +427,7 @@ function Rope(; name, l = 1, n = 10, m = 1, c = 0, d=0, air_resistance=0, d_join
springs = [Translational.Spring(c = ci, s_rel0=li, name=Symbol("link_$i")) for i = 1:n]
dampers = [Translational.Damper(d = di, name=Symbol("damping_$i")) for i = 1:n]
masses = [Body(; m = mi, name=Symbol("mass_$i"), isroot=false, r_cm = [0, -li/2, 0], air_resistance) for i = 1:n]
links = [Prismatic(n = [0, -1, 0], s0 = li, name=Symbol("flexibility_$i"), useAxisFlange=true) for i = 1:n]
links = [Prismatic(n = [0, -1, 0], s0 = li, name=Symbol("flexibility_$i"), axisflange=true) for i = 1:n]
for i = 1:n
push!(eqs, connect(links[i].support, springs[i].flange_a, dampers[i].flange_a))
push!(eqs, connect(links[i].axis, springs[i].flange_b, dampers[i].flange_b))
Expand Down
Loading

0 comments on commit 1c19687

Please sign in to comment.