From cc3fdfc77f69ff4d143e58c6c0e9519442576dd0 Mon Sep 17 00:00:00 2001 From: Twan Koolen Date: Sun, 6 Nov 2016 11:55:09 -0500 Subject: [PATCH 1/2] fix angle_axis_proper (...), add test --- src/spatial.jl | 16 +++------------- src/third_party_addendum.jl | 30 ++++++++++++++++++------------ test/runtests.jl | 1 + test/test_util.jl | 11 +++++++++++ 4 files changed, 33 insertions(+), 25 deletions(-) create mode 100644 test/test_util.jl diff --git a/src/spatial.jl b/src/spatial.jl index 53bc5362..b2994d3d 100644 --- a/src/spatial.jl +++ b/src/spatial.jl @@ -45,16 +45,6 @@ function (+){T}(inertia1::SpatialInertia{T}, inertia2::SpatialInertia{T}) SpatialInertia(inertia1.frame, moment, crossPart, mass) end -function vector_to_skew_symmetric{T}(v::SVector{3, T}) - @SMatrix [zero(T) -v[3] v[2]; - v[3] zero(T) -v[1]; - -v[2] v[1] zero(T)] -end - -function cross(a::SVector{3}, B::AbstractMatrix) - vector_to_skew_symmetric(a) * B -end - @inline function vector_to_skew_symmetric_squared(a::SVector{3}) aSq1 = a[1] * a[1] aSq2 = a[2] * a[2] @@ -83,7 +73,7 @@ function transform{I, T}(inertia::SpatialInertia{I}, t::Transform3D{T})::Spatial m = convert(S, inertia.mass) c = convert(SVector{3, S}, inertia.crossPart) - R = rotationmatrix_normalized_fsa(convert(Quaternion{S}, t.rot)) + R = rotation_matrix(convert(Quaternion{S}, t.rot)) p = convert(SVector{3, S}, t.trans) cnew = R * c @@ -257,7 +247,7 @@ end function transform(jac::GeometricJacobian, transform::Transform3D) framecheck(jac.frame, transform.from) - R = rotationmatrix_normalized_fsa(transform.rot) + R = rotation_matrix(transform.rot) angular = R * jac.angular linear = R * jac.linear + cross(transform.trans, angular) GeometricJacobian(jac.body, jac.base, transform.to, angular, linear) @@ -396,7 +386,7 @@ end function transform(mat::MomentumMatrix, transform::Transform3D) framecheck(mat.frame, transform.from) - R = rotationmatrix_normalized_fsa(transform.rot) + R = rotation_matrix(transform.rot) linear = R * linear_part(mat) T = eltype(linear) angular = R * angular_part(mat) + cross(transform.trans, linear) diff --git a/src/third_party_addendum.jl b/src/third_party_addendum.jl index e3355c69..45ce0575 100644 --- a/src/third_party_addendum.jl +++ b/src/third_party_addendum.jl @@ -1,23 +1,29 @@ -rotate(x::SMatrix{3}, q::Quaternion) = rotationmatrix_normalized_fsa(q) * x +function vector_to_skew_symmetric{T}(v::SVector{3, T}) + @SMatrix [zero(T) -v[3] v[2]; + v[3] zero(T) -v[1]; + -v[2] v[1] zero(T)] +end + +const hat = vector_to_skew_symmetric + +function cross(a::SVector{3}, B::AbstractMatrix) + vector_to_skew_symmetric(a) * B +end + +rotate(x::SMatrix{3}, q::Quaternion) = rotation_matrix(q) * x function rotate(x::SVector{3}, q::Quaternion) qret = q * Quaternion(zero(eltype(x)), x[1], x[2], x[3]) * inv(q) SVector(qret.v1, qret.v2, qret.v3) end -angle_axis_proper(q::Quaternion) = angle_proper(q), axis_proper(q) - -angle_proper(q::Quaternion) = 2 * acos(real(Quaternions.normalize(q))) - -function axis_proper(q::Quaternion) - q = Quaternions.normalize(q) - s = sin(angle(q) / 2) - abs(s) > 0 ? - [q.v1, q.v2, q.v3] / s : - [1.0, 0.0, 0.0] +function angle_axis_proper{T}(q::Quaternion{T}) + Θ = 2 * atan2(√(q.v1^2 + q.v2^2 + q.v3^2), q.s) + axis = isapprox(Θ, zero(T)) ? SVector{3, T}(1, 0, 0) : SVector{3, T}(q.v1, q.v2, q.v3) / sin(Θ / 2) + Θ, axis end -function rotationmatrix_normalized_fsa{T}(q::Quaternion{T}) +function rotation_matrix{T}(q::Quaternion{T}) sx, sy, sz = 2q.s*q.v1, 2q.s*q.v2, 2q.s*q.v3 xx, xy, xz = 2q.v1^2, 2q.v1*q.v2, 2q.v1*q.v3 yy, yz, zz = 2q.v2^2, 2q.v2*q.v3, 2q.v3^2 diff --git a/test/runtests.jl b/test/runtests.jl index c6a13c5c..5f0342af 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -9,6 +9,7 @@ import ForwardDiff # useful utility function for computing time derivatives. create_autodiff(x, dx) = [ForwardDiff.Dual(x[i], dx[i]) for i in 1 : length(x)] +include("test_util.jl") include("test_tree.jl") include("test_frames.jl") include("test_spatial.jl") diff --git a/test/test_util.jl b/test/test_util.jl new file mode 100644 index 00000000..96afe7d2 --- /dev/null +++ b/test/test_util.jl @@ -0,0 +1,11 @@ +import RigidBodyDynamics: angle_axis_proper, hat, rotation_matrix + +@testset "util" begin + @testset "rotation" begin + quat = nquatrand() + R = rotation_matrix(quat) + angle, axis = angle_axis_proper(quat) + rotationvector = angle * axis + @test isapprox(R, expm(hat(rotationvector)); atol = 1e-10) + end +end From 730d90e12f31cc5a28e524100c267ab97a78ba69 Mon Sep 17 00:00:00 2001 From: Twan Koolen Date: Sun, 6 Nov 2016 11:59:24 -0500 Subject: [PATCH 2/2] hat instead of vector_to_skew_symmetric --- src/spatial.jl | 21 +++------------------ src/third_party_addendum.jl | 21 +++++++++++++++++++-- test/test_spatial.jl | 2 +- 3 files changed, 23 insertions(+), 21 deletions(-) diff --git a/src/spatial.jl b/src/spatial.jl index b2994d3d..45847309 100644 --- a/src/spatial.jl +++ b/src/spatial.jl @@ -13,7 +13,7 @@ end function convert{T}(::Type{SMatrix{6, 6, T}}, inertia::SpatialInertia) J = inertia.moment - C = vector_to_skew_symmetric(inertia.crossPart) + C = hat(inertia.crossPart) m = inertia.mass [J C; C' m * eye(SMatrix{3, 3, T})] end @@ -45,21 +45,6 @@ function (+){T}(inertia1::SpatialInertia{T}, inertia2::SpatialInertia{T}) SpatialInertia(inertia1.frame, moment, crossPart, mass) end -@inline function vector_to_skew_symmetric_squared(a::SVector{3}) - aSq1 = a[1] * a[1] - aSq2 = a[2] * a[2] - aSq3 = a[3] * a[3] - b11 = -aSq2 - aSq3 - b12 = a[1] * a[2] - b13 = a[1] * a[3] - b22 = -aSq1 - aSq3 - b23 = a[2] * a[3] - b33 = -aSq1 - aSq2 - @SMatrix [b11 b12 b13; - b12 b22 b23; - b13 b23 b33] -end - function transform{I, T}(inertia::SpatialInertia{I}, t::Transform3D{T})::SpatialInertia{promote_type(I, T)} framecheck(t.from, inertia.frame) S = promote_type(I, T) @@ -77,9 +62,9 @@ function transform{I, T}(inertia::SpatialInertia{I}, t::Transform3D{T})::Spatial p = convert(SVector{3, S}, t.trans) cnew = R * c - Jnew = vector_to_skew_symmetric_squared(cnew) + Jnew = hat_squared(cnew) cnew += m * p - Jnew -= vector_to_skew_symmetric_squared(cnew) + Jnew -= hat_squared(cnew) mInv = inv(m) Jnew *= mInv Jnew += R * J * R' diff --git a/src/third_party_addendum.jl b/src/third_party_addendum.jl index 45ce0575..666a7266 100644 --- a/src/third_party_addendum.jl +++ b/src/third_party_addendum.jl @@ -1,4 +1,4 @@ -function vector_to_skew_symmetric{T}(v::SVector{3, T}) +@inline function vector_to_skew_symmetric{T}(v::SVector{3, T}) @SMatrix [zero(T) -v[3] v[2]; v[3] zero(T) -v[1]; -v[2] v[1] zero(T)] @@ -6,8 +6,25 @@ end const hat = vector_to_skew_symmetric +@inline function vector_to_skew_symmetric_squared(a::SVector{3}) + aSq1 = a[1] * a[1] + aSq2 = a[2] * a[2] + aSq3 = a[3] * a[3] + b11 = -aSq2 - aSq3 + b12 = a[1] * a[2] + b13 = a[1] * a[3] + b22 = -aSq1 - aSq3 + b23 = a[2] * a[3] + b33 = -aSq1 - aSq2 + @SMatrix [b11 b12 b13; + b12 b22 b23; + b13 b23 b33] +end + +const hat_squared = vector_to_skew_symmetric_squared + function cross(a::SVector{3}, B::AbstractMatrix) - vector_to_skew_symmetric(a) * B + hat(a) * B end rotate(x::SMatrix{3}, q::Quaternion) = rotation_matrix(q) * x diff --git a/test/test_spatial.jl b/test/test_spatial.jl index 6ac1ab18..c724a456 100644 --- a/test/test_spatial.jl +++ b/test/test_spatial.jl @@ -1,6 +1,6 @@ function Ad(H::Transform3D) R = rotationmatrix(H.rot) - pHat = Array(RigidBodyDynamics.vector_to_skew_symmetric(H.trans)) + pHat = Array(RigidBodyDynamics.hat(H.trans)) return [R zeros(3, 3); pHat * R R] end