Merge pull request #515 from JuliaRobotics/tk/widen-mat-ctors

Work around #505, lose StaticArrays upper bound.
JuliaRobotics · Nov 19, 2018 · 2357e91 · 2357e91
2 parents e68f5a7 + 3645fd0
commit 2357e91
Show file tree

Hide file tree

Showing 7 changed files with 43 additions and 19 deletions.
diff --git a/REQUIRE b/REQUIRE
@@ -1,5 +1,5 @@
 julia 0.7
-StaticArrays 0.8.2 0.9-
+StaticArrays 0.8.2
 Rotations 0.7.1
 TypeSortedCollections 0.5.0
 LightXML 0.4.0

diff --git a/notebooks/Symbolic double pendulum/Manifest.toml b/notebooks/Symbolic double pendulum/Manifest.toml
@@ -15,9 +15,9 @@ version = "0.5.2"
 
 [[Compat]]
 deps = ["Base64", "Dates", "DelimitedFiles", "Distributed", "InteractiveUtils", "LibGit2", "Libdl", "LinearAlgebra", "Markdown", "Mmap", "Pkg", "Printf", "REPL", "Random", "Serialization", "SharedArrays", "Sockets", "SparseArrays", "Statistics", "Test", "UUIDs", "Unicode"]
-git-tree-sha1 = "2d9e14d19bad3f9ad5cc5e4cffabc3cfa59de825"
+git-tree-sha1 = "ec61a16eed883ad0cfa002d7489b3ce6d039bb9a"
 uuid = "34da2185-b29b-5c13-b0c7-acf172513d20"
-version = "1.3.0"
+version = "1.4.0"
 
 [[Conda]]
 deps = ["Compat", "JSON", "VersionParsing"]
@@ -166,9 +166,9 @@ version = "0.7.2"
 
 [[StaticArrays]]
 deps = ["InteractiveUtils", "LinearAlgebra", "Random", "Statistics", "Test"]
-git-tree-sha1 = "d432c79bef174a830304f8601427a4357dfdbfb7"
+git-tree-sha1 = "ebc5c2a27d91d5ec611a9861168182e2168effd3"
 uuid = "90137ffa-7385-5640-81b9-e52037218182"
-version = "0.8.3"
+version = "0.9.2"
 
 [[Statistics]]
 deps = ["LinearAlgebra", "SparseArrays"]

