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..014ea54f 100644
--- a/test/test_urdf_parser.jl
+++ b/test/test_urdf_parser.jl
@@ -41,5 +41,65 @@
@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 rot ≈ RotZ(3) * RotY(2) * RotX(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.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