add SphericalConstraint

docs/src/examples/ropes_and_cables.md

@@ -98,3 +98,42 @@ Multibody.render(mounted_chain, sol, x=3, filename = "mounted_chain.gif") # May
 ```
 
 ![mounted_chain animation](mounted_chain.gif)
+
+
+## No spring
+
+```@example ropes_and_cables
+using Multibody.Rotations: RotXYZ
+number_of_links = 4
+
+chain_length = 2.1
+x_dist = 0 # Distance between the two mounting points
+
+systems = @named begin
+ # chain = Rope(l = chain_length, m = 5, n=number_of_links, c=0, d_joint=0.2, dir=[1, 0, 0], color=[0.5, 0.5, 0.5, 1], radius=0.05, cutprismatic=false, cutspherical=true)
+ chain = Multibody.CutRope(l = chain_length, m = 5, n=number_of_links, d_joint=0.2, dir=[1, 0, 0])
+ fixed = FixedTranslation(; r=[x_dist, 0, 0]) # Second mounting point
+end
+
+connections = [
+ connect(world.frame_b, fixed.frame_a, chain.frame_a)
+ connect(chain.frame_b, fixed.frame_b)
+]
+
+@named mounted_chain = ODESystem(connections, t, systems = [systems; world])
+
+ssys = structural_simplify(IRSystem(mounted_chain))
+prob = ODEProblem(ssys, [
+ vec(ori(chain.link_1.frame_a).R.mat) .=> vec(RotXYZ(0, 0, 0));
+ vec(ori(chain.joint_3.frame_a).R.mat) .=> vec(RotXYZ(0, pi, 0));
+ chain.joint_3.phi[2] => pi;
+ # collect(chain.joint_4.frame_a.r_0) .=> [0.5, 0, 0];
+ world.g => 1
+], (0, 4))
+
+du = zero(prob.u0)
+prob.f(du, prob.u0, prob.p, 0)
+sol = solve(prob, ImplicitEuler(autodiff=true); initializealg=ShampineCollocationInit())
+@test SciMLBase.successful_retcode(sol)
+Multibody.render(mounted_chain, sol, x=3, filename = "mounted_chain.gif") # May take long time for n>=10
+```

src/Multibody.jl

@@ -141,7 +141,7 @@ include("interfaces.jl")
 export World, world, Mounting1D, Fixed, FixedTranslation, FixedRotation, Body, BodyShape, BodyCylinder, Rope
 include("components.jl")
 
-export Revolute, Prismatic, Spherical, Universal, GearConstraint, RollingWheelJoint,
+export Revolute, Prismatic, Spherical, SphericalConstraint, SphericalSpherical, Universal, GearConstraint, RollingWheelJoint,
  RollingWheel, FreeMotion, RevolutePlanarLoopConstraint
 include("joints.jl")
 

src/components.jl

@@ -444,14 +444,18 @@ function Rope(; name, l = 1, dir = [0,-1, 0], n = 10, m = 1, c = 0, d=0, air_res
  mi = m / n # Segment mass
 
  joints = [Spherical(name=Symbol("joint_$i"),
+ isroot = true,
  # isroot=!(cutspherical && i == 1),
- isroot=true,
- iscut = cutspherical && i == 1,
- # state=!(cutspherical && i == 1),
- state=true,
- color=jointcolor,
- radius=jointradius,
+ iscut = cutspherical && i == n,
+ # state = !(cutspherical && i == 1),
+ state = true,
+ quat = true,
+ color = jointcolor,
+ radius = jointradius,
  d = d_joint) for i = 1:n+1]
+ # if cutspherical
+ # joints[n] = SphericalConstraint(; name=Symbol("joint_$(n)"))
+ # end
 
  eqs = [
  connect(frame_a, joints[1].frame_a)
@@ -485,6 +489,47 @@ function Rope(; name, l = 1, dir = [0,-1, 0], n = 10, m = 1, c = 0, d=0, air_res
  ODESystem(eqs, t; name, systems = [systems; links; joints])
 end
 
+
+function CutRope(; name, l = 1, dir = [0,-1, 0], n = 10, m = 1, c = 0, d=0, air_resistance=0, d_joint = 0, color = [255, 219, 120, 255]./255, radius = 0.05f0, jointradius=0, jointcolor=color, kwargs...)
+
+ @assert n >= 1
+ systems = @named begin
+ frame_a = Frame()
+ frame_b = Frame()
+ end
+ dir = dir / norm(dir)
+
+ li = l / n # Segment length
+ mi = m / n # Segment mass
+
+ joints = [Spherical(name=Symbol("joint_$i"),
+ isroot = true,
+ state = true,
+ color = jointcolor,
+ radius = jointradius,
+ d = d_joint) for i = 1:n-1]
+
+ @named ss = SphericalSpherical(r_0 = li*dir, m=mi)
+
+
+ links = [BodyShape(; m = mi, r = li*dir, name=Symbol("link_$i"), isroot=false, air_resistance, color, radius) for i = 1:n-1]
+
+ eqs = [
+ connect(frame_a, joints[1].frame_a)
+ connect(links[end].frame_b, ss.frame_a)
+ connect(ss.frame_b, frame_b)
+ ]
+
+ for i = 1:n-1
+ push!(eqs, connect(joints[i].frame_b, links[i].frame_a))
+ if i < n-1
+ push!(eqs, connect(links[i].frame_b, joints[i+1].frame_a))
+ end
+ end
+
+ ODESystem(eqs, t; name, systems = [systems; links; joints; ss])
+end
+
 # @component function BodyCylinder(; name, m = 1, r = [0.1, 0, 0], r_0 = 0, r_shape=zeros(3), length = _norm(r - r_shape), kwargs...)
 # @parameters begin
 # # r[1:3]=r, [ # MTKs symbolic language is too weak to handle this as a symbolic parameter in from_nxy

src/joints.jl

@@ -261,6 +261,169 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin
  add_params(sys, pars; name)
 end
 
+@component function SphericalSpherical(; name, state = false, isroot = true, iscut=false, w_rel_a_fixed = false,
+ r_0 = [0,0,0],
+ color = [1, 1, 0, 1],
+ m = 0,
+ radius = 0.1,
+ kinematic_constraint=true
+ )
+
+ @named begin
+ ptf = PartialTwoFrames()
+ Rrel = NumRotationMatrix()
+ end
+ pars = @parameters begin
+ radius = radius, [description = "radius of the joint in animations"]
+ color[1:4] = color, [description = "color of the joint in animations (RGBA)"]
+ end
+ @unpack frame_a, frame_b = ptf
+ # @parameters begin # Currently not using parameters due to these appearing in if statements
+ # sequence[1:3] = sequence
+ # end
+
+ @variables f_rod(t), [description="Constraint force in direction of the rod (positive on frame_a, when directed from frame_a to frame_b)";]
+ @variables rRod_0(t)[1:3]=r_0, [description="Position vector from frame_a to frame_b resolved in world frame";]
+ @variables rRod_a(t)[1:3], [description="Position vector from frame_a to frame_b resolved in frame_a";]
+ @variables eRod_a(t)[1:3], [description="Unit vector in direction from frame_a to frame_b, resolved in frame_a";]
+ @variables r_CM_0(t)[1:3], [description="Dummy if m==0, or position vector from world frame to mid-point of rod, resolved in world frame";]
+ @variables v_CM_0(t)[1:3], [description="First derivative of r_CM_0";]
+ @variables f_CM_a(t)[1:3], [description="Dummy if m==0, or inertial force acting at mid-point of rod due to mass point acceleration, resolved in frame_a";]
+ @variables f_CM_e(t)[1:3], [description="Dummy if m==0, or projection of f_CM_a onto eRod_a, resolved in frame_a";]
+ @variables f_b_a1(t)[1:3], [description="Force acting at frame_b, but without force in rod, resolved in frame_a";]
+
+ rRod_0 = collect(rRod_0)
+ rRod_a = collect(rRod_a)
+ eRod_a = collect(eRod_a)
+ r_CM_0 = collect(r_CM_0)
+ v_CM_0 = collect(v_CM_0)
+ f_CM_a = collect(f_CM_a)
+ f_CM_e = collect(f_CM_e)
+ f_b_a1 = collect(f_b_a1)
+ rodlength = _norm(r_0)
+ constraint_residue = rRod_0'rRod_0 - rodlength^2
+
+
+ eqs = [
+ # Determine relative position vector between the two frames
+ if kinematic_constraint
+ rRod_0 .~ transpose(ori(frame_b).R)*(ori(frame_b)*collect(frame_b.r_0)) - transpose(ori(frame_a).R)*(ori(frame_a)*collect(frame_a.r_0))
+ else
+ rRod_0 .~ frame_b.r_0 - frame_a.r_0
+ end
+
+ #rRod_0 = frame_b.r_0 - frame_a.r_0;
+ rRod_a .~ resolve2(ori(frame_a), rRod_0)
+ eRod_a .~ rRod_a/rodlength
+
+ # Constraint equation
+ constraint_residue ~ 0
+
+ # Cut-torques at frame_a and frame_b
+ frame_a.tau .~ zeros(3)
+ frame_b.tau .~ zeros(3)
+
+ #= Force and torque balance of rod
+ - Kinematics for center of mass CM of mass point
+ r_CM_0 = frame_a.r_0 + rRod_0/2;
+ v_CM_0 = der(r_CM_0);
+ a_CM_a = resolve2(ori(frame_a), der(v_CM_0) - world.gravity_acceleration(r_CM_0));
+ - Inertial and gravity force in direction (f_CM_e) and orthogonal (f_CM_n) to rod
+ f_CM_a = m*a_CM_a
+ f_CM_e = f_CM_a*eRod_a; # in direction of rod
+ f_CM_n = rodlength(f_CM_a - f_CM_e); # orthogonal to rod
+ - Force balance in direction of rod
+ f_CM_e = fa_rod_e + fb_rod_e;
+ - Force balance orthogonal to rod
+ f_CM_n = fa_rod_n + fb_rod_n;
+ - Torque balance with respect to frame_a
+ 0 = (-f_CM_n)*rodlength/2 + fb_rod_n*rodlength
+ The result is:
+ fb_rod_n = f_CM_n/2;
+ fa_rod_n = fb_rod_n;
+ fb_rod_e = f_CM_e - fa_rod_e;
+ fa_rod_e is the unknown computed from loop
+ =#
+
+ # f_b_a1 is needed in aggregation joints to solve kinematic loops analytically
+ if m > 0
+ [r_CM_0 .~ frame_a.r_0 + rRod_0/2;
+ v_CM_0 .~ D.(r_CM_0);
+ f_CM_a .~ m*resolve2(ori(frame_a), D.(v_CM_0) - gravity_acceleration(r_CM_0))
+ f_CM_e .~ (f_CM_a'eRod_a)*eRod_a
+ frame_a.f .~ (f_CM_a - f_CM_e)./2 + f_rod*eRod_a
+ f_b_a1 .~ (f_CM_a + f_CM_e)./2
+ frame_b.f .~ resolve_relative(f_b_a1 - f_rod*eRod_a, ori(frame_a),
+ ori(frame_b));
+ ]
+ else
+ [r_CM_0 .~ zeros(3);
+ v_CM_0 .~ zeros(3);
+ f_CM_a .~ zeros(3);
+ f_CM_e .~ zeros(3);
+ f_b_a1 .~ zeros(3);
+ frame_a.f .~ f_rod*eRod_a;
+ frame_b.f .~ -resolve_relative(frame_a.f, ori(frame_a), ori(frame_b));
+ ]
+ end
+ ]
+
+
+ sys = extend(ODESystem(eqs, t; name=:nothing), ptf)
+ add_params(sys, pars; name)
+end
+
+@component function SphericalConstraint(; name, sequence = [1, 2, 3],
+ color = [1, 1, 0, 1],
+ radius = 0.1,
+ x_locked = true, y_locked = true, z_locked = true,
+)
+
+ @named begin
+ ptf = PartialTwoFrames()
+ Rrel = NumRotationMatrix()
+ Rrel_inv = NumRotationMatrix()
+ end
+ pars = @parameters begin
+ radius = radius, [description = "radius of the joint in animations"]
+ color[1:4] = color, [description = "color of the joint in animations (RGBA)"]
+ end
+ @unpack frame_a, frame_b = ptf
+ @variables begin (r_rel_a(t)[1:3] = zeros(3)),
+ [
+ description = "Position vector from origin of frame_a to origin of frame_b, resolved in frame_a",
+ ] end
+
+ Rrel = relative_rotation(frame_a, frame_b)
+
+ eqs = [
+ if x_locked
+ r_rel_a[1] ~ 0
+ else
+ frame_a.f[1] ~ 0
+ end
+
+ if y_locked
+ r_rel_a[2] ~ 0
+ else
+ frame_a.f[2] ~ 0
+ end
+
+ if z_locked
+ r_rel_a[3] ~ 0
+ else
+ frame_a.f[3] ~ 0
+ end
+ r_rel_a .~ resolve2(ori(frame_a), frame_b.r_0 - frame_a.r_0);
+ zeros(3) .~ collect(frame_b.tau);
+ collect(frame_b.f) .~ -resolve2(Rrel, frame_a.f);
+ zeros(3) .~ collect(frame_a.tau) + resolve1(Rrel, frame_b.tau) - cross(r_rel_a, frame_a.f);
+ ]
+
+ sys = extend(ODESystem(eqs, t; name=:nothing), ptf)
+ add_params(sys, pars; name)
+end
+
 @component function Universal(; name, n_a = [1, 0, 0], n_b = [0, 1, 0], phi_a = 0,
  phi_b = 0,
  w_a = 0,

test/runtests.jl

@@ -1036,3 +1036,25 @@ using LinearAlgebra
 end
 
 ##
+
+
+@mtkmodel TestSphericalSpherical begin
+ @components begin
+ world = W()
+ ss = SphericalSpherical(r_0 = [1, 0, 0], m = 1, kinematic_constraint=true)
+ ss2 = BodyShape(r = [0, 0, 1], m = 1)
+ s = Spherical()
+ trans = FixedTranslation(r = [1,0,1])
+ end
+ @equations begin
+ connect(world.frame_b, ss.frame_a, trans.frame_a)
+ connect(ss.frame_b, ss2.frame_a)
+ connect(ss2.frame_b, s.frame_a)
+ connect(s.frame_b, trans.frame_b)
+ end
+end
+
+@named model = TestSphericalSpherical()
+model = complete(model)
+ssys = structural_simplify(IRSystem(model))
+