diff --git a/docs/make.jl b/docs/make.jl index 24ef3e34..e793fd34 100644 --- a/docs/make.jl +++ b/docs/make.jl @@ -20,31 +20,41 @@ makedocs(; prettyurls = get(ENV, "CI", nothing) == "true", edit_link = nothing), pages = [ - "Home" => "index.md", - "Tutorials" => [ - "Getting started: Pendulum" => "examples/pendulum.md", - ], - "Examples" => [ - "Spring-damper system" => "examples/spring_damper_system.md", - "Spring-mass system" => "examples/spring_mass_system.md", - "Three springs (series forces)" => "examples/three_springs.md", - "Sensors" => "examples/sensors.md", - "Spherical pendulum" => "examples/spherical_pendulum.md", - "Gearbox" => "examples/gearbox.md", - "Free motions" => "examples/free_motion.md", - "Kinematic loops" => "examples/kinematic_loops.md", - "Industrial robot" => "examples/robot.md", - "Ropes, cables and chains" => "examples/ropes_and_cables.md", - "Swing" => "examples/swing.md", - "Bodies in space" => "examples/space.md", - "Gyroscopic effects" => "examples/gyroscopic_effects.md", - "Wheels" => "examples/wheel.md", - "Suspension systems" => "examples/suspension.md", - "Quadrotor with cable-suspended load" => "examples/quad.md", - ], - "Rotations and orientation" => "rotations.md", - "3D rendering" => "rendering.md", - "URDF import" => "urdf.md", + "Home" => "index.md", + "Tutorials" => [ + "Getting started: Pendulum" => "examples/pendulum.md", + ], + "Examples" => [ + "Spring-damper system" => "examples/spring_damper_system.md", + "Spring-mass system" => "examples/spring_mass_system.md", + "Three springs (series forces)" => "examples/three_springs.md", + "Sensors" => "examples/sensors.md", + "Spherical pendulum" => "examples/spherical_pendulum.md", + "Gearbox" => "examples/gearbox.md", + "Free motions" => "examples/free_motion.md", + "Prescribed motions" => "examples/prescribed_pose.md", + "Kinematic loops" => "examples/kinematic_loops.md", + "Industrial robot" => "examples/robot.md", + "Ropes, cables and chains" => "examples/ropes_and_cables.md", + "Swing" => "examples/swing.md", + "Bodies in space" => "examples/space.md", + "Gyroscopic effects" => "examples/gyroscopic_effects.md", + "Wheels" => "examples/wheel.md", + "Suspension systems" => "examples/suspension.md", + "Quadrotor with cable-suspended load" => "examples/quad.md", + ], + "Components" => [ + "Frames" => "frames.md", + "Joints" => "joints.md", + "Components" => "components.md", + "Forces" => "forces.md", + "Sensors" => "sensors.md", + "Trajectory planning" => "trajectory_planning.md", + "Interfaces" => "interfaces.md", + ], + "Rotations and orientation" => "rotations.md", + "3D rendering" => "rendering.md", + "URDF import" => "urdf.md", ]) deploydocs(; diff --git a/docs/src/components.md b/docs/src/components.md new file mode 100644 index 00000000..77796658 --- /dev/null +++ b/docs/src/components.md @@ -0,0 +1,15 @@ +# Components + +The perhaps most fundamental component is a [`Body`](@ref), this component has a single flange, `frame_a`, which is used to connect the body to other components. This component has a mass, a vector `r_cm` from `frame_a` to the center of mass, and a moment of inertia tensor `I` in the center of mass. The body can be thought of as a point mass with a moment of inertia tensor. + +A mass with a shape can be modeled using a [`BodyShape`](@ref). The primary difference between a [`Body`](@ref) and a [`BodyShape`](@ref) is that the latter has an additional flange, `frame_b`, which is used to connect the body to other components. The translation between `flange_a` and `flange_b` is determined by the vector `r`. The [`BodyShape`](@ref) is suitable to model, e.g., cylinders, rods, and boxes. + +A rod without a mass (just a translation), is modeled using [`FixedTranslation`](@ref). + +```@index +``` + +```@autodocs +Modules = [Multibody, Multibody.PlanarMechanics] +Pages = ["components.jl", "wheels.jl", "PlanarMechanics/components.jl"] +``` diff --git a/docs/src/examples/prescribed_pose.md b/docs/src/examples/prescribed_pose.md new file mode 100644 index 00000000..ccd5c871 --- /dev/null +++ b/docs/src/examples/prescribed_pose.md @@ -0,0 +1,179 @@ +# Prescribed movements + +The movement of a frame can be prescribed using any of the components +- [`Position`](@ref) +- [`Pose`](@ref) + +The motion of joints [`Revolute`](@ref) and [`Prismatic`](@ref) can also be prescribed by using `axisflange = true` and attaching a `ModelingToolkitStandardLibrary.Rotational.Position` or `ModelingToolkitStandardLibrary.TranslationalModelica.Position` to the axis flange of the joint. + + +Prescribed motions can be useful to, e.g., +- Use the 3D rendering capabilities to visualize the movement of a mechanism moving in a prescribed way without simulating the system. +- Simplify simulations by prescribing the movement of a part of the mechanism. + + +In the example below, we prescribe the motion of a suspension system using [`Pose`](@ref). We load the `Rotations` package in order to get access to the constructor `RotXYZ` which allows us to specify a rotation matrix using Euler angles. The component prescribing the motion is +```julia +body_upright = Pose(; r = [0, 0.17 + 0.1sin(t), 0], R = RotXYZ(0, 0.5sin(t), 0)) +``` +This makes the `body_upright` frame move in the vertical direction and rotate around the y-axis. Notice how the reference translation `r` and the reference orientation `R` are symbolic expressions that depend on time `t`. To make a frame follow recorded data, you may use [DataInterpolations.jl](https://docs.sciml.ai/DataInterpolations/stable/) to create interpolation objects that when accessed with `t` return the desired position and orientation. + +```@example PRESCRIBED_POSE +using Multibody +using Multibody.Rotations # To specify orientations using Euler angles +using ModelingToolkit +using Plots +using OrdinaryDiffEq +using LinearAlgebra +using JuliaSimCompiler +using Test + +t = Multibody.t +D = Differential(t) +W(args...; kwargs...) = Multibody.world + +n = [1, 0, 0] +AB = 146.5 / 1000 +BC = 233.84 / 1000 +CD = 228.60 / 1000 +DA = 221.43 / 1000 +BP = 129.03 / 1000 +DE = 310.31 / 1000 +t5 = 19.84 |> deg2rad + +@mtkmodel QuarterCarSuspension begin + @structural_parameters begin + spring = true + (jc = [0.5, 0.5, 0.5, 0.7])#, [description = "Joint color"] + mirror = false + end + @parameters begin + cs = 4000, [description = "Damping constant [Ns/m]"] + ks = 44000, [description = "Spring constant [N/m]"] + rod_radius = 0.02 + jr = 0.03, [description = "Radius of revolute joint"] + end + begin + dir = mirror ? -1 : 1 + rRod1_ia = AB*normalize([0, -0.1, 0.2dir]) + rRod2_ib = BC*normalize([0, 0.2, 0dir]) + end + @components begin + r123 = JointRRR(n_a = n*dir, n_b = n*dir, rRod1_ia, rRod2_ib, rod_radius=0.018, rod_color=jc) + r2 = Revolute(; n=n*dir, radius=jr, color=jc) + b1 = FixedTranslation(radius = rod_radius, r = CD*normalize([0, -0.1, 0.3dir])) # CD + chassis = FixedTranslation(r = DA*normalize([0, 0.2, 0.2*sin(t5)*dir]), render=false) + chassis_frame = Frame() + + if spring + springdamper = SpringDamperParallel(c = ks, d = cs, s_unstretched = 1.3*BC, radius=rod_radius, num_windings=10) + end + if spring + spring_mount_F = FixedTranslation(r = 0.7*CD*normalize([0, -0.1, 0.3dir]), render=false) + end + if spring + spring_mount_E = FixedTranslation(r = 1.3DA*normalize([0, 0.2, 0.2*sin(t5)*dir]), render=true) + end + end + begin + A = chassis.frame_b + D = chassis.frame_a + end + @equations begin + # Main loop + connect(A, r123.frame_a) + connect(r123.frame_b, b1.frame_b) + connect(b1.frame_a, r2.frame_b) + connect(r2.frame_a, D) + + # Spring damper + if spring + connect(springdamper.frame_b, spring_mount_E.frame_b) + connect(b1.frame_a, spring_mount_F.frame_a) + connect(D, spring_mount_E.frame_a) + connect(springdamper.frame_a, spring_mount_F.frame_b) + end + + connect(chassis_frame, chassis.frame_a) + end +end + +@mtkmodel ExcitedWheelAssembly begin + @parameters begin + rod_radius = 0.02 + end + @components begin + chassis_frame = Frame() + suspension = QuarterCarSuspension(; rod_radius) + + wheel = SlippingWheel( + radius = 0.2, + m = 15, + I_axis = 0.06, + I_long = 0.12, + x0 = 0.0, + z0 = 0.0, + der_angles = [0, 0, 0], + iscut = true, + ) + + end + @equations begin + connect(wheel.frame_a, suspension.r123.frame_ib) + connect(chassis_frame, suspension.chassis_frame) + end +end + + +@mtkmodel SuspensionWithExcitationAndMass begin + @parameters begin + ms = 1500/4, [description = "Mass of the car [kg]"] + rod_radius = 0.02 + end + @components begin + world = W() + mass = Body(m=ms, r_cm = 0.5DA*normalize([0, 0.2, 0.2*sin(t5)])) + excited_suspension = ExcitedWheelAssembly(; rod_radius) + prescribed_motion = Pose(; r = [0, 0.1 + 0.1sin(t), 0], R = RotXYZ(0, 0.5sin(t), 0)) + end + @equations begin + connect(prescribed_motion.frame_b, excited_suspension.chassis_frame, mass.frame_a) + end + +end + +@named model = SuspensionWithExcitationAndMass() +model = complete(model) +ssys = structural_simplify(IRSystem(model)) +display([unknowns(ssys) diag(ssys.mass_matrix)]) + +defs = [ + model.excited_suspension.suspension.ks => 30*44000 + model.excited_suspension.suspension.cs => 30*4000 + model.excited_suspension.suspension.r2.phi => -0.6031*(1) +] +prob = ODEProblem(ssys, defs, (0, 2π)) +sol = solve(prob, Rodas5P(autodiff=false), initializealg = BrownFullBasicInit()) +@test SciMLBase.successful_retcode(sol) +Multibody.render(model, sol, show_axis=false, x=-0.8, y=0.7, z=0.1, lookat=[0,0.1,0.0], filename="prescribed_motion.gif") # Video +nothing # hide +``` + +![prescribed motion](prescribed_motion.gif) + +Even though we formulated an `ODEProblem` and called `solve`, we do not actually perform any simulation here! If we look at the mass matrix of the system +```@example PRESCRIBED_POSE +ssys.mass_matrix +``` +we see that it is all zeros. This means that there are no differential equations at all in the system, only algebraic equations. The solver will thus only solve for the algebraic variables using a nonlinear root finder. In general, prescribing the value of some state variables removes the need for the solver to solve for them, which can be useful for simplifying simulations. Using the "simulation" above, we can use the solution object to, e.g., find the compression of the spring and the forces acting on the ground over time. + +```@example PRESCRIBED_POSE +plot(sol, idxs=[model.excited_suspension.suspension.springdamper.s, -model.excited_suspension.suspension.springdamper.f, model.excited_suspension.wheel.wheeljoint.f_n], labels=["Spring length [m]" "Spring force [N] " "Normal force [N]"], layout=(2,1), sp=[1 2 2]) +``` +Here, we see that the total spring force and the normal force acting on the ground are not equal, this is due to the spring not applying force only in the vertical direction. We can also compute the slip velocity, the velocity with which the contact between the wheel and the ground is sliding along the ground due to the prescribed motion. + +```@example PRESCRIBED_POSE +wj = model.excited_suspension.wheel.wheeljoint +plot(sol, idxs=[wj.v_slip, wj.v_slip_long, wj.v_slip_lat], labels=["Slip velocity magnitude" "Longitudinal slip velocity" "Lateral slip velocity"], ylabel="Velocity [m/s]") +``` + diff --git a/docs/src/forces.md b/docs/src/forces.md new file mode 100644 index 00000000..fea7a490 --- /dev/null +++ b/docs/src/forces.md @@ -0,0 +1,10 @@ +# Forces + +```@index +``` + + +```@autodocs +Modules = [Multibody, Multibody.PlanarMechanics] +Pages = ["forces.jl"] +``` \ No newline at end of file diff --git a/docs/src/frames.md b/docs/src/frames.md new file mode 100644 index 00000000..2bbb1a40 --- /dev/null +++ b/docs/src/frames.md @@ -0,0 +1,10 @@ +# Frames + +## Docstrings +```@index +``` + +```@autodocs +Modules = [Multibody, Multibody.PlanarMechanics] +Pages = ["frames.jl", "PlanarMechanics/utils.jl"] +``` diff --git a/docs/src/index.md b/docs/src/index.md index f1fca0e2..3d971042 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -175,85 +175,3 @@ Multibody.jl offers components for modeling in both 2D and 3D. 2D modeling, ofte The components from [`ModelingToolkitStandardLibrary.Mechanical`](https://docs.sciml.ai/ModelingToolkitStandardLibrary/stable/API/mechanical/) are 1D, i.e., a single degree of freedom only. These components can be used in both 2D and 3D modeling together with Multibody components that have support for attaching 1D components, such as joints supporting the `axisflange` keyword. - -## Index -```@index -``` - - -## Frames -```@autodocs -Modules = [Multibody, Multibody.PlanarMechanics] -Pages = ["frames.jl", "PlanarMechanics/utils.jl"] -``` - -## Joints - -A joint restricts the number of degrees of freedom (DOF) of a body. For example, a free floating body has 6 DOF, but if it is attached to a [`Revolute`](@ref) joint, the joint restricts all but one rotational degree of freedom (a revolute joint acts like a hinge). Similarily, a [`Prismatic`](@ref) joint restricts all but one translational degree of freedom (a prismatic joint acts like a slider). - -A [`Spherical`](@ref) joints restricts all translational degrees of freedom, but allows all rotational degrees of freedom. It thus transmits no torque. A [`Planar`](@ref) joint moves in a plane, i.e., it restricts one translational DOF and two rotational DOF. A [`Universal`](@ref) joint has two rotational DOF. - -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, Multibody.PlanarMechanics] -Pages = ["joints.jl", "fancy_joints.jl", "PlanarMechanics/joints.jl"] -``` - -## Components - -The perhaps most fundamental component is a [`Body`](@ref), this component has a single flange, `frame_a`, which is used to connect the body to other components. This component has a mass, a vector `r_cm` from `frame_a` to the center of mass, and a moment of inertia tensor `I` in the center of mass. The body can be thought of as a point mass with a moment of inertia tensor. - -A mass with a shape can be modeled using a [`BodyShape`](@ref). The primary difference between a [`Body`](@ref) and a [`BodyShape`](@ref) is that the latter has an additional flange, `frame_b`, which is used to connect the body to other components. The translation between `flange_a` and `flange_b` is determined by the vector `r`. The [`BodyShape`](@ref) is suitable to model, e.g., cylinders, rods, and boxes. - -A rod without a mass (just a translation), is modeled using [`FixedTranslation`](@ref). - - -```@autodocs -Modules = [Multibody, Multibody.PlanarMechanics] -Pages = ["components.jl", "wheels.jl", "PlanarMechanics/components.jl"] -``` - -## Forces -```@autodocs -Modules = [Multibody] -Pages = ["forces.jl"] -``` - -## Sensors -A sensor is an object that translates quantities in the mechanical domain into causal signals which can interact with causal components from [ModelingToolkitStandardLibrary.Blocks](https://docs.sciml.ai/ModelingToolkitStandardLibrary/stable/API/blocks/), such as control systems etc. - -```@autodocs -Modules = [Multibody, Multibody.PlanarMechanics] -Pages = ["sensors.jl", "PlanarMechanics/sensors.jl"] -``` - -## Orientation utilities -```@autodocs -Modules = [Multibody, Multibody.PlanarMechanics] -Pages = ["orientation.jl"] -``` - -## Interfaces -```@autodocs -Modules = [Multibody] -Pages = ["interfaces.jl"] -``` - -## Trajectory planning -Two methods of planning trajectories are available -- [`point_to_point`](@ref): Generate a minimum-time point-to-point trajectory with specified start and endpoints, not exceeding specified speed and acceleration limits. -- [`traj5`](@ref): Generate a 5:th order polynomial trajectory with specified start and end points. Additionally allows specification of start and end values for velocity and acceleration. - -Components that make use of these trajectory generators is provided: -- [`KinematicPTP`](@ref) -- [`Kinematic5`](@ref) - -These both have output connectors of type `RealOutput` called `q, qd, qdd` for positions, velocities and accelerations. - -See [Industrial robot](@ref) for an example making use of the [`point_to_point`](@ref) planner. - -```@autodocs -Modules = [Multibody] -Pages = ["path_planning.jl", "ptp.jl"] -``` \ No newline at end of file diff --git a/docs/src/interfaces.md b/docs/src/interfaces.md new file mode 100644 index 00000000..88096950 --- /dev/null +++ b/docs/src/interfaces.md @@ -0,0 +1,7 @@ +# Interfaces + +## Docstrings +```@autodocs +Modules = [Multibody] +Pages = ["interfaces.jl"] +``` \ No newline at end of file diff --git a/docs/src/joints.md b/docs/src/joints.md new file mode 100644 index 00000000..c600bc9b --- /dev/null +++ b/docs/src/joints.md @@ -0,0 +1,16 @@ +# Joints + +A joint restricts the number of degrees of freedom (DOF) of a body. For example, a free floating body has 6 DOF, but if it is attached to a [`Revolute`](@ref) joint, the joint restricts all but one rotational degree of freedom (a revolute joint acts like a hinge). Similarily, a [`Prismatic`](@ref) joint restricts all but one translational degree of freedom (a prismatic joint acts like a slider). + +A [`Spherical`](@ref) joints restricts all translational degrees of freedom, but allows all rotational degrees of freedom. It thus transmits no torque. A [`Planar`](@ref) joint moves in a plane, i.e., it restricts one translational DOF and two rotational DOF. A [`Universal`](@ref) joint has two rotational DOF. + +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. + +## Docstrings +```@index +``` + +```@autodocs +Modules = [Multibody, Multibody.PlanarMechanics] +Pages = ["joints.jl", "fancy_joints.jl", "PlanarMechanics/joints.jl"] +``` diff --git a/docs/src/rotations.md b/docs/src/rotations.md index a7864505..24773c83 100644 --- a/docs/src/rotations.md +++ b/docs/src/rotations.md @@ -62,5 +62,12 @@ rotation_axis(R), rotation_angle(R) # Get an axis-angle representation See [Orientations and directions](@ref) -## Orientation API -See [Orientation utilities](@ref) \ No newline at end of file + +## Docstrings +```@index +``` + +```@autodocs +Modules = [Multibody, Multibody.PlanarMechanics] +Pages = ["orientation.jl"] +``` \ No newline at end of file diff --git a/docs/src/sensors.md b/docs/src/sensors.md new file mode 100644 index 00000000..900c9194 --- /dev/null +++ b/docs/src/sensors.md @@ -0,0 +1,14 @@ +# Sensors + + +## Docstrings +```@index +``` + + +A sensor is an object that translates quantities in the mechanical domain into causal signals which can interact with causal components from [ModelingToolkitStandardLibrary.Blocks](https://docs.sciml.ai/ModelingToolkitStandardLibrary/stable/API/blocks/), such as control systems etc. + +```@autodocs +Modules = [Multibody, Multibody.PlanarMechanics] +Pages = ["sensors.jl", "PlanarMechanics/sensors.jl"] +``` diff --git a/docs/src/trajectory_planning.md b/docs/src/trajectory_planning.md new file mode 100644 index 00000000..8e24408c --- /dev/null +++ b/docs/src/trajectory_planning.md @@ -0,0 +1,24 @@ +# Trajectory_planning + +Two methods of planning trajectories are available +- [`point_to_point`](@ref): Generate a minimum-time point-to-point trajectory with specified start and endpoints, not exceeding specified speed and acceleration limits. +- [`traj5`](@ref): Generate a 5:th order polynomial trajectory with specified start and end points. Additionally allows specification of start and end values for velocity and acceleration. + +Components that make use of these trajectory generators is provided: +- [`KinematicPTP`](@ref) +- [`Kinematic5`](@ref) + +These both have output connectors of type `RealOutput` called `q, qd, qdd` for positions, velocities and accelerations. + +See [Industrial robot](@ref) for an example making use of the [`point_to_point`](@ref) planner. + +## Docstrings + +```@index +``` + + +```@autodocs +Modules = [Multibody] +Pages = ["path_planning.jl", "ptp.jl"] +``` \ No newline at end of file diff --git a/src/Multibody.jl b/src/Multibody.jl index 4470e20d..56c8effd 100644 --- a/src/Multibody.jl +++ b/src/Multibody.jl @@ -223,7 +223,7 @@ include("frames.jl") export PartialTwoFrames include("interfaces.jl") -export World, world, Mounting1D, Fixed, FixedTranslation, FixedRotation, Body, BodyShape, BodyCylinder, BodyBox, Rope +export World, world, Mounting1D, Fixed, Position, Pose, FixedTranslation, FixedRotation, Body, BodyShape, BodyCylinder, BodyBox, Rope include("components.jl") export Revolute, Prismatic, Planar, Spherical, Universal, diff --git a/src/components.jl b/src/components.jl index 201bae2d..7816f792 100644 --- a/src/components.jl +++ b/src/components.jl @@ -79,6 +79,11 @@ function inner_gravity(point_gravity, mu, g, n, r) end +""" + Fixed(; name, r = [0, 0, 0], render = true) + +A component rigidly attached to the world frame with translation `r` between the world and the `frame_b`. The position vector `r` is resolved in the world frame. +""" @component function Fixed(; name, r = [0, 0, 0], render = true) systems = @named begin frame_b = Frame() end @parameters begin @@ -91,6 +96,89 @@ end add_params(sys, [render]; name) end +""" + Position(; name, r = [0, 0, 0], render = true, fixed_oreintation = true) + +Forced movement of a flange according to a reference position `r`. Similar to [`Fixed`](@ref), but `r` is not a parameter, and may thus be any time-varying symbolic expression. The reference position vector `r` is resolved in the world frame. Example: `r = [sin(t), 0, 0]`. + +# Arguments: +- `r`: Position vector from world frame to frame_b, resolved in world frame +- `render`: Render the component in animations +- `fixed_oreintation`: If `true`, the orientation of the frame is fixed to the world frame. If `false`, the orientation is free to change. + +See also [`Pose`](@ref) for a component that allows for forced orientation as well. +""" +@component function Position(; name, r = [0, 0, 0], render = true, fixed_oreintation = true) + systems = @named begin frame_b = Frame() end + @parameters begin + render = render, [description = "Render the component in animations"] + end + @variables begin + p(t)[1:3], [description = "Position vector from world frame to frame_b, resolved in world frame"] + v(t)[1:3], [description = "Absolute velocity of frame_b, resolved in world frame"] + a(t)[1:3], [description = "Absolute acceleration of frame_b, resolved in world frame"] + end + eqs = [ + collect(frame_b.r_0 .~ r) + collect(frame_b.r_0 .~ p) + collect(v .~ D.(p)) + collect(a .~ D.(v)) + ] + if fixed_oreintation + append!(eqs, ori(frame_b) ~ nullrotation()) + end + sys = compose(ODESystem(eqs, t; name=:nothing), systems...) + add_params(sys, [render]; name) +end + +""" + Pose(; name, r = [0, 0, 0], R, q, render = true) + +Forced movement of a flange according to a reference position `r` and reference orientation `R`. The reference arrays `r` and `R` are resolved in the world frame, and may be any symbolic expression. As an alternative to specifying `R`, it is possible to specify a quaternion `q` (4-vector quaternion with real part first). + +Example usage: +``` +using Multibody.Rotations +R = RotXYZ(0, 0.5sin(t), 0) +@named rot = Multibody.Pose(; r=[sin(t), 0, 0], R) +``` + +# Arguments +- `r`: Position vector from world frame to frame_b, resolved in world frame +- `R`: Reference orientation matrix +- `q`: Reference quaternion (optional alternative to `R`) +- `render`: Render the component in animations +- `normalize`: If a quaternion is provided, normalize the quaternion (default = true) + +See also [`Position`](@ref) for a component that allows for only forced translation. +""" +@component function Pose(; name, r = [0, 0, 0], R=nothing, q=nothing, render = true, normalize=true) + systems = @named begin frame_b = Frame() end + @parameters begin + render = render, [description = "Render the component in animations"] + end + @variables begin + p(t)[1:3], [description = "Position vector from world frame to frame_b, resolved in world frame"] + v(t)[1:3], [description = "Absolute velocity of frame_b, resolved in world frame"] + a(t)[1:3], [description = "Absolute acceleration of frame_b, resolved in world frame"] + end + eqs = [ + collect(frame_b.r_0 .~ r) + collect(frame_b.r_0 .~ p) + collect(v .~ D.(p)) + collect(a .~ D.(v)) + if R !== nothing + ori(frame_b).R ~ R + elseif q !== nothing + ori(frame_b) ~ from_Q(q, 0; normalize) + else + error("Either R or q must be provided") + end + ] + sys = compose(ODESystem(eqs, t; name=:nothing), systems...) + add_params(sys, [render]; name) +end + @component function Mounting1D(; name, n = [1, 0, 0], phi0 = 0) systems = @named begin flange_b = Rotational.Flange() diff --git a/src/orientation.jl b/src/orientation.jl index 81ae7582..1d75d80d 100644 --- a/src/orientation.jl +++ b/src/orientation.jl @@ -256,8 +256,11 @@ end # end Base.:/(q::Rotations.Quaternions.Quaternion, x::Num) = Rotations.Quaternions.Quaternion(q.s / x, q.v1 / x, q.v2 / x, q.v3 / x) -function from_Q(Q2, w) +function from_Q(Q2, w; normalize=false) # Q2 = to_q(Q) # Due to different conventions + if normalize + Q2 = Q2 / _norm(Q2) + end q = Rotations.QuatRotation(Q2, false) R = RotMatrix(q) RotationMatrix(R, w)