diff --git a/src/RigidBodyDynamics.jl b/src/RigidBodyDynamics.jl index f94b1857..7495b9fe 100644 --- a/src/RigidBodyDynamics.jl +++ b/src/RigidBodyDynamics.jl @@ -129,6 +129,8 @@ export center_of_mass, geometric_jacobian, geometric_jacobian!, + point_velocity, + point_acceleration, point_jacobian, point_jacobian!, relative_acceleration, diff --git a/src/spatial/Spatial.jl b/src/spatial/Spatial.jl index 3e061eed..d5b7159e 100644 --- a/src/spatial/Spatial.jl +++ b/src/spatial/Spatial.jl @@ -24,6 +24,7 @@ export angular, linear, point_velocity, + point_acceleration, change_base, log_with_time_derivative, center_of_mass, diff --git a/src/spatial/spatialmotion.jl b/src/spatial/spatialmotion.jl index eb9fe172..02162460 100644 --- a/src/spatial/spatialmotion.jl +++ b/src/spatial/spatialmotion.jl @@ -309,7 +309,7 @@ end """ $(SIGNATURES) -Given a twist ``T_{j}^{k,i}`` of frame ``j`` with respect to frame ``i``, expressed in frame ``k``, +Given the twist ``T_{j}^{k,i}`` of frame ``j`` with respect to frame ``i``, expressed in frame ``k``, and the location of a point fixed in frame ``j``, also expressed in frame ``k``, compute the velocity of the point relative to frame ``i``. """ @@ -318,6 +318,20 @@ function point_velocity(twist::Twist, point::Point3D) FreeVector3D(twist.frame, angular(twist) × point.v + linear(twist)) end +""" +$(SIGNATURES) + +Given the twist ``dot{T}_{j}^{k,i}`` of frame ``j`` with respect to frame ``i``, expressed in frame ``k`` +and its time derivative (a spatial acceleration), as well as the location of a point fixed in frame ``j``, +also expressed in frame ``k``, compute the acceleration of the point relative to frame ``i``. +""" +function point_acceleration(twist::Twist, accel::SpatialAcceleration, point::Point3D) + @framecheck twist.base accel.base + @framecheck twist.body accel.body + @framecheck twist.frame accel.frame + FreeVector3D(accel.frame, angular(accel) × point.v + linear(accel) + angular(twist) × point_velocity(twist, point).v) +end + # SpatialAcceleration-specific functions """ diff --git a/test/test_spatial.jl b/test/test_spatial.jl index 63bde3b3..f70a09ba 100644 --- a/test/test_spatial.jl +++ b/test/test_spatial.jl @@ -194,6 +194,23 @@ end @test Ṫ + zero(Ṫ) == Ṫ end + @testset "point_velocity, point_acceleration" begin + body = CartesianFrame3D("body") + base = CartesianFrame3D("base") + frame = CartesianFrame3D("some other frame") # yes, the math does check out if this is different from the other two; ṗ will just be rotated to this frame + T = rand(Twist{Float64}, body, base, frame) + Ṫ = rand(SpatialAcceleration{Float64}, body, base, frame) + p = Point3D(frame, rand(SVector{3})) + ṗ = point_velocity(T, p) + p̈ = point_acceleration(T, Ṫ, p) + @test p̈.frame == frame + p_dual = Point3D(p.frame, ForwardDiff.Dual.(p.v, ṗ.v)) + T_dual = Twist(T.body, T.base, T.frame, ForwardDiff.Dual.(angular(T), angular(Ṫ)), ForwardDiff.Dual.(linear(T), linear(Ṫ))) + ṗ_dual = point_velocity(T_dual, p_dual) + p̈_check = FreeVector3D(ṗ_dual.frame, map(x -> ForwardDiff.partials(x, 1), ṗ_dual.v)) + @test p̈ ≈ p̈_check atol=1e-12 + end + @testset "kinetic energy" begin I = rand(SpatialInertia{Float64}, f2) T = rand(Twist{Float64}, f2, f1, f2)