Merge pull request #37 from JuliaComputing/rope

add rope component

ext/Render.jl

@@ -167,7 +167,7 @@ function render!(scene, ::typeof(Body), sys, sol, t)
  end
  mesh!(scene, thing; color, specular = Vec3f(1.5), shininess=20f0, diffuse=Vec3f(1))
 
- iszero(sol(0.0, idxs=r_cmv)) && (return true)
+ iszero(r_cm(sol.t[1])) && (return true)
 
  thing = @lift begin # Cylinder
  Ta = framefun($t)

src/components.jl

@@ -1,4 +1,5 @@
 using LinearAlgebra
+import ModelingToolkitStandardLibrary
 
 function isroot(sys)
  sys.metadata isa Dict || return false
@@ -95,7 +96,7 @@ end
 
 Fixed translation of `frame_b` with respect to `frame_a` with position vector `r` resolved in `frame_a`.
 
-Can be though of as a massless rod. For a massive rod, see [`BodyShape`](@ref) or [`BodyCylinder`](@ref).
+Can be thought of as a massless rod. For a massive rod, see [`BodyShape`](@ref) or [`BodyCylinder`](@ref).
 """
 @component function FixedTranslation(; name, r, radius=0.08f0, color = [0.5019608f0,0.0f0,0.5019608f0,1.0f0])
  @named frame_a = Frame()
@@ -211,6 +212,7 @@ Representing a body with 3 translational and 3 rotational degrees-of-freedom.
  phid0 = zeros(3),
  r_0 = 0,
  radius = 0.005,
+ air_resistance = 0.0,
  color = [1,0,0,1],
  useQuaternions=false,)
  @variables r_0(t)[1:3]=r_0 [
@@ -312,8 +314,13 @@ Representing a body with 3 translational and 3 rotational degrees-of-freedom.
  collect(v_0 .~ D.(r_0))
  collect(a_0 .~ D.(v_0))
  collect(z_a .~ D.(w_a))
- collect(frame_a.f .~ m * (resolve2(Ra, a_0 - g_0) + cross(z_a, r_cm) +
- cross(w_a, cross(w_a, r_cm))))
+ if air_resistance > 0
+ collect(frame_a.f .~ m * (resolve2(Ra, a_0 - g_0 + air_resistance*_norm(v_0)*v_0) + cross(z_a, r_cm) +
+ cross(w_a, cross(w_a, r_cm))))
+ else
+ collect(frame_a.f .~ m * (resolve2(Ra, a_0 - g_0) + cross(z_a, r_cm) +
+ cross(w_a, cross(w_a, r_cm))))
+ end
  collect(frame_a.tau .~ I * z_a + cross(w_a, I * w_a) + cross(r_cm, frame_a.f))]
 
  # pars = [m;r_cm;radius;I_11;I_22;I_33;I_21;I_31;I_32;]
@@ -372,3 +379,70 @@ The `BodyShape` component is similar to a [`Body`](@ref), but it has two frames
  connect(frame_a, body.frame_a)]
  ODESystem(eqs, t, [r_0; v_0; a_0], pars; name, systems)
 end
+
+
+"""
+ Rope(; name, l = 1, n = 10, m = 1, c = 0, d = 0, kwargs)
+
+Model a rope (string / cable) of length `l` and mass `m`.
+
+The rope is modeled as a series of `n` links, each connected by a [`Spherical`](@ref) joint. The links are either fixed in length (default, modeled using [`BodyShape`](@ref)) or flexible, in which case they are modeled as a [`Translational.Spring`](@ref) and [`Translational.Damper`](@ref) in parallel with a [`Prismatic`](@ref) joint with a [`Body`](@ref) adding mass to the center of the link segment. The flexibility is controlled by the parameters `c` and `d`, which are the stiffness and damping coefficients of the spring and damper, respectively. The default values are `c = 0` and `d = 0`, which corresponds to a stiff rope.
+
+
+- `l`: Unstretched length of rope
+- `n`: Number of links used to model the rope. For accurate approximations to continuously flexible ropes, a large number may be required.
+- `m`: The total mass of the rope. Each rope segment will have mass `m / n`.
+- `c`: The equivalent stiffness of the rope, i.e., the rope will act like a spring with stiffness `c`. 
+- `d`: The equivalent damping in the stretching direction of the rope, i.e., the taught rope will act like a damper with damping `d`.
+- `d_joint`: Viscous damping in the joints between the links. A positive value makes the rope dissipate energy while flexing (as opposed to the damping `d` which dissipats energy due to stretching).
+
+## Damping
+There are three different methods of adding damping to the rope:
+- Damping in the stretching direction of the rope, controlled by the parameter `d`.
+- Damping in flexing of the rope, modeled as viscous friction in the joints between the links, controlled by the parameter `d_joint`.
+- Air resistance to the rope moving through the air, controlled by the parameter `air_resistance`. This damping is quadratic in the velocity (``f_d ~ -||v||v``) of each link relative to the world frame.
+"""
+function Rope(; name, l = 1, n = 10, m = 1, c = 0, d=0, air_resistance=0, d_joint = 0, kwargs...)
+
+ @assert n >= 1
+ systems = @named begin
+ frame_a = Frame()
+ frame_b = Frame()
+ end
+
+ 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]
+
+ eqs = [
+ connect(frame_a, joints[1].frame_a)
+ connect(frame_b, joints[end].frame_b)
+ ]
+
+ if c > 0
+ ModelingToolkitStandardLibrary.@symcheck m > 0 || error("A rope with flexibility (c > 0) requires a non-zero mass (m > 0)")
+ ci = n * c # Segment stiffness
+ di = n * d # Segment damping
+ 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]
+ 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))
+ push!(eqs, connect(links[i].frame_a, masses[i].frame_a))
+ end
+ links = [links; springs; dampers; masses]
+ else
+ links = [BodyShape(; m = mi, r = [0, -li, 0], name=Symbol("link_$i"), isroot=false, air_resistance) for i = 1:n]
+ end
+
+
+ for i = 1:n
+ push!(eqs, connect(joints[i].frame_b, links[i].frame_a))
+ push!(eqs, connect(links[i].frame_b, joints[i+1].frame_a))
+ end
+
+ ODESystem(eqs, t; name, systems = [systems; links; joints])
+end

src/joints.jl

@@ -155,16 +155,18 @@ The function returns an ODESystem representing the prismatic joint.
 end
 
 """
