{"action":"created","issue":{"url":"https://api.github.com/repos/RobotLocomotion/drake/issues/3723","repository_url":"https://api.github.com/repos/RobotLocomotion/drake","labels_url":"https://api.github.com/repos/RobotLocomotion/drake/issues/3723/labels{/name}","comments_url":"https://api.github.com/repos/RobotLocomotion/drake/issues/3723/comments","events_url":"https://api.github.com/repos/RobotLocomotion/drake/issues/3723/events","html_url":"https://github.com/RobotLocomotion/drake/pull/3723","id":181763515,"number":3723,"title":"Improve algorithm for quaternion to BodyZYX Euler angles","user":{"login":"mitiguy","id":21087628,"avatar_url":"https://avatars.githubusercontent.com/u/21087628?v=3","gravatar_id":"","url":"https://api.github.com/users/mitiguy","html_url":"https://github.com/mitiguy","followers_url":"https://api.github.com/users/mitiguy/followers","following_url":"https://api.github.com/users/mitiguy/following{/other_user}","gists_url":"https://api.github.com/users/mitiguy/gists{/gist_id}","starred_url":"https://api.github.com/users/mitiguy/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/mitiguy/subscriptions","organizations_url":"https://api.github.com/users/mitiguy/orgs","repos_url":"https://api.github.com/users/mitiguy/repos","events_url":"https://api.github.com/users/mitiguy/events{/privacy}","received_events_url":"https://api.github.com/users/mitiguy/received_events","type":"User","site_admin":false},"labels":[],"state":"open","locked":false,"assignee":{"login":"hongkai-dai","id":6314000,"avatar_url":"https://avatars.githubusercontent.com/u/6314000?v=3","gravatar_id":"","url":"https://api.github.com/users/hongkai-dai","html_url":"https://github.com/hongkai-dai","followers_url":"https://api.github.com/users/hongkai-dai/followers","following_url":"https://api.github.com/users/hongkai-dai/following{/other_user}","gists_url":"https://api.github.com/users/hongkai-dai/gists{/gist_id}","starred_url":"https://api.github.com/users/hongkai-dai/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/hongkai-dai/subscriptions","organizations_url":"https://api.github.com/users/hongkai-dai/orgs","repos_url":"https://api.github.com/users/hongkai-dai/repos","events_url":"https://api.github.com/users/hongkai-dai/events{/privacy}","received_events_url":"https://api.github.com/users/hongkai-dai/received_events","type":"User","site_admin":false},"assignees":[{"login":"sherm1","id":4088016,"avatar_url":"https://avatars.githubusercontent.com/u/4088016?v=3","gravatar_id":"","url":"https://api.github.com/users/sherm1","html_url":"https://github.com/sherm1","followers_url":"https://api.github.com/users/sherm1/followers","following_url":"https://api.github.com/users/sherm1/following{/other_user}","gists_url":"https://api.github.com/users/sherm1/gists{/gist_id}","starred_url":"https://api.github.com/users/sherm1/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/sherm1/subscriptions","organizations_url":"https://api.github.com/users/sherm1/orgs","repos_url":"https://api.github.com/users/sherm1/repos","events_url":"https://api.github.com/users/sherm1/events{/privacy}","received_events_url":"https://api.github.com/users/sherm1/received_events","type":"User","site_admin":false},{"login":"hongkai-dai","id":6314000,"avatar_url":"https://avatars.githubusercontent.com/u/6314000?v=3","gravatar_id":"","url":"https://api.github.com/users/hongkai-dai","html_url":"https://github.com/hongkai-dai","followers_url":"https://api.github.com/users/hongkai-dai/followers","following_url":"https://api.github.com/users/hongkai-dai/following{/other_user}","gists_url":"https://api.github.com/users/hongkai-dai/gists{/gist_id}","starred_url":"https://api.github.com/users/hongkai-dai/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/hongkai-dai/subscriptions","organizations_url":"https://api.github.com/users/hongkai-dai/orgs","repos_url":"https://api.github.com/users/hongkai-dai/repos","events_url":"https://api.github.com/users/hongkai-dai/events{/privacy}","received_events_url":"https://api.github.com/users/hongkai-dai/received_events","type":"User","site_admin":false},{"login":"ggould-tri","id":17596413,"avatar_url":"https://avatars.githubusercontent.com/u/17596413?v=3","gravatar_id":"","url":"https://api.github.com/users/ggould-tri","html_url":"https://github.com/ggould-tri","followers_url":"https://api.github.com/users/ggould-tri/followers","following_url":"https://api.github.com/users/ggould-tri/following{/other_user}","gists_url":"https://api.github.com/users/ggould-tri/gists{/gist_id}","starred_url":"https://api.github.com/users/ggould-tri/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/ggould-tri/subscriptions","organizations_url":"https://api.github.com/users/ggould-tri/orgs","repos_url":"https://api.github.com/users/ggould-tri/repos","events_url":"https://api.github.com/users/ggould-tri/events{/privacy}","received_events_url":"https://api.github.com/users/ggould-tri/received_events","type":"User","site_admin":false}],"milestone":null,"comments":14,"created_at":"2016-10-07T20:43:30Z","updated_at":"2016-10-11T22:05:49Z","closed_at":null,"pull_request":{"url":"https://api.github.com/repos/RobotLocomotion/drake/pulls/3723","html_url":"https://github.com/RobotLocomotion/drake/pull/3723","diff_url":"https://github.com/RobotLocomotion/drake/pull/3723.diff","patch_url":"https://github.com/RobotLocomotion/drake/pull/3723.patch"},"body":"Improve algorithm for quaternion to BodyZYX Euler angles.\r\nThe new algorithm addresses inaccuracy when the 2nd-angle\r\n(associated with Y-rotation) is near pi/2 or -pi/2.\n\n<!-- Reviewable:start -->\n---\nThis change is [<img src=\"https://reviewable.io/review_button.svg\" height=\"34\" align=\"absmiddle\" alt=\"Reviewable\"/>](https://reviewable.io/reviews/robotlocomotion/drake/3723)\n<!-- Reviewable:end -->\n"},"comment":{"url":"https://api.github.com/repos/RobotLocomotion/drake/issues/comments/253060961","html_url":"https://github.com/RobotLocomotion/drake/pull/3723#issuecomment-253060961","issue_url":"https://api.github.com/repos/RobotLocomotion/drake/issues/3723","id":253060961,"user":{"login":"sherm1","id":4088016,"avatar_url":"https://avatars.githubusercontent.com/u/4088016?v=3","gravatar_id":"","url":"https://api.github.com/users/sherm1","html_url":"https://github.com/sherm1","followers_url":"https://api.github.com/users/sherm1/followers","following_url":"https://api.github.com/users/sherm1/following{/other_user}","gists_url":"https://api.github.com/users/sherm1/gists{/gist_id}","starred_url":"https://api.github.com/users/sherm1/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/sherm1/subscriptions","organizations_url":"https://api.github.com/users/sherm1/orgs","repos_url":"https://api.github.com/users/sherm1/repos","events_url":"https://api.github.com/users/sherm1/events{/privacy}","received_events_url":"https://api.github.com/users/sherm1/received_events","type":"User","site_admin":false},"created_at":"2016-10-11T22:05:49Z","updated_at":"2016-10-11T22:05:49Z","body":"Review pass done -- PTAL. +@sherm1\n\n---\n\nReviewed 1 of 3 files at r1, 1 of 5 files at r2, 5 of 5 files at r3.\nReview status: all files reviewed at latest revision, 18 unresolved discussions.\n\n---\n\n*[drake/math/quaternion.h, line 20 at r1](https://reviewable.io:443/reviews/robotlocomotion/drake/3723#-KTW0ja8pq3bPST6P5qA:-KTpVnHHuFZXRCmQ3sbN:b-vzkct) ([raw file](https://github.com/robotlocomotion/drake/blob/49aaadaa4a9b3d4aca88f70f26c327ed84771a5e/drake/math/quaternion.h#L20)):*\n<details><summary><i>Previously, mitiguy wrote…</i></summary>\n> Done.\n</details>\nThis conditional include makes me nervous (e.g. how do CMake and CLion sort out the include dependencies?).  It also leaves us with two checks of the macro far apart in the code. Is there an advantage to making this conditional? I think it would be better just to include it unconditionally, even though it may not get used in some builds.\n\n---\n\n*[drake/math/quaternion.h, line 244 at r3](https://reviewable.io:443/reviews/robotlocomotion/drake/3723#-KTpdx8MKDQGy8SRnt1T:-KTpdx8MKDQGy8SRnt1U:bfk5i9l) ([raw file](https://github.com/robotlocomotion/drake/blob/e417d2227aa1ad8a4aef726d4386f1544cd1f3d4/drake/math/quaternion.h#L244)):*\n> ```objc\n>  * http://answers.ros.org/question/58863/incorrect-rollpitch-yaw-values-using-getrpy/\n>  * This accurate algorithm avoids numerical round-off issues encountered by some\n>  * algorithms when pitch is close to PI/2 or -PI/2  (pitch = PI/2 - 1E-6).\n> ```\n\nThis should have more information about the algorithm in the comment. As it is it is yet another mystery algorithm in Drake with no obvious provenance, and some future programmer might think it could easily be replaced with a more \"standard\" approach. Should mention that this is a novel algorithm developed by Paul Mitiguy and point out why it was needed (maybe reference the [bad Wikipedia article](https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles)). Could warn about the total uselessness of `asin()/acos()` near flat spots. Maybe include the equations being used here, as in the pdf version; those are hard to back out of the code.\n\nThe above could be in an Implementation section in doxygen, or could just be a developer comment outside of doxygen. Personally I like to include implementation info in doxygen for interested/skeptical users.\n\n---\n\n*[drake/math/quaternion.h, line 262 at r3](https://reviewable.io:443/reviews/robotlocomotion/drake/3723#-KTpcl5UtgS-eSEJZshM:-KTpcl5VADTW7pLKETpg:btwe067) ([raw file](https://github.com/robotlocomotion/drake/blob/e417d2227aa1ad8a4aef726d4386f1544cd1f3d4/drake/math/quaternion.h#L262)):*\n> ```objc\n>   // e.g., i=2, j=1, k=0; the given formulas (below) for xA, yA,  xB, yB;\n>   // and the returned quantities are specific to SpaceXYZ Euler sequence.\n>   const int i = 2;\n> ```\n\nBTW, could squash into `const int i = 2, j = 1, k = 0;` so this doesn't consume so much mental and physical space.\n\n---\n\n*[drake/math/quaternion.h, line 267 at r3](https://reviewable.io:443/reviews/robotlocomotion/drake/3723#-KTpczjmRrROknyIQ6Uk:-KTpczjmRrROknyIQ6Ul:b-hzjt6) ([raw file](https://github.com/robotlocomotion/drake/blob/e417d2227aa1ad8a4aef726d4386f1544cd1f3d4/drake/math/quaternion.h#L267)):*\n> ```objc\n> \n>   // Calculate theta2 using lots of information in the rotation matrix.\n>   const Scalar Rii = R(i, i);\n> ```\n\nBTW, could squash into `const Scalar Rii = R(i, i), Rij = R(i, j), ...;`. It might be easier to see what this code is doing if it weren't stretched out vertically so much.\n\n---\n\n*[drake/math/quaternion.h, line 283 at r3](https://reviewable.io:443/reviews/robotlocomotion/drake/3723#-KTpd_vFEwgBv0Aw9fcE:-KTpd_vFEwgBv0Aw9fcF:b-gy2tx) ([raw file](https://github.com/robotlocomotion/drake/blob/e417d2227aa1ad8a4aef726d4386f1544cd1f3d4/drake/math/quaternion.h#L283)):*\n> ```objc\n>   const Scalar xA = e0 - e2;\n>   const Scalar yB = e3 - e1;\n>   const Scalar xB = e0 + e2;\n> ```\n\nBTW, consider squashing the e0..e3 and yA..xB lines to use less vertical space.\n\n---\n\n*[drake/math/quaternion.h, line 298 at r3](https://reviewable.io:443/reviews/robotlocomotion/drake/3723#-KTphpYVaTcSluvgQIBZ:-KTphpYVaTcSluvgQIB_:b-hkox1o) ([raw file](https://github.com/robotlocomotion/drake/blob/e417d2227aa1ad8a4aef726d4386f1544cd1f3d4/drake/math/quaternion.h#L298)):*\n> ```objc\n>   // Returns in SpaceXYZ (roll-pitch-yaw) order rather\n>   // than BodyZYX theta1, theta2, theta3 order.\n>   Vector3<Scalar> spaceXYZ_angles(theta3, theta2, theta1);\n> ```\n\nBTW, consider using 0-based numbering for theta to be consistent with everything else here and in Drake at large.\n\n---\n\n*[drake/math/quaternion.h, line 300 at r3](https://reviewable.io:443/reviews/robotlocomotion/drake/3723#-KTpi45aeBHxOZJ7GSXU:-KTpi45aeBHxOZJ7GSXV:b-6kb9e) ([raw file](https://github.com/robotlocomotion/drake/blob/e417d2227aa1ad8a4aef726d4386f1544cd1f3d4/drake/math/quaternion.h#L300)):*\n> ```objc\n>   Vector3<Scalar> spaceXYZ_angles(theta3, theta2, theta1);\n> \n> #ifdef DRAKE_ASSERT_IS_ARMED\n> ```\n\nThis test needs a comment clarifying what it is doing and why 1e-11 is the right number.\n\n---\n\n*[drake/math/quaternion.h, line 315 at r3](https://reviewable.io:443/reviews/robotlocomotion/drake/3723#-KTpYXVwkM61PgiHs5ZP:-KTpYXVwkM61PgiHs5ZQ:b-ceo5ne) ([raw file](https://github.com/robotlocomotion/drake/blob/e417d2227aa1ad8a4aef726d4386f1544cd1f3d4/drake/math/quaternion.h#L315)):*\n> ```objc\n>  * @see rpy2rotmat\n>  * This accurate algorithm avoids numerical round-off issues encountered by some\n>  * algorithms when pitch is close to PI/2 or -PI/2  (pitch = PI/2 - 1E-6).\n> ```\n\nEven if you aren't ready yet to tell the compiler this is deprecated, I think the comment should say so. Please change the comment to something like:\n```c++\n/** (Deprecated) Computes SpaceXYZ Euler angles from quaternion.\nUse `QuaternionToSpaceXYZ()` instead.\n@see QuaternionToSpaceXYZ() **/\n```\n\n---\n\n*[drake/math/quaternion.h, line 321 at r3](https://reviewable.io:443/reviews/robotlocomotion/drake/3723#-KTpZTGiUxbkYEr4TaGN:-KTpZTGiUxbkYEr4TaGO:b-jvq9mg) ([raw file](https://github.com/robotlocomotion/drake/blob/e417d2227aa1ad8a4aef726d4386f1544cd1f3d4/drake/math/quaternion.h#L321)):*\n> ```objc\n>     const Eigen::MatrixBase<Derived>& quaternion) {\n>   // TODO(hongkai.dai@tri.global): Switch to Eigen's Quaternion when we fix\n>   // the range problem in Eigen\n> ```\n\nBTW, this TODO isn't needed assuming this method will be deprecated.\n\n---\n\n*[drake/math/roll_pitch_yaw.h, line 3 at r3](https://reviewable.io:443/reviews/robotlocomotion/drake/3723#-KTpiXJq4F9DZs22Azel:-KTpiXJq4F9DZs22Azem:bnn5c2q) ([raw file](https://github.com/robotlocomotion/drake/blob/e417d2227aa1ad8a4aef726d4386f1544cd1f3d4/drake/math/roll_pitch_yaw.h#L3)):*\n> ```objc\n> #pragma once\n> #include \"drake/math/roll_pitch_yaw_depend_quaternion.h\"\n> #include \"drake/math/roll_pitch_yaw_independent_quaternion.h\"\n> ```\n\nI believe this requires modifying a CMakeLists.txt file somewhere to make sure that all three of these include files get installed. I don't see that change.\n\nThis header files needs (brief) documentation explaining why it is chopped up the way it is. (Optional: In doxygen you can use `@file` to provide some file-level documentation that doxygen can present nicely.)\n\n---\n\n*[drake/math/roll_pitch_yaw_depend_quaternion.h, line 2 at r3](https://reviewable.io:443/reviews/robotlocomotion/drake/3723#-KTpAbVexaz4jB6QtoZU:-KTpjLQihL1hwEsf5Qrp:blye0lg) ([raw file](https://github.com/robotlocomotion/drake/blob/e417d2227aa1ad8a4aef726d4386f1544cd1f3d4/drake/math/roll_pitch_yaw_depend_quaternion.h#L2)):*\n<details><summary><i>Previously, ggould-tri wrote…</i></summary>\n> BTW, the name of this file is a little bit cryptic -- the grammatical role of \"depend\" is unclear.  Perhaps \"using\" would be clearer than \"depend\"?  In any event it should start with a file-level documentation comment just like the next file does explaining when it should be used.\n</details>\nI agree -- especially about the need for file-level documentation!\n\n---\n\n*[drake/math/roll_pitch_yaw_independent_quaternion.h, line 2 at r3](https://reviewable.io:443/reviews/robotlocomotion/drake/3723#-KTpjiBcYSlnisBsEAmD:-KTpjiBcYSlnisBsEAmE:b-q2st5q) ([raw file](https://github.com/robotlocomotion/drake/blob/e417d2227aa1ad8a4aef726d4386f1544cd1f3d4/drake/math/roll_pitch_yaw_independent_quaternion.h#L2)):*\n> ```objc\n> // This  file includes the conversion from a roll pitch yaw representation, that\n> // does not depend on quaternion.h\n> ```\n\nBTW, should say why this file exists and when it should be included. Should also mention that most users should include `roll_pitch_yaw.h` and ignore these half-files.\n\n---\n\n*[drake/math/roll_pitch_yaw_independent_quaternion.h, line 16 at r3](https://reviewable.io:443/reviews/robotlocomotion/drake/3723#-KTpk2-rgGlz7y8Yzd_M:-KTpk2-rgGlz7y8Yzd_N:bg5o6qd) ([raw file](https://github.com/robotlocomotion/drake/blob/e417d2227aa1ad8a4aef726d4386f1544cd1f3d4/drake/math/roll_pitch_yaw_independent_quaternion.h#L16)):*\n> ```objc\n>  * Computes the quaternion representation from Euler angles.\n>  * @param rpy A 3 x 1 vector. The Euler angles about Body-fixed z-y'-x'' axes\n>  * by angles [rpy(2), rpy(1), rpy(0)].\n> ```\n\nBetter to describe this as a space-fixed x,y,z sequence; seems somewhat contorted to describe it this way. (See rpy2rotmat() below.)\n\n---\n\n*[drake/math/roll_pitch_yaw_independent_quaternion.h, line 103 at r3](https://reviewable.io:443/reviews/robotlocomotion/drake/3723#-KTpknvhbu4cHwV_HFLD:-KTpknvhbu4cHwV_HFLE:b-c8piqw) ([raw file](https://github.com/robotlocomotion/drake/blob/e417d2227aa1ad8a4aef726d4386f1544cd1f3d4/drake/math/roll_pitch_yaw_independent_quaternion.h#L103)):*\n> ```objc\n> }\n> }\n> }\n> ```\n\nStyle guide requires namespace-closing comments formatted as: `// namespace math` etc.\n\n---\n\n*[drake/math/test/rotation_conversion_test.cc, line 84 at r3](https://reviewable.io:443/reviews/robotlocomotion/drake/3723#-KTpl5qwedwtKXq2r2Fc:-KTpl5qxGE_YgviC0cUe:b-hdxrov) ([raw file](https://github.com/robotlocomotion/drake/blob/e417d2227aa1ad8a4aef726d4386f1544cd1f3d4/drake/math/test/rotation_conversion_test.cc#L84)):*\n> ```cpp\n>   // matrix w.r.t Euler angle is very big.  To account for this sensitivity,\n>   // a new algorithm was implemented for calculating roll, pitch yaw angles,\n>   // the precision improves to 1E-13 (independent of pitch = +- PI/2).\n> ```\n\nBTW, I'm not sure this problem has anything to do with the derivative. Does it?\n\n---\n\n*[drake/math/test/rotation_conversion_test.cc, line 212 at r3](https://reviewable.io:443/reviews/robotlocomotion/drake/3723#-KTplKxOPU8HgZgaqTB1:-KTplKxPR9yjD4eLr3E7:b-xoklas) ([raw file](https://github.com/robotlocomotion/drake/blob/e417d2227aa1ad8a4aef726d4386f1544cd1f3d4/drake/math/test/rotation_conversion_test.cc#L212)):*\n> ```cpp\n>     // pitch = 0.5*pi - 1E-15\n>     rpy_test_cases_.push_back(\n>         Vector3d(M_PI * 0.8, 0.5 * M_PI - 1E-15, 0.9 * M_PI));\n> ```\n\nBTW, wow -- that's amazingly good!\n\n---\n\n\n*Comments from [Reviewable](https://reviewable.io:443/reviews/robotlocomotion/drake/3723#-:-KTplpAafH_I7rrVhzWI:b-fb18gm)*\n<!-- Sent from Reviewable.io -->\n"}}
{"actor":{"display_login":"sherm1"}}