From 70d843250e5d536df2b43030cc8b72e205a4dfe2 Mon Sep 17 00:00:00 2001 From: Robin Deits Date: Fri, 11 May 2018 15:43:09 -0400 Subject: [PATCH 1/2] fix parsing of ROS-formatted rpy in URDFs and add some tests --- src/parse_urdf.jl | 4 +-- test/test_urdf_parser.jl | 58 ++++++++++++++++++++++++++++++++++++++++ 2 files changed, 60 insertions(+), 2 deletions(-) diff --git a/src/parse_urdf.jl b/src/parse_urdf.jl index 128d4dbe..bce430c7 100644 --- a/src/parse_urdf.jl +++ b/src/parse_urdf.jl @@ -28,8 +28,8 @@ function parse_pose(::Type{T}, xml_pose::Nothing) where {T} end function parse_pose(::Type{T}, xml_pose::XMLElement) where {T} - rpy = RotXYZ(parse_vector(T, xml_pose, "rpy", "0 0 0")...) - rot = RotMatrix(rpy) + rpy = parse_vector(T, xml_pose, "rpy", "0 0 0") + rot = RotMatrix(RotZYX(rpy[3], rpy[2], rpy[1])) trans = SVector{3}(parse_vector(T, xml_pose, "xyz", "0 0 0")) rot, trans end diff --git a/test/test_urdf_parser.jl b/test/test_urdf_parser.jl index 106c6a1f..af222709 100644 --- a/test/test_urdf_parser.jl +++ b/test/test_urdf_parser.jl @@ -41,5 +41,63 @@ @test jt.x_axis ≈ SVector(1, 0, 0) @test jt.y_axis ≈ SVector(0, 1, 0) end + + @testset "rotation handling" begin + # https://github.com/JuliaRobotics/RigidBodyDynamics.jl/issues/429 + xml = """ + + """ + xpose = LightXML.root(LightXML.parse_string(xml)) + rot, trans = RigidBodyDynamics.parse_pose(Float64, xpose) + @test rot ≈ SDiagonal(1, 1, 1) + @test trans ≈ SVector(0, 0, 0) + + xml = """ + + """ + xpose = LightXML.root(LightXML.parse_string(xml)) + rot, trans = RigidBodyDynamics.parse_pose(Float64, xpose) + @test rot ≈ RotMatrix(RotX(0.3)) + @test trans ≈ SVector(0, 0, 0) + + xml = """ + + """ + xpose = LightXML.root(LightXML.parse_string(xml)) + rot, trans = RigidBodyDynamics.parse_pose(Float64, xpose) + @test rot ≈ RotMatrix(RotY(0.2)) + @test trans ≈ SVector(0, 0, 0) + + xml = """ + + """ + xpose = LightXML.root(LightXML.parse_string(xml)) + rot, trans = RigidBodyDynamics.parse_pose(Float64, xpose) + @test rot ≈ RotMatrix(RotZ(0.1)) + @test trans ≈ SVector(0, 0, 0) + + # Comparison against ROS's tf.transformations.quaternion_from_euler + xml = """ + + """ + xpose = LightXML.root(LightXML.parse_string(xml)) + rot, trans = RigidBodyDynamics.parse_pose(Float64, xpose) + @test rot ≈ [0.41198225 -0.83373765 -0.36763046; + -0.05872664 -0.42691762 0.90238159; + -0.90929743 -0.35017549 -0.2248451] atol=1e-7 + @test trans ≈ SVector(0, 0, 0) + + + # Comparison against ROS's tf.transformations.quaternion_from_euler + xml = """ + + """ + xpose = LightXML.root(LightXML.parse_string(xml)) + rot, trans = RigidBodyDynamics.parse_pose(Float64, xpose) + @test rot ≈ [0.97517033 -0.12744012 0.18111281; + 0.19767681 0.86959819 -0.45246312; + -0.09983342 0.47703041 0.8731983] atol=1e-7 + @test trans ≈ SVector(0, 0, 0) + end end From f314f18f5d0575cc86743e24761dd2498c4fec1b Mon Sep 17 00:00:00 2001 From: Robin Deits Date: Fri, 11 May 2018 15:55:26 -0400 Subject: [PATCH 2/2] more testing --- test/test_urdf_parser.jl | 2 ++ 1 file changed, 2 insertions(+) diff --git a/test/test_urdf_parser.jl b/test/test_urdf_parser.jl index af222709..014ea54f 100644 --- a/test/test_urdf_parser.jl +++ b/test/test_urdf_parser.jl @@ -85,6 +85,7 @@ @test rot ≈ [0.41198225 -0.83373765 -0.36763046; -0.05872664 -0.42691762 0.90238159; -0.90929743 -0.35017549 -0.2248451] atol=1e-7 + @test rot ≈ RotZ(3) * RotY(2) * RotX(1) @test trans ≈ SVector(0, 0, 0) @@ -97,6 +98,7 @@ @test rot ≈ [0.97517033 -0.12744012 0.18111281; 0.19767681 0.86959819 -0.45246312; -0.09983342 0.47703041 0.8731983] atol=1e-7 + @test rot ≈ RotZ(0.2) * RotY(0.1) * RotX(0.5) @test trans ≈ SVector(0, 0, 0) end end