Merge pull request #472 from JuliaRobotics/tk/point-accel

Add point_acceleration.

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,

src/spatial/Spatial.jl

@@ -24,6 +24,7 @@ export
     angular,
     linear,
     point_velocity,
+    point_acceleration,
     change_base,
     log_with_time_derivative,
     center_of_mass,

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
 """

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)