{"action":"created","comment":{"url":"https://api.github.com/repos/RobotLocomotion/drake/pulls/comments/38553124","id":38553124,"diff_hunk":"@@ -96,48 +153,241 @@ Eigen::Quaternion<typename Derived::Scalar> quat2eigenQuaternion(const Eigen::Ma\n  * axis2x\n  */\n template <typename Derived>\n-DLLEXPORT Eigen::Vector4d axis2quat(const Eigen::MatrixBase<Derived>& a);\n+Eigen::Vector4d axis2quat(const Eigen::MatrixBase<Derived>& a) {\n+  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 4);\n+  auto axis = a.template head<3>();\n+  auto angle = a(3);\n+  auto arg = 0.5 * angle;\n+  auto c = std::cos(arg);\n+  auto s = std::sin(arg);\n+  Eigen::Vector4d ret;\n+  ret << c, s * axis;\n+  return ret;\n+}\n \n template <typename Derived>\n-DLLEXPORT Eigen::Matrix<typename Derived::Scalar, 3, 3> axis2rotmat(const Eigen::MatrixBase<Derived>& a);\n+Eigen::Matrix<typename Derived::Scalar, 3, 3> axis2rotmat(const Eigen::MatrixBase<Derived>& a) {\n+  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 4);\n+  const auto& axis = (a.template head<3>())/(a.template head<3>()).norm();\n+  const auto& theta = a(3);\n+  auto x = axis(0);\n+  auto y = axis(1);\n+  auto z = axis(2);\n+  auto ctheta = std::cos(theta);\n+  auto stheta = std::sin(theta);\n+  auto c = 1 - ctheta;\n+  Eigen::Matrix<typename Derived::Scalar, 3, 3> R;\n+  R <<\n+  ctheta + x * x * c , x * y * c - z * stheta, x * z * c + y * stheta,\n+      y * x * c + z * stheta, ctheta + y * y * c, y * z * c - x * stheta,\n+      z * x * c - y * stheta, z * y * c + x * stheta, ctheta + z * z * c;\n+\n+  return R;\n+};\n \n template <typename Derived>\n-DLLEXPORT Eigen::Matrix<typename Derived::Scalar, 3, 1> axis2rpy(const Eigen::MatrixBase<Derived>& a);\n+Eigen::Matrix<typename Derived::Scalar, 3, 1> axis2rpy(const Eigen::MatrixBase<Derived>& a) {\n+  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 4);\n+  return quat2rpy(axis2quat(a));\n+};\n \n /*\n  * expmap2x\n  */\n template <typename Derived>\n-DLLEXPORT GradientVar<typename Derived::Scalar, QUAT_SIZE, 1> expmap2quat(const Eigen::MatrixBase<Derived>& q, const int gradient_ordr);\n+GradientVar<typename Derived::Scalar, QUAT_SIZE, 1> expmap2quat(const Eigen::MatrixBase<Derived>& v, const int gradient_order) {\n+  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 3);\n+  GradientVar<typename Derived::Scalar, QUAT_SIZE, 1> ret(QUAT_SIZE, 1, EXPMAP_SIZE,gradient_order);\n+  auto theta = v.norm();\n+  if (theta < pow(std::numeric_limits<typename Derived::Scalar>::epsilon(),0.25)) {\n+    ret.value() = expmap2quatDegenerate(v, theta);\n+    if(gradient_order>0)\n+    {\n+      ret.gradient().value() = dexpmap2quatDegenerate(v, theta);\n+      if(gradient_order>1)\n+      {\n+        ret.gradient().gradient().value() = ddexpmap2quatDegenerate(v, theta);\n+        if(gradient_order>2)\n+        {\n+          throw std::runtime_error(\"expmap2quat does not support gradient order larger than 2\");","path":"drake/util/drakeGeometryUtil.h","position":166,"original_position":166,"commit_id":"9a87fd3afc8b7635418b0597a981f40bf70d685c","original_commit_id":"9a87fd3afc8b7635418b0597a981f40bf70d685c","user":{"login":"tkoolen","id":2585880,"avatar_url":"https://avatars.githubusercontent.com/u/2585880?v=3","gravatar_id":"","url":"https://api.github.com/users/tkoolen","html_url":"https://github.com/tkoolen","followers_url":"https://api.github.com/users/tkoolen/followers","following_url":"https://api.github.com/users/tkoolen/following{/other_user}","gists_url":"https://api.github.com/users/tkoolen/gists{/gist_id}","starred_url":"https://api.github.com/users/tkoolen/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/tkoolen/subscriptions","organizations_url":"https://api.github.com/users/tkoolen/orgs","repos_url":"https://api.github.com/users/tkoolen/repos","events_url":"https://api.github.com/users/tkoolen/events{/privacy}","received_events_url":"https://api.github.com/users/tkoolen/received_events","type":"User","site_admin":false},"body":"Just FYI, I didn't write this code, just moved it from the source file to the header.\r\nI think you're right. However, if we're going to be getting rid of GradientVar and our user gradients completely and relying totally on AutoDiffScalar, this code will be removed in the process.\r\nI can change it for now.","created_at":"2015-09-02T16:25:10Z","updated_at":"2015-09-02T16:25:10Z","html_url":"https://github.com/RobotLocomotion/drake/pull/1304#discussion_r38553124","pull_request_url":"https://api.github.com/repos/RobotLocomotion/drake/pulls/1304","_links":{"self":{"href":"https://api.github.com/repos/RobotLocomotion/drake/pulls/comments/38553124"},"html":{"href":"https://github.com/RobotLocomotion/drake/pull/1304#discussion_r38553124"},"pull_request":{"href":"https://api.github.com/repos/RobotLocomotion/drake/pulls/1304"}}},"pull_request":{"url":"https://api.github.com/repos/RobotLocomotion/drake/pulls/1304","id":43721079,"html_url":"https://github.com/RobotLocomotion/drake/pull/1304","diff_url":"https://github.com/RobotLocomotion/drake/pull/1304.diff","patch_url":"https://github.com/RobotLocomotion/drake/pull/1304.patch","issue_url":"https://api.github.com/repos/RobotLocomotion/drake/issues/1304","number":1304,"state":"open","locked":false,"title":"KinematicsCache object, updated","user":{"login":"tkoolen","id":2585880,"avatar_url":"https://avatars.githubusercontent.com/u/2585880?v=3","gravatar_id":"","url":"https://api.github.com/users/tkoolen","html_url":"https://github.com/tkoolen","followers_url":"https://api.github.com/users/tkoolen/followers","following_url":"https://api.github.com/users/tkoolen/following{/other_user}","gists_url":"https://api.github.com/users/tkoolen/gists{/gist_id}","starred_url":"https://api.github.com/users/tkoolen/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/tkoolen/subscriptions","organizations_url":"https://api.github.com/users/tkoolen/orgs","repos_url":"https://api.github.com/users/tkoolen/repos","events_url":"https://api.github.com/users/tkoolen/events{/privacy}","received_events_url":"https://api.github.com/users/tkoolen/received_events","type":"User","site_admin":false},"body":"See https://github.com/RobotLocomotion/drake/pull/1286 (I couldn't reopen because I force pushed after rebasing).\r\n\r\nProfling showed that creating new DrakeMexPointers every time you call doKinematics is too expensive. I'm now creating two default KinematicsCache DrakeMexPointers (one with gradients, one without) upon creation of a RigidBodyManipulator in Matlab. These will get reused by doKinematics unless you set options.kinematics_cache_ptr_to_use to another KinematicsCache DrakeMexPointer. I added a mex function to explicitly create a new KinematicsCache.\r\n\r\nIn addition, I tried to reduce the need for malloc a bit by making the dynamic size members of KinematicsCacheElement have a fixed *maximum* size. This means that these matrices will get allocated on the stack, yet it's possible to resize them up to their maximum size.\r\n\r\nFixes #1167.\r\nFixes #585 by just getting rid of the Matlab code + test.","created_at":"2015-08-30T21:40:50Z","updated_at":"2015-09-02T16:25:10Z","closed_at":null,"merged_at":null,"merge_commit_sha":"2237d2b271b199b1a14c1c75afbc1e47f995f969","assignee":null,"milestone":null,"commits_url":"https://api.github.com/repos/RobotLocomotion/drake/pulls/1304/commits","review_comments_url":"https://api.github.com/repos/RobotLocomotion/drake/pulls/1304/comments","review_comment_url":"https://api.github.com/repos/RobotLocomotion/drake/pulls/comments{/number}","comments_url":"https://api.github.com/repos/RobotLocomotion/drake/issues/1304/comments","statuses_url":"https://api.github.com/repos/RobotLocomotion/drake/statuses/9a87fd3afc8b7635418b0597a981f40bf70d685c","head":{"label":"tkoolen:tk-kinematics-cache","ref":"tk-kinematics-cache","sha":"9a87fd3afc8b7635418b0597a981f40bf70d685c","user":{"login":"tkoolen","id":2585880,"avatar_url":"https://avatars.githubusercontent.com/u/2585880?v=3","gravatar_id":"","url":"https://api.github.com/users/tkoolen","html_url":"https://github.com/tkoolen","followers_url":"https://api.github.com/users/tkoolen/followers","following_url":"https://api.github.com/users/tkoolen/following{/other_user}","gists_url":"https://api.github.com/users/tkoolen/gists{/gist_id}","starred_url":"https://api.github.com/users/tkoolen/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/tkoolen/subscriptions","organizations_url":"https://api.github.com/users/tkoolen/orgs","repos_url":"https://api.github.com/users/tkoolen/repos","events_url":"https://api.github.com/users/tkoolen/events{/privacy}","received_events_url":"https://api.github.com/users/tkoolen/received_events","type":"User","site_admin":false},"repo":{"id":18049528,"name":"drake","full_name":"tkoolen/drake","owner":{"login":"tkoolen","id":2585880,"avatar_url":"https://avatars.githubusercontent.com/u/2585880?v=3","gravatar_id":"","url":"https://api.github.com/users/tkoolen","html_url":"https://github.com/tkoolen","followers_url":"https://api.github.com/users/tkoolen/followers","following_url":"https://api.github.com/users/tkoolen/following{/other_user}","gists_url":"https://api.github.com/users/tkoolen/gists{/gist_id}","starred_url":"https://api.github.com/users/tkoolen/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/tkoolen/subscriptions","organizations_url":"https://api.github.com/users/tkoolen/orgs","repos_url":"https://api.github.com/users/tkoolen/repos","events_url":"https://api.github.com/users/tkoolen/events{/privacy}","received_events_url":"https://api.github.com/users/tkoolen/received_events","type":"User","site_admin":false},"private":false,"html_url":"https://github.com/tkoolen/drake","description":"A planning, control, and analysis toolbox for nonlinear dynamical systems.  Please see the WIKI for documentation.","fork":true,"url":"https://api.github.com/repos/tkoolen/drake","forks_url":"https://api.github.com/repos/tkoolen/drake/forks","keys_url":"https://api.github.com/repos/tkoolen/drake/keys{/key_id}","collaborators_url":"https://api.github.com/repos/tkoolen/drake/collaborators{/collaborator}","teams_url":"https://api.github.com/repos/tkoolen/drake/teams","hooks_url":"https://api.github.com/repos/tkoolen/drake/hooks","issue_events_url":"https://api.github.com/repos/tkoolen/drake/issues/events{/number}","events_url":"https://api.github.com/repos/tkoolen/drake/events","assignees_url":"https://api.github.com/repos/tkoolen/drake/assignees{/user}","branches_url":"https://api.github.com/repos/tkoolen/drake/branches{/branch}","tags_url":"https://api.github.com/repos/tkoolen/drake/tags","blobs_url":"https://api.github.com/repos/tkoolen/drake/git/blobs{/sha}","git_tags_url":"https://api.github.com/repos/tkoolen/drake/git/tags{/sha}","git_refs_url":"https://api.github.com/repos/tkoolen/drake/git/refs{/sha}","trees_url":"https://api.github.com/repos/tkoolen/drake/git/trees{/sha}","statuses_url":"https://api.github.com/repos/tkoolen/drake/statuses/{sha}","languages_url":"https://api.github.com/repos/tkoolen/drake/languages","stargazers_url":"https://api.github.com/repos/tkoolen/drake/stargazers","contributors_url":"https://api.github.com/repos/tkoolen/drake/contributors","subscribers_url":"https://api.github.com/repos/tkoolen/drake/subscribers","subscription_url":"https://api.github.com/repos/tkoolen/drake/subscription","commits_url":"https://api.github.com/repos/tkoolen/drake/commits{/sha}","git_commits_url":"https://api.github.com/repos/tkoolen/drake/git/commits{/sha}","comments_url":"https://api.github.com/repos/tkoolen/drake/comments{/number}","issue_comment_url":"https://api.github.com/repos/tkoolen/drake/issues/comments{/number}","contents_url":"https://api.github.com/repos/tkoolen/drake/contents/{+path}","compare_url":"https://api.github.com/repos/tkoolen/drake/compare/{base}...{head}","merges_url":"https://api.github.com/repos/tkoolen/drake/merges","archive_url":"https://api.github.com/repos/tkoolen/drake/{archive_format}{/ref}","downloads_url":"https://api.github.com/repos/tkoolen/drake/downloads","issues_url":"https://api.github.com/repos/tkoolen/drake/issues{/number}","pulls_url":"https://api.github.com/repos/tkoolen/drake/pulls{/number}","milestones_url":"https://api.github.com/repos/tkoolen/drake/milestones{/number}","notifications_url":"https://api.github.com/repos/tkoolen/drake/notifications{?since,all,participating}","labels_url":"https://api.github.com/repos/tkoolen/drake/labels{/name}","releases_url":"https://api.github.com/repos/tkoolen/drake/releases{/id}","created_at":"2014-03-24T02:31:26Z","updated_at":"2015-08-26T05:27:33Z","pushed_at":"2015-09-02T04:01:19Z","git_url":"git://github.com/tkoolen/drake.git","ssh_url":"git@github.com:tkoolen/drake.git","clone_url":"https://github.com/tkoolen/drake.git","svn_url":"https://github.com/tkoolen/drake","homepage":"https://github.com/RobotLocomotion/drake/wiki","size":188549,"stargazers_count":0,"watchers_count":0,"language":"Matlab","has_issues":false,"has_downloads":true,"has_wiki":true,"has_pages":false,"forks_count":0,"mirror_url":null,"open_issues_count":0,"forks":0,"open_issues":0,"watchers":0,"default_branch":"body-wrench-in-qp"}},"base":{"label":"RobotLocomotion:master","ref":"master","sha":"89275a515f347f80232f16d316a56f6773ea74eb","user":{"login":"RobotLocomotion","id":6496345,"avatar_url":"https://avatars.githubusercontent.com/u/6496345?v=3","gravatar_id":"","url":"https://api.github.com/users/RobotLocomotion","html_url":"https://github.com/RobotLocomotion","followers_url":"https://api.github.com/users/RobotLocomotion/followers","following_url":"https://api.github.com/users/RobotLocomotion/following{/other_user}","gists_url":"https://api.github.com/users/RobotLocomotion/gists{/gist_id}","starred_url":"https://api.github.com/users/RobotLocomotion/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/RobotLocomotion/subscriptions","organizations_url":"https://api.github.com/users/RobotLocomotion/orgs","repos_url":"https://api.github.com/users/RobotLocomotion/repos","events_url":"https://api.github.com/users/RobotLocomotion/events{/privacy}","received_events_url":"https://api.github.com/users/RobotLocomotion/received_events","type":"Organization","site_admin":false},"repo":{"id":16256144,"name":"drake","full_name":"RobotLocomotion/drake","owner":{"login":"RobotLocomotion","id":6496345,"avatar_url":"https://avatars.githubusercontent.com/u/6496345?v=3","gravatar_id":"","url":"https://api.github.com/users/RobotLocomotion","html_url":"https://github.com/RobotLocomotion","followers_url":"https://api.github.com/users/RobotLocomotion/followers","following_url":"https://api.github.com/users/RobotLocomotion/following{/other_user}","gists_url":"https://api.github.com/users/RobotLocomotion/gists{/gist_id}","starred_url":"https://api.github.com/users/RobotLocomotion/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/RobotLocomotion/subscriptions","organizations_url":"https://api.github.com/users/RobotLocomotion/orgs","repos_url":"https://api.github.com/users/RobotLocomotion/repos","events_url":"https://api.github.com/users/RobotLocomotion/events{/privacy}","received_events_url":"https://api.github.com/users/RobotLocomotion/received_events","type":"Organization","site_admin":false},"private":false,"html_url":"https://github.com/RobotLocomotion/drake","description":"A planning, control, and analysis toolbox for nonlinear dynamical systems.  Please see the WIKI for documentation.","fork":false,"url":"https://api.github.com/repos/RobotLocomotion/drake","forks_url":"https://api.github.com/repos/RobotLocomotion/drake/forks","keys_url":"https://api.github.com/repos/RobotLocomotion/drake/keys{/key_id}","collaborators_url":"https://api.github.com/repos/RobotLocomotion/drake/collaborators{/collaborator}","teams_url":"https://api.github.com/repos/RobotLocomotion/drake/teams","hooks_url":"https://api.github.com/repos/RobotLocomotion/drake/hooks","issue_events_url":"https://api.github.com/repos/RobotLocomotion/drake/issues/events{/number}","events_url":"https://api.github.com/repos/RobotLocomotion/drake/events","assignees_url":"https://api.github.com/repos/RobotLocomotion/drake/assignees{/user}","branches_url":"https://api.github.com/repos/RobotLocomotion/drake/branches{/branch}","tags_url":"https://api.github.com/repos/RobotLocomotion/drake/tags","blobs_url":"https://api.github.com/repos/RobotLocomotion/drake/git/blobs{/sha}","git_tags_url":"https://api.github.com/repos/RobotLocomotion/drake/git/tags{/sha}","git_refs_url":"https://api.github.com/repos/RobotLocomotion/drake/git/refs{/sha}","trees_url":"https://api.github.com/repos/RobotLocomotion/drake/git/trees{/sha}","statuses_url":"https://api.github.com/repos/RobotLocomotion/drake/statuses/{sha}","languages_url":"https://api.github.com/repos/RobotLocomotion/drake/languages","stargazers_url":"https://api.github.com/repos/RobotLocomotion/drake/stargazers","contributors_url":"https://api.github.com/repos/RobotLocomotion/drake/contributors","subscribers_url":"https://api.github.com/repos/RobotLocomotion/drake/subscribers","subscription_url":"https://api.github.com/repos/RobotLocomotion/drake/subscription","commits_url":"https://api.github.com/repos/RobotLocomotion/drake/commits{/sha}","git_commits_url":"https://api.github.com/repos/RobotLocomotion/drake/git/commits{/sha}","comments_url":"https://api.github.com/repos/RobotLocomotion/drake/comments{/number}","issue_comment_url":"https://api.github.com/repos/RobotLocomotion/drake/issues/comments{/number}","contents_url":"https://api.github.com/repos/RobotLocomotion/drake/contents/{+path}","compare_url":"https://api.github.com/repos/RobotLocomotion/drake/compare/{base}...{head}","merges_url":"https://api.github.com/repos/RobotLocomotion/drake/merges","archive_url":"https://api.github.com/repos/RobotLocomotion/drake/{archive_format}{/ref}","downloads_url":"https://api.github.com/repos/RobotLocomotion/drake/downloads","issues_url":"https://api.github.com/repos/RobotLocomotion/drake/issues{/number}","pulls_url":"https://api.github.com/repos/RobotLocomotion/drake/pulls{/number}","milestones_url":"https://api.github.com/repos/RobotLocomotion/drake/milestones{/number}","notifications_url":"https://api.github.com/repos/RobotLocomotion/drake/notifications{?since,all,participating}","labels_url":"https://api.github.com/repos/RobotLocomotion/drake/labels{/name}","releases_url":"https://api.github.com/repos/RobotLocomotion/drake/releases{/id}","created_at":"2014-01-26T16:11:05Z","updated_at":"2015-08-31T09:32:54Z","pushed_at":"2015-09-02T15:55:24Z","git_url":"git://github.com/RobotLocomotion/drake.git","ssh_url":"git@github.com:RobotLocomotion/drake.git","clone_url":"https://github.com/RobotLocomotion/drake.git","svn_url":"https://github.com/RobotLocomotion/drake","homepage":"https://github.com/RobotLocomotion/drake/wiki","size":206892,"stargazers_count":147,"watchers_count":147,"language":"Matlab","has_issues":true,"has_downloads":true,"has_wiki":true,"has_pages":false,"forks_count":126,"mirror_url":null,"open_issues_count":126,"forks":126,"open_issues":126,"watchers":147,"default_branch":"master"}},"_links":{"self":{"href":"https://api.github.com/repos/RobotLocomotion/drake/pulls/1304"},"html":{"href":"https://github.com/RobotLocomotion/drake/pull/1304"},"issue":{"href":"https://api.github.com/repos/RobotLocomotion/drake/issues/1304"},"comments":{"href":"https://api.github.com/repos/RobotLocomotion/drake/issues/1304/comments"},"review_comments":{"href":"https://api.github.com/repos/RobotLocomotion/drake/pulls/1304/comments"},"review_comment":{"href":"https://api.github.com/repos/RobotLocomotion/drake/pulls/comments{/number}"},"commits":{"href":"https://api.github.com/repos/RobotLocomotion/drake/pulls/1304/commits"},"statuses":{"href":"https://api.github.com/repos/RobotLocomotion/drake/statuses/9a87fd3afc8b7635418b0597a981f40bf70d685c"}}}}