diff --git a/src/spatial/motion_force_interaction.jl b/src/spatial/motion_force_interaction.jl
@@ -259,8 +259,6 @@ for (MatrixType, VectorType) in (:WrenchMatrix => :(Union{Twist, SpatialAccelera
     end
 end
 
-
-
 """
 $(SIGNATURES)
 

diff --git a/src/spatial/spatialforce.jl b/src/spatial/spatialforce.jl
@@ -4,7 +4,7 @@ for ForceSpaceMatrix in (:MomentumMatrix, :WrenchMatrix)
         angular::A
         linear::A
 
-        @inline function $ForceSpaceMatrix(frame::CartesianFrame3D, angular::A, linear::A) where {A<:AbstractMatrix}
+        @inline function $ForceSpaceMatrix{A}(frame::CartesianFrame3D, angular::AbstractMatrix, linear::AbstractMatrix) where {A<:AbstractMatrix}
             @boundscheck size(angular, 1) == 3 || throw(DimensionMismatch())
             @boundscheck size(linear, 1) == 3 || throw(DimensionMismatch())
             @boundscheck size(angular, 2) == size(linear, 2) || throw(DimensionMismatch())
@@ -27,11 +27,15 @@ MomentumMatrix
 
 for ForceSpaceMatrix in (:MomentumMatrix, :WrenchMatrix)
     @eval begin
-        function $ForceSpaceMatrix{A}(mat::$ForceSpaceMatrix) where A
-            $ForceSpaceMatrix(mat.frame, convert(A, angular(mat)), convert(A, linear(mat)))
+        @inline function $ForceSpaceMatrix(frame::CartesianFrame3D, angular::A, linear::A) where {A<:AbstractMatrix}
+            $ForceSpaceMatrix{A}(frame, angular, linear)
+        end
+
+        @inline function $ForceSpaceMatrix(frame::CartesianFrame3D, angular::A1, linear::A2) where {A1<:AbstractMatrix, A2<:AbstractMatrix}
+            $ForceSpaceMatrix(frame, promote(angular, linear)...)
         end
 
-        function $ForceSpaceMatrix{A}(mat::$ForceSpaceMatrix{A}) where A
+        @inline function $ForceSpaceMatrix{A}(mat::$ForceSpaceMatrix) where A
             $ForceSpaceMatrix(mat.frame, A(angular(mat)), A(linear(mat)))
         end
 

diff --git a/src/spatial/spatialmotion.jl b/src/spatial/spatialmotion.jl
@@ -11,7 +11,9 @@ struct GeometricJacobian{A<:AbstractMatrix}
     angular::A
     linear::A
 
-    @inline function GeometricJacobian(body::CartesianFrame3D, base::CartesianFrame3D, frame::CartesianFrame3D, angular::A, linear::A) where {A<:AbstractMatrix}
+    @inline function GeometricJacobian{A}(
+            body::CartesianFrame3D, base::CartesianFrame3D, frame::CartesianFrame3D,
+            angular::AbstractMatrix, linear::AbstractMatrix) where A<:AbstractMatrix
         @boundscheck size(angular, 1) == 3 || throw(DimensionMismatch())
         @boundscheck size(linear, 1) == 3 || throw(DimensionMismatch())
         @boundscheck size(angular, 2) == size(linear, 2) || throw(DimensionMismatch())
@@ -20,11 +22,19 @@ struct GeometricJacobian{A<:AbstractMatrix}
 end
 
 # GeometricJacobian-specific functions
-function GeometricJacobian{A}(jac::GeometricJacobian) where A
-    GeometricJacobian(jac.body, jac.base, jac.frame, convert(A, angular(jac)), convert(A, linear(jac)))
+@inline function GeometricJacobian(
+        body::CartesianFrame3D, base::CartesianFrame3D, frame::CartesianFrame3D,
+        angular::A, linear::A) where {A<:AbstractMatrix}
+    GeometricJacobian{A}(body, base, frame, angular, linear)
 end
 
-function GeometricJacobian{A}(jac::GeometricJacobian{A}) where A
+@inline function GeometricJacobian(
+        body::CartesianFrame3D, base::CartesianFrame3D, frame::CartesianFrame3D,
+        angular::A1, linear::A2) where {A1<:AbstractMatrix, A2<:AbstractMatrix}
+    GeometricJacobian(body, base, frame, promote(angular, linear)...)
+end
+
+@inline function GeometricJacobian{A}(jac::GeometricJacobian) where A
     GeometricJacobian(jac.body, jac.base, jac.frame, A(angular(jac)), A(linear(jac)))
 end
 

diff --git a/test/test_mechanism_algorithms.jl b/test/test_mechanism_algorithms.jl
@@ -156,9 +156,15 @@ end
                 vj = v[vrange]
 
                 Jv_to_q̇_j = RigidBodyDynamics.velocity_to_configuration_derivative_jacobian(joint, qj)
-                @test Jv_to_q̇_j * vj ≈ q̇j
                 Jq̇_to_v_j = RigidBodyDynamics.configuration_derivative_to_velocity_jacobian(joint, qj)
-                @test Jq̇_to_v_j * q̇j ≈ vj
+
+                if num_velocities(joint) > 0
+                    @test Jv_to_q̇_j * vj ≈ q̇j
+                    @test Jq̇_to_v_j * q̇j ≈ vj
+                else
+                    @test size(Jv_to_q̇_j) == (0, 0)
+                    @test size(Jq̇_to_v_j) == (0, 0)
+                end
             end
         end
     end
@@ -379,7 +385,13 @@ end
             S = motion_subspace(joint, configuration(x, joint))
             tf = joint_transform(joint, qjoint)
             T = constraint_wrench_subspace(joint, tf)
-            @test isapprox(angular(T)' * angular(S) + linear(T)' * linear(S), zeros(num_constraints(joint), num_velocities(joint)); atol = 1e-12)
+            if 0 < num_constraints(joint) < 6
+                @test isapprox(angular(T)' * angular(S) + linear(T)' * linear(S), zeros(num_constraints(joint), num_velocities(joint)); atol = 1e-12)
+            elseif num_constraints(joint) == 0
+                @test size(T) == (6, 0)
+            else
+                @test size(S) == (6, 0)
+            end
         end
     end
 

diff --git a/test/test_notebooks.jl b/test/test_notebooks.jl
@@ -2,7 +2,7 @@ let
     notebookdir = joinpath(@__DIR__, "..", "notebooks")
     excludedirs = [".ipynb_checkpoints"]
     excludefiles = String[]
-    # push!(excludefiles, "Rigorous error bounds using IntervalArithmetic.ipynb")
+    push!(excludefiles, "Symbolic double pendulum.ipynb")
     for (root, dir, files) in walkdir(notebookdir)
         basename(root) in excludedirs && continue
         for file in files