{"action":"created","issue":{"url":"https://api.github.com/repos/dartsim/dart/issues/333","labels_url":"https://api.github.com/repos/dartsim/dart/issues/333/labels{/name}","comments_url":"https://api.github.com/repos/dartsim/dart/issues/333/comments","events_url":"https://api.github.com/repos/dartsim/dart/issues/333/events","html_url":"https://github.com/dartsim/dart/issues/333","id":57670166,"number":333,"title":"Bug in FreeJoint helper","user":{"login":"ana-GT","id":555379,"avatar_url":"https://avatars.githubusercontent.com/u/555379?v=3","gravatar_id":"","url":"https://api.github.com/users/ana-GT","html_url":"https://github.com/ana-GT","followers_url":"https://api.github.com/users/ana-GT/followers","following_url":"https://api.github.com/users/ana-GT/following{/other_user}","gists_url":"https://api.github.com/users/ana-GT/gists{/gist_id}","starred_url":"https://api.github.com/users/ana-GT/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/ana-GT/subscriptions","organizations_url":"https://api.github.com/users/ana-GT/orgs","repos_url":"https://api.github.com/users/ana-GT/repos","events_url":"https://api.github.com/users/ana-GT/events{/privacy}","received_events_url":"https://api.github.com/users/ana-GT/received_events","type":"User","site_admin":false},"labels":[{"url":"https://api.github.com/repos/dartsim/dart/labels/Kind:%20Bug","name":"Kind: Bug","color":"009800"},{"url":"https://api.github.com/repos/dartsim/dart/labels/Priority:%20High","name":"Priority: High","color":"0052cc"}],"state":"open","locked":false,"assignee":{"login":"mxgrey","id":1307217,"avatar_url":"https://avatars.githubusercontent.com/u/1307217?v=3","gravatar_id":"","url":"https://api.github.com/users/mxgrey","html_url":"https://github.com/mxgrey","followers_url":"https://api.github.com/users/mxgrey/followers","following_url":"https://api.github.com/users/mxgrey/following{/other_user}","gists_url":"https://api.github.com/users/mxgrey/gists{/gist_id}","starred_url":"https://api.github.com/users/mxgrey/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/mxgrey/subscriptions","organizations_url":"https://api.github.com/users/mxgrey/orgs","repos_url":"https://api.github.com/users/mxgrey/repos","events_url":"https://api.github.com/users/mxgrey/events{/privacy}","received_events_url":"https://api.github.com/users/mxgrey/received_events","type":"User","site_admin":false},"milestone":null,"comments":7,"created_at":"2015-02-13T23:56:35Z","updated_at":"2015-02-14T08:51:56Z","closed_at":null,"body":"* File: dart/dynamics/FreeJoint.cpp\r\n* Functions compromised: convertToPositions (line 61) and converToTransform (line 70)\r\n* Problem description: I am generating a set of transformations with rotations around Z with angles in the range  [0,2*PI], increasing at each step. I use these Transformations to set the joint positions of a freeJoint (using the same formula as used in convertToPositions). \r\n\r\nThe joint positions I get are wrong for some angles. The joint positions correspond to the intended Transformation rotated 90 around Z. This happens usually after half of the range (either if I use [0,2PI] or [-PI,PI]. An example in which the bug happens is copied at the end of this issue. In this example I use:\r\n\r\nInput transform:\r\n\r\n-0.815214  -0.57916         0  0.437796\r\n -0.57916  0.815214         0  0.488175\r\n        0         0        -1      1.03\r\n        0         0         0         1\r\n\r\nPositions obtained using convertToPositions():\r\n-0.954925  -2.99295        -0  0.437796  0.488175      1.03\r\n\r\nOutput transform obtained using convertToTransform() (should be the same as input transform):\r\n   -0.815214      0.57916 -5.39747e-16     0.437796\r\n     0.57916     0.815214  1.72211e-16     0.488175\r\n 5.39747e-16 -1.72211e-16           -1         1.03\r\n6.95317e-310 1.38338e-322 6.92085e-310 2.12999e-317\r\n\r\nNotice the flipped signs in x an y columns.\r\n\r\n\r\nThe problem described above is related to convertToPositions. I also noticed that in convertToTransform, there is a line missing: Tf.setIdentity(). Otherwise, the last row of Tf will be filled with garbage (as in the example above).\r\n\r\n* Final note: Please, if somebody can fix this quickly I would appreciate it. I spent most of today searching for the bug in  my own code until I realized that the bug was (very possibly and unless I am mistaken) in dart so I would like to continue with my work once the bug is fixed. Thanks!\r\n\r\n\r\nPS.- Bug test file:\r\n\r\n#include <dart/math/Geometry.h>\r\n#include <iostream>\r\n\r\nEigen::Vector6d convertToPositions(const Eigen::Isometry3d& _tf);\r\nEigen::Isometry3d convertToTransform(const Eigen::Vector6d& _positions);\r\n\r\nint main( int argc, char* argv[] ) {\r\n\r\n  Eigen::Isometry3d Tf;\r\n  Eigen::VectorXd q;\r\n  Eigen::Isometry3d Tf_back;\r\n\r\n  Tf.setIdentity();\r\n  Tf.translation() << 0.1, 0.2, 0.3;\r\n\r\n  double dang = 10*M_PI/180.0;\r\n  int n = (int) (2*M_PI /dang);\r\n\r\n  Tf.matrix() << -0.815214,  -0.57916,        -0,  0.437796,\r\n \t-0.57916,  0.815214,         0,  0.488175,\r\n        0,         0,        -1,      1.03,\r\n        0,         0,         0,         1;\r\n\r\n   double det = Tf.linear().determinant();\r\n  std::cout << \"Determinant of test TF: \"<< det << std::endl;\r\n\r\n/*\r\n  for( size_t i = 0; i < n; ++i ) {\r\n    Eigen::Matrix3d rot;\r\n    rot = Eigen::AngleAxisd( i*dang, Eigen::Vector3d(0,0,1) );\r\n    Tf.linear() = rot;\r\n*/\r\n    // Get position with helper function\r\n    q = convertToPositions( Tf );  \r\n\r\n    // Get translation back with helper function\r\n    Tf_back = convertToTransform( q );\r\n    //std::cout << \"i: \"<< i << std::endl;\r\n    std::cout << \"\\n ** Tf original: \\n\"<< Tf.matrix() << std::endl;\r\n    std::cout << \"\\n ** Positions from Tf: \\n\"<< q.transpose() << std::endl;\r\n    std::cout << \"\\n ** Tf from q: \\n\" << Tf_back.matrix() << std::endl;\r\n    std::cout << \"\\n\\n\" << std::endl;\r\n\r\n/*\r\n  }\r\n*/\r\n\r\n}\r\n\r\n\r\n//==============================================================================\r\nEigen::Vector6d convertToPositions(const Eigen::Isometry3d& _tf)\r\n{\r\n  Eigen::Vector6d x;\r\n  x.head<3>() = dart::math::logMap(_tf.linear());\r\n  x.tail<3>() = _tf.translation();\r\n  return x;\r\n}\r\n\r\n//==============================================================================\r\nEigen::Isometry3d convertToTransform(\r\n    const Eigen::Vector6d& _positions)\r\n{\r\n  Eigen::Isometry3d tf;\r\n  tf.linear() = dart::math::expMapRot(_positions.head<3>());\r\n  tf.translation() = _positions.tail<3>();\r\n  return tf;\r\n}\r\n\r\n\r\n"},"comment":{"url":"https://api.github.com/repos/dartsim/dart/issues/comments/74367555","html_url":"https://github.com/dartsim/dart/issues/333#issuecomment-74367555","issue_url":"https://api.github.com/repos/dartsim/dart/issues/333","id":74367555,"user":{"login":"mxgrey","id":1307217,"avatar_url":"https://avatars.githubusercontent.com/u/1307217?v=3","gravatar_id":"","url":"https://api.github.com/users/mxgrey","html_url":"https://github.com/mxgrey","followers_url":"https://api.github.com/users/mxgrey/followers","following_url":"https://api.github.com/users/mxgrey/following{/other_user}","gists_url":"https://api.github.com/users/mxgrey/gists{/gist_id}","starred_url":"https://api.github.com/users/mxgrey/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/mxgrey/subscriptions","organizations_url":"https://api.github.com/users/mxgrey/orgs","repos_url":"https://api.github.com/users/mxgrey/repos","events_url":"https://api.github.com/users/mxgrey/events{/privacy}","received_events_url":"https://api.github.com/users/mxgrey/received_events","type":"User","site_admin":false},"created_at":"2015-02-14T08:51:55Z","updated_at":"2015-02-14T08:51:55Z","body":"@ana-GT I have a working solution for the problem you've encountered, but I'd like to put together a formal regression test for it.\r\n\r\nIn my attempts to put together a regression test, I have not been able to reproduce the error that you experienced by using formally generated rotation matrices (in other words, using tools like Eigen::AngleAxis to create a well-formed rotation matrix). This makes me suspect that the issue only occurs if a hard-coded (and therefore poorly conditioned) rotation matrix is used somewhere in the mix. Have you encountered a case where this issue occurs when you're not using a hard-coded matrix? If so, I'd like to write a test around it.\r\n\r\nIf not, the solution that I've come up with is going to be more robust to hard-coded and imprecise matrices, while also being faster. Of course the real credit for it goes to Eigen, since I'm just exploiting some built-in Eigen features."}}