From 1c1968760d45a46d98dbb76395aecb80aaefaecb Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Wed, 12 Jun 2024 17:26:04 +0200 Subject: [PATCH] follow a more julian naming convention --- docs/src/examples/free_motion.md | 4 +- docs/src/examples/gearbox.md | 2 +- docs/src/examples/kinematic_loops.md | 4 +- docs/src/examples/pendulum.md | 12 +-- docs/src/examples/sensors.md | 2 +- docs/src/examples/spherical_pendulum.md | 2 +- docs/src/examples/spring_damper_system.md | 2 +- docs/src/examples/spring_mass_system.md | 4 +- docs/src/examples/three_springs.md | 6 +- docs/src/index.md | 2 +- examples/JuliaSim_logo.jl | 6 +- examples/fourbar.jl | 4 +- src/Multibody.jl | 1 + src/components.jl | 30 +++--- src/forces.jl | 80 +++++++-------- src/joints.jl | 106 ++++++++++---------- src/orientation.jl | 44 ++++----- src/robot/FullRobot.jl | 114 +++++++++++----------- src/robot/path_planning.jl | 92 ++++++++--------- src/robot/ptp.jl | 19 +++- src/robot/robot_components.jl | 12 +-- src/sensors.jl | 46 ++++----- test/runtests.jl | 50 +++++----- test/test_robot.jl | 4 +- test/test_traj.jl | 10 +- 25 files changed, 338 insertions(+), 320 deletions(-) diff --git a/docs/src/examples/free_motion.md b/docs/src/examples/free_motion.md index 1c5ce5e5..9cb58701 100644 --- a/docs/src/examples/free_motion.md +++ b/docs/src/examples/free_motion.md @@ -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 @@ -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) diff --git a/docs/src/examples/gearbox.md b/docs/src/examples/gearbox.md index d1e57f3c..becacf2a 100644 --- a/docs/src/examples/gearbox.md +++ b/docs/src/examples/gearbox.md @@ -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) diff --git a/docs/src/examples/kinematic_loops.md b/docs/src/examples/kinematic_loops.md index 123686f1..1cab2f0e 100644 --- a/docs/src/examples/kinematic_loops.md +++ b/docs/src/examples/kinematic_loops.md @@ -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() diff --git a/docs/src/examples/pendulum.md b/docs/src/examples/pendulum.md index 345f59de..2d2e2b70 100644 --- a/docs/src/examples/pendulum.md +++ b/docs/src/examples/pendulum.md @@ -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) @@ -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) @@ -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) @@ -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) diff --git a/docs/src/examples/sensors.md b/docs/src/examples/sensors.md index 86056d77..5ff89c83 100644 --- a/docs/src/examples/sensors.md +++ b/docs/src/examples/sensors.md @@ -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 diff --git a/docs/src/examples/spherical_pendulum.md b/docs/src/examples/spherical_pendulum.md index 4bbabeee..1858aef7 100644 --- a/docs/src/examples/spherical_pendulum.md +++ b/docs/src/examples/spherical_pendulum.md @@ -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 diff --git a/docs/src/examples/spring_damper_system.md b/docs/src/examples/spring_damper_system.md index f982cc65..c1d749fc 100644 --- a/docs/src/examples/spring_damper_system.md +++ b/docs/src/examples/spring_damper_system.md @@ -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) diff --git a/docs/src/examples/spring_mass_system.md b/docs/src/examples/spring_mass_system.md index 48d6e8e2..a20c5b4e 100644 --- a/docs/src/examples/spring_mass_system.md +++ b/docs/src/examples/spring_mass_system.md @@ -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 = [ diff --git a/docs/src/examples/three_springs.md b/docs/src/examples/three_springs.md index 90f1e8bb..4cb233e2 100644 --- a/docs/src/examples/three_springs.md +++ b/docs/src/examples/three_springs.md @@ -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 @@ -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) @@ -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))) diff --git a/docs/src/index.md b/docs/src/index.md index bb7913b4..376b6435 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -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] diff --git a/examples/JuliaSim_logo.jl b/examples/JuliaSim_logo.jl index 5d79b016..81ac801c 100644 --- a/examples/JuliaSim_logo.jl +++ b/examples/JuliaSim_logo.jl @@ -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) diff --git a/examples/fourbar.jl b/examples/fourbar.jl index 72fe33a3..076df41e 100644 --- a/examples/fourbar.jl +++ b/examples/fourbar.jl @@ -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() diff --git a/src/Multibody.jl b/src/Multibody.jl index 2e117d64..0f4126dc 100644 --- a/src/Multibody.jl +++ b/src/Multibody.jl @@ -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") diff --git a/src/components.jl b/src/components.jl index 22ec5465..56648e3d 100644 --- a/src/components.jl +++ b/src/components.jl @@ -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]) @@ -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 @@ -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)] @@ -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", @@ -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] @@ -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 @@ -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) @@ -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; @@ -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() @@ -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 @@ -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) @@ -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)) diff --git a/src/forces.jl b/src/forces.jl index e079b332..c3f2925c 100644 --- a/src/forces.jl +++ b/src/forces.jl @@ -3,11 +3,11 @@ import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica as TP import ModelingToolkitStandardLibrary.Blocks """ - BasicTorque(; name, resolveInFrame = :world) + BasicTorque(; name, resolve_frame = :world) Low-level torque component used to build [`Torque`](@ref) """ -function BasicTorque(; name, resolveInFrame = :world) +function BasicTorque(; name, resolve_frame = :world) @named ptf = PartialTwoFrames() @named torque = Blocks.RealInput(; nin = 3) @unpack frame_a, frame_b = ptf @@ -27,20 +27,20 @@ function BasicTorque(; name, resolveInFrame = :world) # torque balance zeros(3) .~ collect(frame_a.tau) + resolve2(frame_a, t_b_0)] - if resolveInFrame == :frame_a + if resolve_frame == :frame_a append!(eqs, [t_b_0 .~ -resolve1(frame_a, torque.u) collect(frame_b.tau) .~ resolve2(frame_b, t_b_0)]) - elseif resolveInFrame == :frame_b + elseif resolve_frame == :frame_b append!(eqs, [t_b_0 .~ -resolve1(frame_b, torque.u) collect(frame_b.tau) .~ collect(-torque.u)]) - elseif resolveInFrame == :world + elseif resolve_frame == :world append!(eqs, [t_b_0 .~ collect(-torque.u) collect(frame_b.tau) .~ resolve2(frame_b, t_b_0)]) else - error("Unknown value of argument resolveInFrame") + error("Unknown value of argument resolve_frame") append!(eqs, [t_b_0 .~ zeros(3) collect(frame_b.tau) .~ zeros(3)]) end @@ -49,24 +49,24 @@ function BasicTorque(; name, resolveInFrame = :world) end """ - Torque(; name, resolveInFrame = :frame_b) + Torque(; name, resolve_frame = :frame_b) Torque acting between two frames, defined by 3 input signals and resolved in frame `world`, `frame_a`, `frame_b` (default) # Connectors: - `frame_a` - `frame_b` -- `torque`: Of type `Blocks.RealInput(3)`. x-, y-, z-coordinates of torque resolved in frame defined by `resolveInFrame`. +- `torque`: Of type `Blocks.RealInput(3)`. x-, y-, z-coordinates of torque resolved in frame defined by `resolve_frame`. # Keyword arguments: -- `resolveInFrame`: The frame in which the cut force and cut torque are resolved. Default is `:frame_b`, options include `:frame_a` and `:world`. +- `resolve_frame`: The frame in which the cut force and cut torque are resolved. Default is `:frame_b`, options include `:frame_a` and `:world`. """ -function Torque(; name, resolveInFrame = :frame_b) +function Torque(; name, resolve_frame = :frame_b) @named ptf = PartialTwoFrames() @unpack frame_a, frame_b = ptf @named begin torque = Blocks.RealInput(; nin = 3) - basicTorque = BasicTorque(; resolveInFrame = resolveInFrame) + basicTorque = BasicTorque(; resolve_frame = resolve_frame) end eqs = [connect(basicTorque.frame_a, frame_a) @@ -75,7 +75,7 @@ function Torque(; name, resolveInFrame = :frame_b) extend(ODESystem(eqs, t, name = name, systems = [torque, basicTorque]), ptf) end -function BasicForce(; name, resolveInFrame = :frame_b) +function BasicForce(; name, resolve_frame = :frame_b) @named ptf = PartialTwoFrames() @named force = Blocks.RealInput(; nin = 3) @unpack frame_a, frame_b = ptf @@ -93,44 +93,44 @@ function BasicForce(; name, resolveInFrame = :frame_b) 0 .~ collect(frame_a.f) + resolve2(frame_a, f_b_0) 0 .~ collect(frame_a.tau) + resolve2(frame_a, cross(r_0, f_b_0))] - if resolveInFrame == :frame_a + if resolve_frame == :frame_a append!(eqs, [f_b_0 .~ -resolve1(frame_a, force.u) collect(frame_b.tau) .~ resolve2(frame_b, f_b_0)]) - elseif resolveInFrame == :frame_b + elseif resolve_frame == :frame_b append!(eqs, [f_b_0 .~ -resolve1(frame_b, force.u) collect(frame_b.tau) .~ collect(-force.u)]) - elseif resolveInFrame == :world + elseif resolve_frame == :world append!(eqs, [f_b_0 .~ collect(-force.u) collect(frame_b.tau) .~ resolve2(frame_b, f_b_0)]) else - error("Unknown value of argument resolveInFrame") + error("Unknown value of argument resolve_frame") end extend(ODESystem(eqs, t, name = name, systems = [force]), ptf) end """ - Force(; name, resolveInFrame = :frame_b) + Force(; name, resolve_frame = :frame_b) Force acting between two frames, defined by 3 input signals and resolved in frame `world`, `frame_a`, `frame_b` (default) # Connectors: - `frame_a` - `frame_b` -- `force`: Of type `Blocks.RealInput(3)`. x-, y-, z-coordinates of force resolved in frame defined by `resolveInFrame`. +- `force`: Of type `Blocks.RealInput(3)`. x-, y-, z-coordinates of force resolved in frame defined by `resolve_frame`. # Keyword arguments: -- `resolveInFrame`: The frame in which the cut force and cut torque are resolved. Default is `:frame_b`, options include `:frame_a` and `:world`. +- `resolve_frame`: The frame in which the cut force and cut torque are resolved. Default is `:frame_b`, options include `:frame_a` and `:world`. """ -function Force(; name, resolveInFrame = :frame_b) +function Force(; name, resolve_frame = :frame_b) @named ptf = PartialTwoFrames() @unpack frame_a, frame_b = ptf @named begin - force = Blocks.RealInput(; nin = 3) # x-, y-, z-coordinates of force resolved in frame defined by resolveInFrame - basicForce = BasicForce(; resolveInFrame = resolveInFrame) + force = Blocks.RealInput(; nin = 3) # x-, y-, z-coordinates of force resolved in frame defined by resolve_frame + basicForce = BasicForce(; resolve_frame = resolve_frame) end eqs = [connect(basicForce.frame_a, frame_a) @@ -139,8 +139,8 @@ function Force(; name, resolveInFrame = :frame_b) extend(ODESystem(eqs, t, name = name, systems = [force, basicForce]), ptf) end -function LineForceBase(; name, length = 0, s_small = 1e-10, fixedRotationAtFrame_a = false, - fixedRotationAtFrame_b = false, r_rel_0 = 0, s0 = 0) +function LineForceBase(; name, length = 0, s_small = 1e-10, fixed_rotation_at_frame_a = false, + fixed_rotation_at_frame_b = false, r_rel_0 = 0, s0 = 0) @named frame_a = Frame() @named frame_b = Frame() @@ -164,16 +164,16 @@ function LineForceBase(; name, length = 0, s_small = 1e-10, fixedRotationAtFrame # Modelica stdlib has the option to inser special equations when two line forces are connected, this option does not yet exisst here https://github.com/modelica/ModelicaStandardLibrary/blob/10238e9927e2078571e41b53cda128c5207f69f7/Modelica/Mechanics/MultiBody/Interfaces/LineForceBase.mo#L49 - if fixedRotationAtFrame_a + if fixed_rotation_at_frame_a # TODO: frame_a.R should be rooted here - eqs = [eqs; vec(ori(frame_a).R .~ nullRotation().R)] + eqs = [eqs; vec(ori(frame_a).R .~ nullrotation().R)] else eqs = [eqs; frame_a.tau .~ 0] end - if fixedRotationAtFrame_b + if fixed_rotation_at_frame_b # TODO: frame_b.R should be rooted here - eqs = [eqs; vec(ori(frame_b).R .~ nullRotation().R)] + eqs = [eqs; vec(ori(frame_b).R .~ nullrotation().R)] else eqs = [eqs; frame_b.tau .~ 0] end @@ -258,7 +258,7 @@ function PartialLineForce(; name, kwargs...) # Determine forces and torques at frame_a and frame_b collect(frame_a.f) .~ collect(-e_a * f) - collect(frame_b.f) .~ -resolve2(relativeRotation(frame_a, frame_b), + collect(frame_b.f) .~ -resolve2(relative_rotation(frame_a, frame_b), frame_a.f)] extend(ODESystem(equations, t; name), lfb) end @@ -293,7 +293,7 @@ See also [`SpringDamperParallel`](@ref) @component function Spring(; c, name, m = 0, lengthFraction = 0.5, s_unstretched = 0, kwargs...) @named ptf = PartialTwoFrames() @unpack frame_a, frame_b = ptf - @named lineForce = LineForceWithMass(; length = s_unstretched, m, lengthFraction, + @named lineforce = LineForceWithMass(; length = s_unstretched, m, lengthFraction, kwargs...) # @parameters c=c [description = "spring constant", bounds = (0, Inf)] @@ -332,16 +332,16 @@ See also [`SpringDamperParallel`](@ref) r_rel_a .~ resolve2(ori(frame_a), r_rel_0) e_a .~ r_rel_a / s f ~ spring2d.f - length ~ lineForce.length - s ~ lineForce.s - r_rel_0 .~ lineForce.r_rel_0 - e_rel_0 .~ lineForce.e_rel_0 - connect(lineForce.frame_a, frame_a) - connect(lineForce.frame_b, frame_b) - connect(spring2d.flange_b, lineForce.flange_b) - connect(spring2d.flange_a, lineForce.flange_a)] - - extend(ODESystem(eqs, t; name, systems = [lineForce, spring2d]), ptf) + length ~ lineforce.length + s ~ lineforce.s + r_rel_0 .~ lineforce.r_rel_0 + e_rel_0 .~ lineforce.e_rel_0 + connect(lineforce.frame_a, frame_a) + connect(lineforce.frame_b, frame_b) + connect(spring2d.flange_b, lineforce.flange_b) + connect(spring2d.flange_a, lineforce.flange_a)] + + extend(ODESystem(eqs, t; name, systems = [lineforce, spring2d]), ptf) end """ diff --git a/src/joints.jl b/src/joints.jl index f6687551..ea4e70be 100644 --- a/src/joints.jl +++ b/src/joints.jl @@ -4,20 +4,20 @@ function add_params(sys, params; name) end """ - Revolute(; name, phi0 = 0, w0 = 0, n, useAxisFlange = false) + Revolute(; name, phi0 = 0, w0 = 0, n, axisflange = false) Revolute joint with 1 rotational degree-of-freedom - `phi0`: Initial angle - `w0`: Iniitial angular velocity - `n`: The axis of rotation -- `useAxisFlange`: If true, the joint will have two additional frames from Mechanical.Rotational, `axis` and `support`, between which rotational components such as springs and dampers can be connected. +- `axisflange`: If true, the joint will have two additional frames from Mechanical.Rotational, `axis` and `support`, between which rotational components such as springs and dampers can be connected. -If `useAxisFlange`, flange connectors for ModelicaStandardLibrary.Mechanics.Rotational are also available: +If `axisflange`, flange connectors for ModelicaStandardLibrary.Mechanics.Rotational are also available: - `axis`: 1-dim. rotational flange that drives the joint - `support`: 1-dim. rotational flange of the drive support (assumed to be fixed in the world frame, NOT in the joint) """ -@component function Revolute(; name, phi0 = 0, w0 = 0, n = Float64[0, 0, 1], useAxisFlange = false, +@component function Revolute(; name, phi0 = 0, w0 = 0, n = Float64[0, 0, 1], axisflange = false, isroot = true, iscut = false, radius = 0.05, color = [0.5019608f0,0.0f0,0.5019608f0,1.0f0], state_priority = 3.0) norm(n) ≈ 1 || error("Axis of rotation must be a unit vector") @named frame_a = Frame() @@ -44,18 +44,18 @@ If `useAxisFlange`, flange connectors for ModelicaStandardLibrary.Mechanics.Rota if iscut # NOTE: only equations for isroot=false available here eqs = Equation[Rrel ~ planar_rotation(-n, phi, w) - residue(ori(frame_a), absoluteRotation(ori(frame_b), Rrel)) .~ 0 # If joint is a cut joint, this equation is replaced + residue(ori(frame_a), absolute_rotation(ori(frame_b), Rrel)) .~ 0 # If joint is a cut joint, this equation is replaced collect(frame_b.f) .~ -resolve1(Rrel, frame_a.f) collect(frame_b.tau) .~ -resolve1(Rrel, frame_a.tau)] else if isroot eqs = Equation[Rrel ~ planar_rotation(n, phi, w) - ori(frame_b) ~ absoluteRotation(ori(frame_a), Rrel) + ori(frame_b) ~ absolute_rotation(ori(frame_a), Rrel) collect(frame_a.f) .~ -resolve1(Rrel, frame_b.f) collect(frame_a.tau) .~ -resolve1(Rrel, frame_b.tau)] else eqs = Equation[Rrel ~ planar_rotation(-n, phi, w) - ori(frame_a) ~ absoluteRotation(ori(frame_b), Rrel) + ori(frame_a) ~ absolute_rotation(ori(frame_b), Rrel) collect(frame_b.f) .~ -resolve1(Rrel, frame_a.f) collect(frame_b.tau) .~ -resolve1(Rrel, frame_a.tau)] end @@ -64,7 +64,7 @@ If `useAxisFlange`, flange connectors for ModelicaStandardLibrary.Mechanics.Rota D(phi) ~ w tau ~ -collect(frame_b.tau)'n] append!(eqs, moreeqs) - sys = if useAxisFlange + sys = if axisflange # @named internalAxis = Rotational.InternalSupport(tau=tau) @named fixed = Rotational.Fixed() @@ -87,21 +87,21 @@ If `useAxisFlange`, flange connectors for ModelicaStandardLibrary.Mechanics.Rota end """ - Prismatic(; name, n = [0, 0, 1], useAxisFlange = false, isroot = true) + Prismatic(; name, n = [0, 0, 1], axisflange = false, isroot = true) Prismatic joint with 1 translational degree-of-freedom - `n`: The axis of motion (unit vector) -- `useAxisFlange`: If true, the joint will have two additional frames from Mechanical.Translational, `axis` and `support`, between which translational components such as springs and dampers can be connected. +- `axisflange`: If true, the joint will have two additional frames from Mechanical.Translational, `axis` and `support`, between which translational components such as springs and dampers can be connected. - `isroot`: If true, the joint will be considered the root of the system. -If `useAxisFlange`, flange connectors for ModelicaStandardLibrary.Mechanics.TranslationalModelica are also available: +If `axisflange`, flange connectors for ModelicaStandardLibrary.Mechanics.TranslationalModelica are also available: - `axis`: 1-dim. translational flange that drives the joint - `support`: 1-dim. translational flange of the drive support (assumed to be fixed in the world frame, NOT in the joint) The function returns an ODESystem representing the prismatic joint. """ -@component function Prismatic(; name, n = Float64[0, 0, 1], useAxisFlange = false, +@component function Prismatic(; name, n = Float64[0, 0, 1], axisflange = false, isroot = true, s0 = 0, v0 = 0) norm(n) ≈ 1 || error("Axis of motion must be a unit vector") @named frame_a = Frame() @@ -140,7 +140,7 @@ The function returns an ODESystem representing the prismatic joint. # d'Alemberts principle f ~ -(n'collect(frame_b.f))[]] - if useAxisFlange + if axisflange @named fixed = Translational.Fixed(s0=0) @named axis = Translational.Flange() @named support = Translational.Flange() @@ -155,16 +155,16 @@ 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, d = 0) + Spherical(; name, state = false, isroot = true, w_rel_a_fixed = false, z_rel_a_fixed = false, sequence, 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. +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 `state` 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` (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 +- `isroot`: Indicate that `frame_a` is the root, otherwise `frame_b` is the root. Only relevant if `state = true`. +- `sequence`: 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, +@component function Spherical(; name, state = false, isroot = true, w_rel_a_fixed = false, + z_rel_a_fixed = false, sequence = [1, 2, 3], phi = 0, phi_d = 0, d = 0, phi_dd = 0) @@ -175,7 +175,7 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin end @unpack frame_a, frame_b = ptf # @parameters begin # Currently not using parameters due to these appearing in if statements - # sequence_angleStates[1:3] = sequence_angleStates + # sequence[1:3] = sequence # end @variables begin (w_rel(t)[1:3] = zeros(3)), [ @@ -194,7 +194,7 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin collect(frame_b.r_0) .~ collect(frame_a.r_0)] end - if enforceState + if state @variables begin (phi(t)[1:3] = phi), [state_priority = 10, description = "3 angles to rotate frame_a into frame_b"] @@ -204,18 +204,18 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin [state_priority = 10, description = "3 angle second derivatives"] end append!(eqs, - [R_rel ~ axesRotations(sequence_angleStates, phi, phi_d) - collect(w_rel) .~ angularVelocity2(R_rel) + [R_rel ~ axes_rotations(sequence, phi, phi_d) + collect(w_rel) .~ angular_velocity2(R_rel) collect(phi_d .~ D.(phi)) collect(phi_dd .~ D.(phi_d))]) if isroot append!(eqs, - [ori(frame_b) ~ absoluteRotation(frame_a, R_rel) + [ori(frame_b) ~ absolute_rotation(frame_a, R_rel) zeros(3) .~ collect(frame_a.f) + resolve1(R_rel, frame_b.f)]) else append!(eqs, - [R_rel_inv ~ inverseRotation(R_rel) - ori(frame_a) ~ absoluteRotation(frame_b, R_rel_inv) + [R_rel_inv ~ inverse_rotation(R_rel) + ori(frame_a) ~ absolute_rotation(frame_b, R_rel_inv) zeros(3) .~ collect(frame_b.f) + resolve2(R_rel, frame_a.f)]) end @@ -224,10 +224,10 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin append!(eqs, #frame_b.r_0 ~ transpose(frame_b.R.T)*(frame_b.R.T*(transpose(frame_a.R.T)*(frame_a.R.T*frame_a.r_0))); zeros(3) .~ collect(frame_a.f) + - resolveRelative(frame_b.f, frame_b, frame_a)) + resolve_relative(frame_b.f, frame_b, frame_a)) if w_rel_a_fixed || z_rel_a_fixed - append!(w_rel .~ angularVelocity2(frame_b) - resolve2(frame_b, - angularVelocity1(frame_a))) + append!(w_rel .~ angular_velocity2(frame_b) - resolve2(frame_b, + angular_velocity1(frame_a))) else append!(w_rel .~ zeros(3)) end @@ -319,14 +319,14 @@ This ideal massless joint provides a gear constraint between frames `frame_a` an systems = @named begin bearing = Frame() #"Coordinate system fixed in the bearing" - actuatedRevolute_a = Revolute(useAxisFlange = true, + actuatedRevolute_a = Revolute(axisflange = true, n = n_a) - actuatedRevolute_b = Revolute(useAxisFlange = true, + actuatedRevolute_b = Revolute(axisflange = true, n = n_b) idealGear = Rotational.IdealGear(ratio = ratio) - fixedTranslation1 = FixedTranslation(r = r_b) - fixedTranslation2 = FixedTranslation(r = r_a) + translation1 = FixedTranslation(r = r_b) + translation2 = FixedTranslation(r = r_a) end @unpack frame_a, frame_b = ptf @@ -378,10 +378,10 @@ This ideal massless joint provides a gear constraint between frames `frame_a` an a_b ~ D(w_b) connect(actuatedRevolute_a.axis, idealGear.flange_a) connect(idealGear.flange_b, actuatedRevolute_b.axis) - connect(actuatedRevolute_a.frame_a, fixedTranslation2.frame_b) - connect(fixedTranslation2.frame_a, bearing) - connect(fixedTranslation1.frame_a, bearing) - connect(fixedTranslation1.frame_b, actuatedRevolute_b.frame_a) + connect(actuatedRevolute_a.frame_a, translation2.frame_b) + connect(translation2.frame_a, bearing) + connect(translation1.frame_a, bearing) + connect(translation1.frame_b, actuatedRevolute_b.frame_a) connect(frame_a, actuatedRevolute_a.frame_b) connect(actuatedRevolute_b.frame_b, frame_b)] @@ -391,9 +391,9 @@ This ideal massless joint provides a gear constraint between frames `frame_a` an totalPower ~ frame_a.f'resolve2(frame_a, D.(frame_a.r_0)) + frame_b.f'resolve2(frame_b, D.(frame_b.r_0)) + bearing.f'resolve2(bearing, D.(bearing.r_0)) + - frame_a.tau'angularVelocity2(frame_a) + - frame_b.tau'angularVelocity2(frame_b) + - bearing.tau'angularVelocity2(bearing)) + frame_a.tau'angular_velocity2(frame_a) + + frame_b.tau'angular_velocity2(frame_b) + + bearing.tau'angular_velocity2(bearing)) end extend(ODESystem(eqs, t; name, systems), ptf) @@ -515,7 +515,7 @@ function RollingWheelJoint(; name, radius, angles = zeros(3), x0, y0, z0 = 0) # frame_a.R is computed from generalized coordinates collect(frame_a.r_0) .~ [x, y, z] collect(der_angles) .~ D.(angles) - ori(frame_a) ~ axesRotations([3, 2, 1], angles, der_angles) + ori(frame_a) ~ axes_rotations([3, 2, 1], angles, der_angles) # Road description collect(r_road_0) .~ [s, w, 0] @@ -632,7 +632,7 @@ with the wheel itself. 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) + FreeMotion(; name, state = 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. @@ -644,7 +644,7 @@ The relative position vector `r_rel_a` from the origin of `frame_a` to the origi # Arguments -- `enforceState`: Enforce this joint having state, this is often desired and is the default choice. +- `state`: Enforce this joint having state, this is often desired and is the default choice. - `sequence`: Rotation sequence - `w_rel_a_fixed`: = true, if `w_rel_a_start` are used as initial values, else as guess values - `z_rel_a_fixed`: = true, if `z_rel_a_start` are used as initial values, else as guess values @@ -658,7 +658,7 @@ The relative position vector `r_rel_a` from the origin of `frame_a` to the origi - `v_rel_a` - `a_rel_a` """ -@component function FreeMotion(; name, enforceState = true, sequence = [1, 2, 3], isroot = true, +@component function FreeMotion(; name, state = true, sequence = [1, 2, 3], isroot = true, w_rel_a_fixed = false, z_rel_a_fixed = false, phi = 0, phi_d = 0, phi_dd = 0, @@ -707,28 +707,28 @@ The relative position vector `r_rel_a` from the origin of `frame_a` to the origi # Kinematic relationships frame_b.r_0 .~ frame_a.r_0 .+ resolve1(frame_a, r_rel_a)] - if enforceState + if state if isroot append!(eqs, - ori(frame_b) ~ absoluteRotation(frame_a, R_rel)) + ori(frame_b) ~ absolute_rotation(frame_a, R_rel)) else append!(eqs, - [R_rel_inv ~ inverseRotation(R_rel) - ori(frame_a) ~ absoluteRotation(frame_b, R_rel_inv)]) + [R_rel_inv ~ inverse_rotation(R_rel) + ori(frame_a) ~ absolute_rotation(frame_b, R_rel_inv)]) end append!(eqs, [phi_d .~ D.(phi) phi_dd .~ D.(phi_d) - R_rel ~ axesRotations(sequence, phi, phi_d) - w_rel_b .~ angularVelocity2(R_rel)]) + R_rel ~ axes_rotations(sequence, phi, phi_d) + w_rel_b .~ angular_velocity2(R_rel)]) else # Free motion joint does not have state if w_rel_a_fixed || z_rel_a_fixed append!(eqs, - w_rel_b .~ angularVelocity2(frame_b) - resolve2(frame_b. - R, angularVelocity1(frame_a))) + w_rel_b .~ angular_velocity2(frame_b) - resolve2(frame_b. + R, angular_velocity1(frame_a))) end end compose(ODESystem(eqs, t; name), frame_a, frame_b) @@ -788,7 +788,7 @@ If a planar loop is present, e.g., consisting of 4 revolute joints where the joi Rb = ori(frame_b) eqs = [ - R_rel ~ relativeRotation(ori(frame_a), ori(frame_b)) + R_rel ~ relative_rotation(ori(frame_a), ori(frame_b)) r_rel_a .~ resolve2(ori(frame_a), collect(frame_b.r_0 - frame_a.r_0)) 0 ~ (ex_a'r_rel_a)[] 0 ~ (ey_a'r_rel_a)[] diff --git a/src/orientation.jl b/src/orientation.jl index 297d1f09..f1a9721d 100644 --- a/src/orientation.jl +++ b/src/orientation.jl @@ -51,7 +51,7 @@ function NumRotationMatrix(; R = collect(1.0I(3)), w = zeros(3), name, varw = fa RotationMatrix(R, w) end -nullRotation() = RotationMatrix() +nullrotation() = RotationMatrix() function ModelingToolkit.ODESystem(RM::RotationMatrix; name) # @variables R(t)[1:3, 1:3]=Matrix(RM) [description="Orientation rotation matrix ∈ SO(3)"] @@ -99,7 +99,7 @@ end function get_w(Q::AbstractVector) Q = collect(Q) DQ = collect(D.(Q)) - angularVelocity2(Q, DQ) + angular_velocity2(Q, DQ) end """ @@ -131,7 +131,7 @@ resolve1(R21::RotationMatrix, v2) = R21'collect(v2) resolve1(sys::ODESystem, v) = resolve1(ori(sys), v) resolve2(sys::ODESystem, v) = resolve2(ori(sys), v) -function resolveRelative(v1, R1, R2) +function resolve_relative(v1, R1, R2) R1 isa ODESystem && (R1 = ori(R1)) R2 isa ODESystem && (R2 = ori(R2)) v2 = resolve2(R2, resolve1(R1, v1)) @@ -150,13 +150,13 @@ function planar_rotation(axis, phi, der_angle) end """ - R2 = absoluteRotation(R1, R_rel) + R2 = absolute_rotation(R1, R_rel) - `R1`: `Orientation` object to rotate frame 0 into frame 1 - `R_rel`: `Orientation` object to rotate frame 1 into frame 2 - `R2`: `Orientation` object to rotate frame 0 into frame 2 """ -function absoluteRotation(R1, R_rel) +function absolute_rotation(R1, R_rel) # R2 = R_rel.R*R1.R # w = resolve2(R_rel, R1.w) + R_rel.w # RotationMatrix(R2, w) @@ -165,7 +165,7 @@ function absoluteRotation(R1, R_rel) R_rel * R1 end -function relativeRotation(R1, R2) +function relative_rotation(R1, R2) R1 isa ODESystem && (R1 = ori(R1)) R2 isa ODESystem && (R2 = ori(R2)) R = R2'R1 @@ -173,7 +173,7 @@ function relativeRotation(R1, R2) RotationMatrix(R.R, w) end -function inverseRotation(R) +function inverse_rotation(R) R isa ODESystem && (R = ori(R)) Ri = R.R' wi = -resolve1(R, R.w) @@ -186,7 +186,7 @@ function Base.:~(R1::RotationMatrix, R2::RotationMatrix) vec(R1.R.mat .~ R2.R.mat) end -function angularVelocity2(R::RotationMatrix) +function angular_velocity2(R::RotationMatrix) R.w end @@ -255,7 +255,7 @@ end orientation_constraint(q::AbstractVector) = q'q - 1 orientation_constraint(q::Quaternion) = orientation_constraint(q.Q) -# function angularVelocity2(q::AbstractVector, q̇) +# function angular_velocity2(q::AbstractVector, q̇) # Q = [q[4] q[3] -q[2] -q[1]; -q[3] q[4] q[1] -q[2]; q[2] -q[1] q[4] -q[3]] # 2 * Q * q̇ # end @@ -267,26 +267,26 @@ function from_Q(Q, w) RotationMatrix(R, w) end -function angularVelocity1(Q, der_Q) +function angular_velocity1(Q, der_Q) 2*([Q[4] -Q[3] Q[2] -Q[1]; Q[3] Q[4] -Q[1] -Q[2]; -Q[2] Q[1] Q[4] -Q[3]]*der_Q) end -function angularVelocity2(Q, der_Q) +function angular_velocity2(Q, der_Q) 2*([Q[4] Q[3] -Q[2] -Q[1]; -Q[3] Q[4] Q[1] -Q[2]; Q[2] -Q[1] Q[4] -Q[3]]*der_Q) end ## Euler -function axesRotations(sequence, angles, der_angles, name = :R_ar) - R = axisRotation(sequence[3], angles[3]) * - axisRotation(sequence[2], angles[2]) * - axisRotation(sequence[1], angles[1]) +function axes_rotations(sequence, angles, der_angles, name = :R_ar) + R = axis_rotation(sequence[3], angles[3]) * + axis_rotation(sequence[2], angles[2]) * + axis_rotation(sequence[1], angles[1]) w = axis(sequence[3]) * der_angles[3] + - resolve2(axisRotation(sequence[3], angles[3]), axis(sequence[2]) * der_angles[2]) + - resolve2(axisRotation(sequence[3], angles[3]) * - axisRotation(sequence[2], angles[2]), + resolve2(axis_rotation(sequence[3], angles[3]), axis(sequence[2]) * der_angles[2]) + + resolve2(axis_rotation(sequence[3], angles[3]) * + axis_rotation(sequence[2], angles[2]), axis(sequence[1]) * der_angles[1]) RotationMatrix(R.R, w) end @@ -294,7 +294,7 @@ end axis(s) = float.(s .== (1:3)) """ - axisRotation(sequence, angle; name = :R) + axis_rotation(sequence, angle; name = :R) Generate a rotation matrix for a rotation around the specified axis. @@ -303,7 +303,7 @@ Generate a rotation matrix for a rotation around the specified axis. Returns a `RotationMatrix` object. """ -function axisRotation(sequence, angle; name = :R) +function axis_rotation(sequence, angle; name = :R) if sequence == 1 return RotationMatrix(rotx(angle), zeros(3)) elseif sequence == 2 @@ -412,10 +412,10 @@ end # RotationMatrix([e_x e_y e_z], zeros(3)) # end -function resolveDyade1(R, D2) +function resolve_dyade1(R, D2) R'D2*R end -function resolveDyade2(R, D1) +function resolve_dyade2(R, D1) R*D1*R' end \ No newline at end of file diff --git a/src/robot/FullRobot.jl b/src/robot/FullRobot.jl index 5fb3dfba..5641737f 100644 --- a/src/robot/FullRobot.jl +++ b/src/robot/FullRobot.jl @@ -1,5 +1,5 @@ """ - RobotAxis(; name, mLoad = 15, kp = 5, ks = 0.5, Ts = 0.05, startAngle = 0, endAngle = 120, swingTime = 0.5, refSpeedMax = 3, refAccMax = 10, kwargs) + RobotAxis(; name, mLoad = 15, kp = 5, ks = 0.5, Ts = 0.05, q0 = 0, q1 = 120, swingtime = 0.5, refSpeedMax = 3, refAccMax = 10, kwargs) A single robot axis. @@ -7,21 +7,21 @@ A single robot axis. - `kp`: Proportional gain of position controller - `ks`: Proportional gain of velocity controller - `Ts`: Time constant of integrator of velocity controller -- `startAngle`: -- `endAngle`: +- `q0`: Start angle in degrees +- `q1`: End angle in degrees """ -function RobotAxis(; name, mLoad = 15, kp = 5.0, ks = 0.5, Ts = 0.05, startAngle = 0, - endAngle = 120, swingTime = 0.5, refSpeedMax = 3, refAccMax = 10, kwargs...) +function RobotAxis(; name, mLoad = 15, kp = 5.0, ks = 0.5, Ts = 0.05, q0 = 0, + q1 = 120, swingtime = 0.5, refSpeedMax = 3, refAccMax = 10, kwargs...) # parameter SI.Mass mLoad(min=0)=15 "Mass of load"; # parameter Real kp=5 "Gain of position controller of axis"; # parameter Real ks=0.5 "Gain of speed controller of axis"; # parameter SI.Time Ts=0.05 # "Time constant of integrator of speed controller of axis"; - # parameter Modelica.Units.NonSI.Angle_deg startAngle = 0 "Start angle of axis"; - # parameter Modelica.Units.NonSI.Angle_deg endAngle = 120 "End angle of axis"; + # parameter Modelica.Units.NonSI.Angle_deg q0 = 0 "Start angle of axis"; + # parameter Modelica.Units.NonSI.Angle_deg q1 = 120 "End angle of axis"; - # parameter SI.Time swingTime=0.5 + # parameter SI.Time swingtime=0.5 # "Additional time after reference motion is in rest before simulation is stopped"; # parameter SI.AngularVelocity refSpeedMax=3 "Maximum reference speed"; # parameter SI.AngularAcceleration refAccMax=10 @@ -32,9 +32,9 @@ function RobotAxis(; name, mLoad = 15, kp = 5.0, ks = 0.5, Ts = 0.05, startAngle kp = kp, [description = "Gain of position controller of axis"] ks = ks, [description = "Gain of speed controller of axis"] Ts = Ts, [description = "Time constant of integrator of speed controller of axis"] - # startAngle = startAngle, [description = "Start angle of axis"] - # endAngle = endAngle, [description = "End angle of axis"] - swingTime = swingTime, + # q0 = q0, [description = "Start angle of axis"] + # q1 = q1, [description = "End angle of axis"] + swingtime = swingtime, [ description = "Additional time after reference motion is in rest before simulation is stopped", ] @@ -53,11 +53,11 @@ function RobotAxis(; name, mLoad = 15, kp = 5.0, ks = 0.5, Ts = 0.05, startAngle ks = ks, Ts = Ts) load = Rotational.Inertia(J = 1.3 * mLoad) - pathPlanning = PathPlanning1(;swingTime = swingTime, - angleBegDeg = startAngle, - angleEndDeg = endAngle, - speedMax = refSpeedMax, - accMax = refAccMax, + pathPlanning = PathPlanning1(;swingtime = swingtime, + q0deg = q0, + q1deg = q1, + speed_max = refSpeedMax, + acc_max = refAccMax, kwargs... ) controlBus = ControlBus() @@ -87,18 +87,18 @@ function Robot6DOF(; name, kwargs...) # [ # description = "Additional time after reference motion is in rest before simulation is stopped", # ] - # startAngle1 = -60, [description = "Start angle of axis 1"] - # startAngle2 = 20, [description = "Start angle of axis 2"] - # startAngle3 = 90, [description = "Start angle of axis 3"] - # startAngle4 = 0, [description = "Start angle of axis 4"] - # startAngle5 = -110, [description = "Start angle of axis 5"] - # startAngle6 = 0, [description = "Start angle of axis 6"] - # endAngle1 = 60, [description = "End angle of axis 1"] - # endAngle2 = -70, [description = "End angle of axis 2"] - # endAngle3 = -35, [description = "End angle of axis 3"] - # endAngle4 = 45, [description = "End angle of axis 4"] - # endAngle5 = 110, [description = "End angle of axis 5"] - # endAngle6 = 45, [description = "End angle of axis 6"] + # q01 = -60, [description = "Start angle of axis 1"] + # q02 = 20, [description = "Start angle of axis 2"] + # q03 = 90, [description = "Start angle of axis 3"] + # q04 = 0, [description = "Start angle of axis 4"] + # q05 = -110, [description = "Start angle of axis 5"] + # q06 = 0, [description = "Start angle of axis 6"] + # q11 = 60, [description = "End angle of axis 1"] + # q12 = -70, [description = "End angle of axis 2"] + # q13 = -35, [description = "End angle of axis 3"] + # q14 = 45, [description = "End angle of axis 4"] + # q15 = 110, [description = "End angle of axis 5"] + # q16 = 45, [description = "End angle of axis 6"] # refSpeedMax[1:6] = [3, 1.5, 5, 3.1, 3.1, 4.1], # [description = "Maximum reference speeds of all joints"] # refAccMax[1:6] = [15, 15, 15, 60, 60, 60], @@ -149,44 +149,44 @@ function Robot6DOF(; name, kwargs...) ks6 = 0.5 #, [description = "Gain of speed controller"] Ts6 = 0.05 #, [description = "Time constant of integrator of speed controller"] - startAngle1 = -60 # Can't yet have these as parameters - startAngle2 = 20 # Can't yet have these as parameters - startAngle3 = 90 # Can't yet have these as parameters - startAngle4 = 0 # Can't yet have these as parameters - startAngle5 = -110 # Can't yet have these as parameters - startAngle6 = 0 # Can't yet have these as parameters - endAngle1 = 60 # Can't yet have these as parameters - endAngle2 = -70 # Can't yet have these as parameters - endAngle3 = -35 # Can't yet have these as parameters - endAngle4 = 45 # Can't yet have these as parameters - endAngle5 = 110 # Can't yet have these as parameters - endAngle6 = 45 # Can't yet have these as parameters + q01 = -60 # Can't yet have these as parameters + q02 = 20 # Can't yet have these as parameters + q03 = 90 # Can't yet have these as parameters + q04 = 0 # Can't yet have these as parameters + q05 = -110 # Can't yet have these as parameters + q06 = 0 # Can't yet have these as parameters + q11 = 60 # Can't yet have these as parameters + q12 = -70 # Can't yet have these as parameters + q13 = -35 # Can't yet have these as parameters + q14 = 45 # Can't yet have these as parameters + q15 = 110 # Can't yet have these as parameters + q16 = 45 # Can't yet have these as parameters systems = @named begin mechanics = MechanicalStructure(mLoad = (mLoad), rLoad = (rLoad), g = (g)) pathPlanning = PathPlanning6(;naxis = 6, - angleBegDeg = [ - startAngle1, - startAngle2, - startAngle3, - startAngle4, - startAngle5, - startAngle6, + q0deg = [ + q01, + q02, + q03, + q04, + q05, + q06, ], - angleEndDeg = [ - endAngle1, - endAngle2, - endAngle3, - endAngle4, - endAngle5, - endAngle6, + q1deg = [ + q11, + q12, + q13, + q14, + q15, + q16, ], - speedMax = refSpeedMax, - accMax = refAccMax, + speed_max = refSpeedMax, + acc_max = refAccMax, startTime = refStartTime, - swingTime = refSwingTime, + swingtime = refSwingTime, kwargs...) axis1 = AxisType1(w = 4590, diff --git a/src/robot/path_planning.jl b/src/robot/path_planning.jl index 813b1f02..155aadcf 100644 --- a/src/robot/path_planning.jl +++ b/src/robot/path_planning.jl @@ -4,33 +4,33 @@ using ModelingToolkitStandardLibrary.Blocks: RealInput, RealOutput include("ptp.jl") "Generate reference angles for specified kinematic movement" -function PathPlanning1(; name, angleBegDeg = 0, angleEndDeg = 1, time = 0:0.01:10, - swingTime = 0.5, kwargs...) +function PathPlanning1(; name, q0deg = 0, q1deg = 1, time = 0:0.01:10, + swingtime = 0.5, speed_max=1, acc_max=1, kwargs...) # @parameters begin - # angleBegDeg = angleBegDeg, [description = "Start angle"] - # angleEndDeg = angleEndDeg, [description = "End angle"] - # speedMax = speedMax, [description = "Maximum axis speed"] - # accMax = accMax, [description = "Maximum axis acceleration"] + # q0deg = q0deg, [description = "Start angle"] + # q1deg = q1deg, [description = "End angle"] + # speed_max = speed_max, [description = "Maximum axis speed"] + # acc_max = acc_max, [description = "Maximum axis acceleration"] # startTime = startTime, [description = "Start time of movement"] - # swingTime = swingTime, + # swingtime = swingtime, # [ # description = "Additional time after reference motion is in rest before simulation is stopped", # ] - # angleBeg = deg2rad(angleBegDeg), [description = "Start angles"] - # angleEnd = deg2rad(angleEndDeg), [description = "End angles"] + # angleBeg = deg2rad(q0deg), [description = "Start angles"] + # angleEnd = deg2rad(q1deg), [description = "End angles"] # end systems = @named begin controlBus = ControlBus() - path = KinematicPTP(; q_end = deg2rad.(angleEndDeg), + path = KinematicPTP(; q1 = deg2rad.(q1deg), time, - # qd_max = speedMax, - # qdd_max = accMax, + qd_max = (speed_max), + qdd_max = (acc_max), # startTime = startTime, - q_begin = deg2rad.(angleBegDeg), kwargs...) + q0 = deg2rad.(q0deg), kwargs...) pathToAxis1 = PathToAxisControlBus(nAxis = 1, axisUsed = 1) # terminateSimulation = TerminateSimulation(condition = time >= - # path.endTime + swingTime) + # path.endTime + swingtime) end eqs = [connect(path.q, pathToAxis1.q) @@ -41,33 +41,33 @@ function PathPlanning1(; name, angleBegDeg = 0, angleEndDeg = 1, time = 0:0.01:1 ODESystem(eqs, t; name, systems) end -function PathPlanning6(; name, naxis = 6, angleBegDeg = zeros(naxis), - angleEndDeg = ones(naxis), time = 0:0.01:4, - speedMax = fill(3, naxis), - accMax = fill(2.5, naxis), startTime = 0, swingTime = 0.5, kwargs...) +function PathPlanning6(; name, naxis = 6, q0deg = zeros(naxis), + q1deg = ones(naxis), time = 0:0.01:4, + speed_max = fill(3, naxis), + acc_max = fill(2.5, naxis), startTime = 0, swingtime = 0.5, kwargs...) # @parameters begin # naxis = naxis, [description = "Number of driven axis"] - # angleBegDeg[1:naxis] = angleBegDeg, [description = "Start angles"] - # angleEndDeg[1:naxis] = angleEndDeg, [description = "End angles"] - # # speedMax[1:naxis] = speedMax, [description = "Maximum axis speed"] - # # accMax[1:naxis] = accMax, [description = "Maximum axis acceleration"] + # q0deg[1:naxis] = q0deg, [description = "Start angles"] + # q1deg[1:naxis] = q1deg, [description = "End angles"] + # # speed_max[1:naxis] = speed_max, [description = "Maximum axis speed"] + # # acc_max[1:naxis] = acc_max, [description = "Maximum axis acceleration"] # # startTime = startTime, [description = "Start time of movement"] - # swingTime = swingTime, + # swingtime = swingtime, # [ # description = "Additional time after reference motion is in rest before simulation is stopped", # ] - # angleBeg[1:6] = deg2rad.(angleBegDeg), [description = "Start angles"] - # angleEnd[1:6] = deg2rad.(angleEndDeg), [description = "End angles"] + # angleBeg[1:6] = deg2rad.(q0deg), [description = "Start angles"] + # angleEnd[1:6] = deg2rad.(q1deg), [description = "End angles"] # end systems = @named begin controlBus = ControlBus() - path = KinematicPTP(; q_end = deg2rad.(angleEndDeg), + path = KinematicPTP(; q1 = deg2rad.(q1deg), time, - qd_max = speedMax, - qdd_max = accMax, + qd_max = speed_max, + qdd_max = acc_max, # startTime = startTime, - q_begin = deg2rad.(angleBegDeg), kwargs...) + q0 = deg2rad.(q0deg), kwargs...) pathToAxis1 = PathToAxisControlBus(nAxis = naxis, axisUsed = 1) pathToAxis2 = PathToAxisControlBus(nAxis = naxis, axisUsed = 2) pathToAxis3 = PathToAxisControlBus(nAxis = naxis, axisUsed = 3) @@ -75,7 +75,7 @@ function PathPlanning6(; name, naxis = 6, angleBegDeg = zeros(naxis), pathToAxis5 = PathToAxisControlBus(nAxis = naxis, axisUsed = 5) pathToAxis6 = PathToAxisControlBus(nAxis = naxis, axisUsed = 6) # terminateSimulation = TerminateSimulation(condition = time >= - # path.endTime + swingTime) + # path.endTime + swingtime) end eqs = [connect(path.q, pathToAxis1.q) @@ -186,25 +186,25 @@ end # plot!(t, centraldiff(centraldiff(q)), sp = 3) """ - KinematicPTP(; time, name, q_begin = 0, q_end = 1, qd_begin = 0, qd_end = 0, qdd_begin = 0, qdd_end = 0) + KinematicPTP(; time, name, q0 = 0, q1 = 1, qd0 = 0, qd1 = 0, qdd0 = 0, qdd1 = 0) A simple trajectory planner that plans a 5:th order polynomial trajectory between two points, subject to specified boundary conditions on the position, velocity and acceleration. """ -function KinematicPTP(; time, name, q_begin = 0, q_end = 1, qd_begin = 0, qd_end = 0, - qdd_begin = 0, qdd_end = 0, trivial = false, qd_max=1, qdd_max=1) - nout = max(length(q_begin), length(q_end), length(qd_end), length(qdd_end)) +function KinematicPTP(; time, name, q0 = 0, q1 = 1, qd0 = 0, qd1 = 0, + qdd0 = 0, qdd1 = 0, trivial = false, qd_max=1, qdd_max=1) + nout = max(length(q0), length(q1), length(qd1), length(qdd1)) # @parameters begin - # q_begin = q_begin, [description = "Start position"] - # q_end = q_end, [description = "End position"] + # q0 = q0, [description = "Start position"] + # q1 = q1, [description = "End position"] # qd_max = qd_max, [description = "Maximum velocities der(q)"] # qdd_max = qdd_max, [description = "Maximum accelerations der(qd)"] # startTime = startTime, [description = "Time instant at which movement starts"] - # p_q_begin[1:nout] = ones(nout) .* q_begin - # p_q_end[1:nout] = ones(nout) .* q_end + # p_q0[1:nout] = ones(nout) .* q0 + # p_q1[1:nout] = ones(nout) .* q1 # p_qd_max[1:nout] = ones(nout) .* qd_max # p_qdd_max[1:nout] = ones(nout) .* qdd_max - # p_deltaq[1:nout] = p_q_end - p_q_begin + # p_deltaq[1:nout] = p_q1 - p_q0 # end @@ -220,21 +220,21 @@ function KinematicPTP(; time, name, q_begin = 0, q_end = 1, qd_begin = 0, qd_end startTime = time[1] time0 = time .- startTime # traj5 wants time vector to start at 0 if !trivial - q_vec, qd_vec, qdd_vec = PTP(time; q0 = q_begin, q1 = q_end, qd_max, qdd_max) + q_vec, qd_vec, qdd_vec = point_to_point(time; q0 = q0, q1 = q1, qd_max, qdd_max) end interp_eqs = map(1:nout) do i if trivial - _q, _qd, _qdd = traj5(t, time[end]; q0 = q_begin[i], q1 = q_end[i], - q̇0 = zero(q_begin[i]), - q̇1 = zero(q_begin[i]), - q̈0 = zero(q_begin[i]), - q̈1 = zero(q_begin[i])) + _q, _qd, _qdd = traj5(t, time[end]; q0 = q0[i], q1 = q1[i], + q̇0 = zero(q0[i]), + q̇1 = zero(q0[i]), + q̈0 = zero(q0[i]), + q̈1 = zero(q0[i])) [q.u[i] ~ _q qd.u[i] ~ _qd qdd.u[i] ~ _qdd] else - # q_vec, qd_vec, qdd_vec = PTP(time; q0 = q_begin[i], q1 = q_end[i], qd_max, qdd_max) + # q_vec, qd_vec, qdd_vec = PTP(time; q0 = q0[i], q1 = q1[i], qd_max, qdd_max) qfun = CubicSpline(q_vec[:, i], time; extrapolate=true) qdfun = LinearInterpolation(qd_vec[:, i], time; extrapolate=true) qddfun = ConstantInterpolation(qdd_vec[:, i], time; extrapolate=true) diff --git a/src/robot/ptp.jl b/src/robot/ptp.jl index b499359a..13309812 100644 --- a/src/robot/ptp.jl +++ b/src/robot/ptp.jl @@ -12,7 +12,24 @@ function centraldiff(v::AbstractVector) a = a1+a2 end -function PTP(time; q0 = 0.0, q1 = 1.0, t0 = 0, qd_max = 1, qdd_max = 1) + + +""" + q,qd,qdd = point_to_point(time; q0 = 0.0, q1 = 1.0, t0 = 0, qd_max = 1, qdd_max = 1) + +Generate a minimum-time point-to-point trajectory with specified start and endpoints, not exceeding specified speed and acceleration limits. + +The trajectory produced by this function will typically exhibit piecewise constant accleration, piecewise linear velocity and piecewise quadratic position curves. + +# Arguments: +- `time`: A scalar or a vector of time points. +- `q0`: Initial coordinate, may be a scalar or a vector. +- `q1`: End coordinate +- `t0`: Tiem at which the motion starts. If `time` contains time points before `t0`, the trajectory will stand still at `q0` until `time` reaches `t0`. +- `qd_max`: Maximum allowed speed. +- `qdd_max`: Maximum allowed acceleration. +""" +function point_to_point(time; q0 = 0.0, q1 = 1.0, t0 = 0, qd_max = 1, qdd_max = 1) nout = max(length(q0), length(q1), length(qd_max), length(qdd_max)) p_q0 = q0 p_q1 = q1 diff --git a/src/robot/robot_components.jl b/src/robot/robot_components.jl index 906680ee..2efa823b 100644 --- a/src/robot/robot_components.jl +++ b/src/robot/robot_components.jl @@ -444,12 +444,12 @@ function MechanicalStructure(; name, mLoad = 15, rLoad = [0, 0.25, 0], g = 9.81) axis4 = Rotational.Flange() axis5 = Rotational.Flange() axis6 = Rotational.Flange() - r1 = Revolute(n = [0, 1, 0], useAxisFlange = true, isroot = false, radius=0.15) - r2 = Revolute(n = [1, 0, 0], useAxisFlange = true, isroot = false, radius=0.1) - r3 = Revolute(n = [1, 0, 0], useAxisFlange = true, isroot = false, radius=0.075) - r4 = Revolute(n = [0, 1, 0], useAxisFlange = true, isroot = false, radius=0.06) - r5 = Revolute(n = [1, 0, 0], useAxisFlange = true, isroot = false, radius=0.05) - r6 = Revolute(n = [0, 1, 0], useAxisFlange = true, isroot = false, radius=0.02) + r1 = Revolute(n = [0, 1, 0], axisflange = true, isroot = false, radius=0.15) + r2 = Revolute(n = [1, 0, 0], axisflange = true, isroot = false, radius=0.1) + r3 = Revolute(n = [1, 0, 0], axisflange = true, isroot = false, radius=0.075) + r4 = Revolute(n = [0, 1, 0], axisflange = true, isroot = false, radius=0.06) + r5 = Revolute(n = [1, 0, 0], axisflange = true, isroot = false, radius=0.05) + r6 = Revolute(n = [0, 1, 0], axisflange = true, isroot = false, radius=0.02) b0 = BodyShape(r = [0, 0.351, 0], # r_shape = [0, 0, 0], # lengthDirection = [1, 0, 0], diff --git a/src/sensors.jl b/src/sensors.jl index 7d6c40c6..3189bf85 100644 --- a/src/sensors.jl +++ b/src/sensors.jl @@ -23,11 +23,11 @@ function PartialAbsoluteSensor(; name, n_out) end """ - PartialCutForceBaseSensor(; name, resolveInFrame = :frame_a) + PartialCutForceBaseSensor(; name, resolve_frame = :frame_a) -- `resolveInFrame`: The frame in which the cut force and cut torque are resolved. Default is `:frame_a`, options include `:frame_a` and `:world`. +- `resolve_frame`: The frame in which the cut force and cut torque are resolved. Default is `:frame_a`, options include `:frame_a` and `:world`. """ -function PartialCutForceBaseSensor(; name, resolveInFrame = :frame_a) +function PartialCutForceBaseSensor(; name, resolve_frame = :frame_a) @named begin frame_a = Frame() frame_b = Frame() @@ -41,48 +41,48 @@ function PartialCutForceBaseSensor(; name, resolveInFrame = :frame_a) end """ - CutTorque(; name, resolveInFrame) + CutTorque(; name, resolve_frame) Basic sensor to measure cut torque vector. Contains a connector of type `Blocks.RealOutput` with name `torque`. -- `resolveInFrame`: The frame in which the cut force and cut torque are resolved. Default is `:frame_a`, options include `:frame_a` and `:world`. +- `resolve_frame`: The frame in which the cut force and cut torque are resolved. Default is `:frame_a`, options include `:frame_a` and `:world`. """ -function CutTorque(; name, resolveInFrame = :frame_a) - @named pcfbs = PartialCutForceBaseSensor(; resolveInFrame) - @named torque = Blocks.RealOutput(nout = 3) # "Cut torque resolved in frame defined by resolveInFrame" +function CutTorque(; name, resolve_frame = :frame_a) + @named pcfbs = PartialCutForceBaseSensor(; resolve_frame) + @named torque = Blocks.RealOutput(nout = 3) # "Cut torque resolved in frame defined by resolve_frame" @unpack frame_a, frame_b = pcfbs - eqs = if resolveInFrame === :world + eqs = if resolve_frame === :world collect(torque.u) .~ resolve1(ori(frame_a), frame_a.tau) - elseif resolveInFrame === :frame_a + elseif resolve_frame === :frame_a collect(torque.u) .~ collect(frame_a.tau) else - error("resolveInFrame must be :world or :frame_a") + error("resolve_frame must be :world or :frame_a") end extend(compose(ODESystem(eqs, t; name), torque), pcfbs) end """ - BasicCutForce(; name, resolveInFrame) + BasicCutForce(; name, resolve_frame) Basic sensor to measure cut force vector. Contains a connector of type `Blocks.RealOutput` with name `force`. -- `resolveInFrame`: The frame in which the cut force and cut torque are resolved. Default is `:frame_a`, options include `:frame_a` and `:world`. +- `resolve_frame`: The frame in which the cut force and cut torque are resolved. Default is `:frame_a`, options include `:frame_a` and `:world`. """ -function CutForce(; name, resolveInFrame = :frame_a) - @named pcfbs = PartialCutForceBaseSensor(; resolveInFrame) - @named force = Blocks.RealOutput(nout = 3) # "Cut force resolved in frame defined by resolveInFrame" +function CutForce(; name, resolve_frame = :frame_a) + @named pcfbs = PartialCutForceBaseSensor(; resolve_frame) + @named force = Blocks.RealOutput(nout = 3) # "Cut force resolved in frame defined by resolve_frame" @unpack frame_a, frame_b = pcfbs - eqs = if resolveInFrame === :world + eqs = if resolve_frame === :world collect(force.u) .~ resolve1(ori(frame_a), frame_a.f) - elseif resolveInFrame === :frame_a + elseif resolve_frame === :frame_a collect(force.u) .~ collect(frame_a.f) else - error("resolveInFrame must be :world or :frame_a") + error("resolve_frame must be :world or :frame_a") end extend(compose(ODESystem(eqs, t; name), force), pcfbs) end -function RelativePosition(; name, resolveInFrame = :frame_a) +function RelativePosition(; name, resolve_frame = :frame_a) @named begin prs = PartialRelativeBaseSensor(; name) end @unpack frame_a, frame_b = prs @@ -104,8 +104,8 @@ function RelativeAngles(; name, sequence = [1, 2, 3]) frame_a.tau .~ zeros(3) |> collect frame_b.f .~ zeros(3) |> collect frame_b.tau .~ zeros(3) |> collect - R_rel ~ relativeRotation(frame_a, frame_b) - angles .~ axesRotationsAngles(R_rel, sequence, guessAngle1)] + R_rel ~ relative_rotation(frame_a, frame_b) + angles .~ axes_rotationangles(R_rel, sequence, guessAngle1)] compose(ODESystem(eqs, t; name), frame_a, frame_b, angles) end @@ -118,6 +118,6 @@ function AbsoluteAngles(; name, sequence = [1, 2, 3]) @named R_abs = NumRotationMatrix() eqs = [collect(frame_a.f .~ 0) collect(frame_a.tau .~ 0) - angles.u .~ axesRotationsAngles(ori(frame_a), [1, 2, 3])] + angles.u .~ axes_rotationangles(ori(frame_a), [1, 2, 3])] extend(compose(ODESystem(eqs, t; name)), pas) end diff --git a/test/runtests.jl b/test/runtests.jl index 76f93c53..c03670ae 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -51,9 +51,9 @@ The multibody paper mentions this as an interesting example, figure 8: @testset "spring - harmonic oscillator" begin - @named body = Body(; m = 1, isroot = true, r_cm = [0, 1, 0], phi0 = [0, 1, 0], useQuaternions=false) # This time the body isroot since there is no joint containing state - @named spring = Multibody.Spring(c = 1, fixedRotationAtFrame_a = false, - fixedRotationAtFrame_b = false) + @named body = Body(; m = 1, isroot = true, r_cm = [0, 1, 0], phi0 = [0, 1, 0], quat=false) # This time the body isroot since there is no joint containing state + @named spring = Multibody.Spring(c = 1, fixed_rotation_at_frame_a = false, + fixed_rotation_at_frame_b = false) connections = [connect(world.frame_b, spring.frame_a) connect(spring.frame_b, body.frame_a)] @@ -135,7 +135,7 @@ end world = Multibody.world @named body = Body(; m = 1, isroot = false, r_cm = [0.5, 0, 0]) @named damper = Rotational.Damper(d = 0.1) -@named rev = Multibody.Revolute(n = [0, 0, 1], useAxisFlange = true, isroot = true) +@named rev = Multibody.Revolute(n = [0, 0, 1], axisflange = true, isroot = true) connections = [connect(world.frame_b, rev.frame_a) connect(damper.flange_b, rev.axis) @@ -170,7 +170,7 @@ world = Multibody.world @named rod = FixedTranslation(r = [0.5, 0, 0]) @named body = Body(; m = 1, isroot = false, r_cm = [0, 0, 0]) @named damper = Rotational.Damper(d = 0.1) -@named rev = Multibody.Revolute(n = [0, 0, 1], useAxisFlange = true, isroot = true) +@named rev = Multibody.Revolute(n = [0, 0, 1], axisflange = true, isroot = true) connections = [connect(world.frame_b, rev.frame_a) connect(damper.flange_b, rev.axis) @@ -204,7 +204,7 @@ world = Multibody.world @named rod = FixedRotation(r = [0.5, 0, 0], n = [0, 0, 1], angle = 0) @named body = Body(; m = 1, isroot = false, r_cm = [0, 0, 0]) @named damper = Rotational.Damper(d = 0.1) -@named rev = Multibody.Revolute(n = [0, 0, 1], useAxisFlange = true, isroot = true) +@named rev = Multibody.Revolute(n = [0, 0, 1], axisflange = true, isroot = true) connections = [connect(world.frame_b, rev.frame_a) connect(damper.flange_b, rev.axis) @@ -240,8 +240,8 @@ using LinearAlgebra @named body2 = Body(; m = 1, isroot = false, r_cm = [0.0, 0, 0]) @named damper1 = Rotational.Damper(d = 5) @named damper2 = Rotational.Damper(d = 1) -@named rev1 = Multibody.Revolute(n = normalize([0.1, 0, 1]), useAxisFlange = true, isroot = true) -@named rev2 = Multibody.Revolute(n = [0, 0, 1], useAxisFlange = true, isroot = true) +@named rev1 = Multibody.Revolute(n = normalize([0.1, 0, 1]), axisflange = true, isroot = true) +@named rev2 = Multibody.Revolute(n = [0, 0, 1], axisflange = true, isroot = true) connections = [connect(damper1.flange_b, rev1.axis) connect(rev1.support, damper1.flange_a) @@ -296,7 +296,7 @@ end @named body = Body(; m = 1, isroot = false, r_cm = [0, 0, 0]) @named damper = Translational.Damper(d=0.5) @named spring = Translational.Spring(c=1) -@named joint = Prismatic(n = [0, 1, 0], isroot = true, useAxisFlange = true) +@named joint = Prismatic(n = [0, 1, 0], isroot = true, axisflange = true) connections = [connect(world.frame_b, joint.frame_a) connect(damper.flange_b, spring.flange_b, joint.axis) @@ -322,11 +322,11 @@ end world = Multibody.world @named begin body1 = Body(; m = 1, isroot = true, r_cm = [0.0, 0, 0], I_11 = 0.1, I_22 = 0.1, - I_33 = 0.1, r_0 = [0.3, -0.2, 0], useQuaternions=false) # This is root since there is no joint parallel to the spring leading to this body + I_33 = 0.1, r_0 = [0.3, -0.2, 0], quat=false) # This is root since there is no joint parallel to the spring leading to this body 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) @@ -407,7 +407,7 @@ world = Multibody.world 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) @@ -442,7 +442,7 @@ sol = solve(prob, Rodas4())#, u0 = prob.u0 .+ 1e-1 .* rand.()) doplot() && plot(sol, idxs = [body1.r_0...]) |> display # end # TODO: add tutorial explaining what interesting things this demos illustrates -# fixedRotationAtFrame_a and b = true required +# fixed_rotation_at_frame_a and b = true required end # ============================================================================== ## FreeBody ==================================================================== @@ -515,7 +515,7 @@ D = Differential(t) world = Multibody.world @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 @@ -603,7 +603,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 = Multibody.Fixed() inertia1 = Rotational.Inertia(J = cyl1.I_11) idealGear = Rotational.IdealGear(ratio = 10, use_support = true) @@ -679,7 +679,7 @@ end # Model a free-falling body # Test 1, enforce state = false world = Multibody.world -@named freeMotion = FreeMotion(enforceState = false, isroot = false) +@named freeMotion = FreeMotion(state = false, isroot = false) @named body = Body(m = 1, isroot = true) eqs = [connect(world.frame_b, freeMotion.frame_a) @@ -703,7 +703,7 @@ y = sol(0:0.1:10, idxs = body.r_0[2]) ## test 2 enforce state = true -@named freeMotion = FreeMotion(enforceState = true, isroot = true) +@named freeMotion = FreeMotion(state = true, isroot = true) @named body = Body(m = 1, isroot = false) eqs = [connect(world.frame_b, freeMotion.frame_a) @@ -753,8 +753,8 @@ end # ============================================================================== @testset "Dzhanibekov effect" begin world = Multibody.world -@named freeMotion = FreeMotion(enforceState = false, isroot = false) -@named body = BodyShape(m = 1, r=0.1*[1,2,3], isroot = true, I_11 = 1, I_22 = 10, I_33 = 100, useQuaternions=false) +@named freeMotion = FreeMotion(state = false, isroot = false) +@named body = BodyShape(m = 1, r=0.1*[1,2,3], isroot = true, I_11 = 1, I_22 = 10, I_33 = 100, quat=false) eqs = [connect(world.frame_b, freeMotion.frame_a) connect(freeMotion.frame_b, body.frame_a)] @@ -792,7 +792,7 @@ end @testset "Harmonic oscillator with Body as root and quaternions as state variables" begin -@named body = Body(; m = 1, isroot = true, r_cm = [0.0, 0, 0], phi0 = [0, 0.9, 0], useQuaternions=true) # This time the body isroot since there is no joint containing state +@named body = Body(; m = 1, isroot = true, r_cm = [0.0, 0, 0], phi0 = [0, 0.9, 0], quat=true) # This time the body isroot since there is no joint containing state @named spring = Multibody.Spring(c = 1) connections = [connect(world.frame_b, spring.frame_a) @@ -845,7 +845,7 @@ W(args...; kwargs...) = Multibody.world @components begin world = W() torque = RTorque() - joint = Revolute(useAxisFlange=true) # The axis flange provides an interface to the 1D torque input from ModelingToolkitStandardLibrary.Mechanical.Rotational + joint = Revolute(axisflange=true) # The axis flange provides an interface to the 1D torque input from ModelingToolkitStandardLibrary.Mechanical.Rotational torque_signal = BSine(frequency=1/5) body = BodyShape(; m = 1, r = [0.4, 0, 0]) end @@ -899,7 +899,7 @@ prob = ODEProblem(ssys, [ @test SciMLBase.successful_retcode(sol) -if true +if false import GLMakie @time "render stiff_rope" render(stiff_rope, sol) # Takes very long time for n>=8 end @@ -928,7 +928,7 @@ prob = ODEProblem(ssys, [ ], (0, 10)) @time "Flexible rope pendulum" sol = solve(prob, Rodas4(autodiff=false); u0 = prob.u0 .+ 0.5); @test SciMLBase.successful_retcode(sol) -if true +if false import GLMakie @time "render flexible_rope" render(flexible_rope, sol) # Takes very long time for n>=8 end @@ -941,13 +941,13 @@ end systems = @named begin - joint = Spherical(enforceState=true, isroot=true, phi = [π/2, 0, 0], d = 0.3) + joint = Spherical(state=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) + joint2 = Multibody.Revolute(n = [1, 0, 0], axisflange = true, isroot = true) bar2 = FixedTranslation(r = [0, 1, 0]) body2 = Body(; m = 1, isroot = false) damper = Rotational.Damper(d = 0.3) diff --git a/test/test_robot.jl b/test/test_robot.jl index 47f37b92..50ec23d5 100644 --- a/test/test_robot.jl +++ b/test/test_robot.jl @@ -234,7 +234,7 @@ u = cm.axis2.controller.PI.ctr_output.u zdd = ModelingToolkit.missing_variable_defaults(oneaxis); op = merge(Dict(zdd), op) - prob = ODEProblem(ssys, collect(op), (0.0, 10),) + prob = ODEProblem(ssys, collect(op), (0.0, 3),) sol = solve(prob, Rodas4()); if doplot() plot(sol, layout=length(unknowns(ssys)), size=(1900, 1200)) @@ -249,7 +249,7 @@ u = cm.axis2.controller.PI.ctr_output.u @test sol(tv[1], idxs=oneaxis.pathPlanning.controlBus.axisControlBus1.angle_ref) ≈ deg2rad(0) atol=1e-8 @test sol(tv[end], idxs=oneaxis.pathPlanning.controlBus.axisControlBus1.angle_ref) ≈ deg2rad(120) - @test maximum(abs, control_error) < 1e-5 + @test maximum(abs, control_error) < 1e-3 end diff --git a/test/test_traj.jl b/test/test_traj.jl index 7ed5ade7..ad1ce030 100644 --- a/test/test_traj.jl +++ b/test/test_traj.jl @@ -2,17 +2,17 @@ using Test Ts = 0.001 t = -1:Ts:3 -q, qd, qdd = PTP(t) +q, qd, qdd = point_to_point(t) q1 = [1, 1.2] qd_max = [0.7, 1.2] qdd_max = [0.9, 1.1] -q, qd, qdd = PTP(t; q1, qd_max) +q, qd, qdd = point_to_point(t; q1, qd_max) @test all(q .<= q1') @test all(qd .<= qd_max') @test all(qdd.<= qdd_max') -_, _, _, t1 = PTP(0; q1, qd_max) +_, _, _, t1 = point_to_point(0; q1, qd_max) ci = findlast(qd .!= 0) t02 = t[ci[1]] @test t02 ≈ t1 rtol=1e-3 @@ -31,11 +31,11 @@ q1 = randn(N) qd_max = rand(N) qdd_max = rand(N) O = zeros(N) -q, qd, qdd, t1 = PTP(0; q0, q1, qd_max, qdd_max, t0) +q, qd, qdd, t1 = point_to_point(0; q0, q1, qd_max, qdd_max, t0) Ts = 0.01 Tf = t1 + 0.1 t = (t0 - 1):Ts:Tf -q, qd, qdd = PTP(t; q0, q1, qd_max, qdd_max, t0) +q, qd, qdd = point_to_point(t; q0, q1, qd_max, qdd_max, t0) @test q[1,:] ≈ q0 norm=maximum atol=1e-3 @test q[end,:] ≈ q1 norm=maximum atol=1e-3 @test qd[1,:] == O