- Spherical(; name, enforceState = false, isroot = true, w_rel_a_fixed = false, z_rel_a_fixed = false, sequence_angleStates, phi = 0, phi_d = 0, phi_dd = 0)
+ Spherical(; name, enforceState = false, isroot = true, w_rel_a_fixed = false, z_rel_a_fixed = false, sequence_angleStates, phi = 0, phi_d = 0, phi_dd = 0, d = 0)
 
 Joint with 3 constraints that define that the origin of `frame_a` and the origin of `frame_b` coincide. By default this joint defines only the 3 constraints without any potential state variables. If parameter `enforceState` is set to true, three states are introduced. The orientation of `frame_b` is computed by rotating `frame_a` along the axes defined in parameter vector `sequence_angleStates` (default = [1,2,3], i.e., the Cardan angle sequence) around the angles used as state. If angles are used as state there is the slight disadvantage that a singular configuration is present leading to a division by zero.
 
 - `isroot`: Indicate that `frame_a` is the root, otherwise `frame_b` is the root. Only relevant if `enforceState = true`.
 - `sequence_angleStates`: Rotation sequence
+- `d`: Viscous damping constant. If `d > 0`. the joint dissipates energy due to viscous damping according to ``τ ~ -d*ω``.
 """
 @component function Spherical(; name, enforceState = false, isroot = true, w_rel_a_fixed = false,
  z_rel_a_fixed = false, sequence_angleStates = [1, 2, 3], phi = 0,
  phi_d = 0,
+ d = 0,
  phi_dd = 0)
  @named begin
  ptf = PartialTwoFrames()
@@ -181,9 +183,16 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin
  ] end
 
  # torque balance
- eqs = [zeros(3) .~ collect(frame_a.tau)
- zeros(3) .~ collect(frame_b.tau)
- collect(frame_b.r_0) .~ collect(frame_a.r_0)]
+ if d <= 0
+ eqs = [zeros(3) .~ collect(frame_a.tau)
+ zeros(3) .~ collect(frame_b.tau)
+ collect(frame_b.r_0) .~ collect(frame_a.r_0)]
+ else
+ fric = d*w_rel
+ eqs = [-fric .~ collect(frame_a.tau)
+ fric .~ resolve1(R_rel, collect(frame_b.tau))
+ collect(frame_b.r_0) .~ collect(frame_a.r_0)]
+ end
 
  if enforceState
  @variables begin

test/runtests.jl

@@ -869,4 +869,118 @@ doplot() && plot(sol) |> display
 # render(model, sol, z=-3)
 
 
-end
+end
+
+
+# ==============================================================================
+## Rope pendulum ===============================================================
+# ==============================================================================
+
+# Stiff rope
+world = Multibody.world
+number_of_links = 6
+@named rope = Multibody.Rope(l = 1, m = 1, n=number_of_links, c=0, d=0, air_resistance=0, d_joint=1)
+@named body = Body(; m = 1, isroot = false, r_cm = [0, 0, 0])
+
+connections = [connect(world.frame_b, rope.frame_a)
+ connect(rope.frame_b, body.frame_a)]
+
+@named stiff_rope = ODESystem(connections, t, systems = [world, body, rope])
+# ssys = structural_simplify(model, allow_parameter = false)
+
+@time "Simplify stiff rope pendulum" ssys = structural_simplify(IRSystem(stiff_rope))
+
+D = Differential(t)
+prob = ODEProblem(ssys, [
+ collect(body.r_0) .=> [1,1,1];
+ collect(body.w_a) .=> [1,1,1]
+], (0, 5))
+@time "Stiff rope pendulum" sol = solve(prob, Rodas4(autodiff=false); u0 = prob.u0 .+ 0.1);
+@test SciMLBase.successful_retcode(sol)
+
+
+if true
+ import GLMakie
+ @time "render stiff_rope" render(stiff_rope, sol) # Takes very long time for n>=8
+end
+
+
+
+## Flexible rope
+world = Multibody.world
+number_of_links = 3
+@named rope = Multibody.Rope(l = 1, m = 5, n=number_of_links, c=800.0, d=0.001, d_joint=0.1, air_resistance=0.2)
+@named body = Body(; m = 300, isroot = false, r_cm = [0, 0, 0], air_resistance=0)
+
+connections = [connect(world.frame_b, rope.frame_a)
+ connect(rope.frame_b, body.frame_a)]
+
+@named flexible_rope = ODESystem(connections, t, systems = [world, body, rope])
+# ssys = structural_simplify(model, allow_parameter = false)
+
+@time "Simplify flexible rope pendulum" ssys = structural_simplify(IRSystem(flexible_rope))
+D = Differential(t)
+prob = ODEProblem(ssys, [
+ # D.(D.(collect(rope.r))) .=> 0;
+ collect(body.r_0) .=> [1,1,1];
+ collect(body.w_a) .=> [1,1,1];
+ collect(body.v_0) .=> [10,10,10]
+], (0, 10))
+@time "Flexible rope pendulum" sol = solve(prob, Rodas4(autodiff=false); u0 = prob.u0 .+ 0.5);
+@test SciMLBase.successful_retcode(sol)
+if true
+ import GLMakie
+ @time "render flexible_rope" render(flexible_rope, sol) # Takes very long time for n>=8
+end
+
+
+
+
+## Resistance in spherical joint
+# This test creates a spherical pendulum and one simple one with Revolute joint. The damping should work the same
+
+
+systems = @named begin
+ joint = Spherical(enforceState=true, isroot=true, phi = [π/2, 0, 0], d = 0.3)
+ bar = FixedTranslation(r = [0, -1, 0])
+ body = Body(; m = 1, isroot = false)
+
+
+ bartop = FixedTranslation(r = [1, 0, 0])
+ joint2 = Multibody.Revolute(n = [1, 0, 0], useAxisFlange = true, isroot = true)
+ bar2 = FixedTranslation(r = [0, 1, 0])
+ body2 = Body(; m = 1, isroot = false)
+ damper = Rotational.Damper(d = 0.3)
+end
+
+connections = [connect(world.frame_b, joint.frame_a)
+ connect(joint.frame_b, bar.frame_a)
+ connect(bar.frame_b, body.frame_a)
+
+ connect(world.frame_b, bartop.frame_a)
+ connect(bartop.frame_b, joint2.frame_a)
+ connect(joint2.frame_b, bar2.frame_a)
+ connect(bar2.frame_b, body2.frame_a)
+
+ connect(joint2.support, damper.flange_a)
+ connect(damper.flange_b, joint2.axis)
+ ]
+
+@named model = ODESystem(connections, t, systems = [world; systems])
+ssys = structural_simplify(IRSystem(model))
+
+prob = ODEProblem(ssys, [
+ D.(joint.phi) .=> 0;
+ D.(D.(joint.phi)) .=> 0;
+ joint2.phi => π/2
+], (0, 10))
+
+sol = solve(prob, Rodas4())
+@assert SciMLBase.successful_retcode(sol)
+
+# plot(sol, idxs = [body.r_0;], layout=3)
+# plot!(sol, idxs = [body2.r_0; ], sp=[1 2 3])
+# render(model, sol)
+
+tt = 0:0.1:10
+@test Matrix(sol(tt, idxs = [collect(body.r_0[2:3]);])) ≈ Matrix(sol(tt, idxs = [collect(body2.r_0[2:3]);]))