{"action":"created","comment":{"url":"https://api.github.com/repos/ros-industrial-consortium/godel/pulls/comments/25025643","id":25025643,"diff_hunk":"@@ -0,0 +1,188 @@\n+#include \"godel_path_planning/trajectory_planning.h\"\n+\n+#include <moveit/robot_trajectory/robot_trajectory.h>\n+#include <moveit/robot_model_loader/robot_model_loader.h>\n+\n+#include \"descartes_moveit/moveit_state_adapter.h\"\n+#include \"descartes_trajectory/cart_trajectory_pt.h\"\n+#include \"descartes_planner/dense_planner.h\"\n+\n+#include <tf_conversions/tf_eigen.h>\n+\n+\n+// Anonymous namespace\n+namespace\n+{\n+  const static double TOOL_POINT_DELAY = 0.75;\n+\n+  // Create a descartes RobotModel for graph planning\n+  descartes_core::RobotModelPtr createRobotModel(const moveit::core::RobotStatePtr robot_state,\n+                                                 const std::string& group_name,\n+                                                 const std::string& tool_frame,\n+                                                 const std::string& world_frame,\n+                                                 const uint8_t iterations)\n+  {\n+    using descartes_core::RobotModelPtr;\n+    using descartes_moveit::MoveitStateAdapter;\n+\n+    return RobotModelPtr(new MoveitStateAdapter(*robot_state, group_name, tool_frame, world_frame, iterations)); \n+  }\n+\n+  // Create a CartTrajectoryPt that defines a point & axis with free rotation about z\n+  descartes_core::TrajectoryPtPtr pointAxisTrajectoryFactory(double x, double y, double z, \n+                                                             double rx, double ry, double rz,\n+                                                             double angle_discretization)\n+  {\n+    using namespace descartes_core;\n+    using namespace descartes_trajectory;\n+\n+    return boost::shared_ptr<TrajectoryPt>(\n+      new CartTrajectoryPt(\n+            TolerancedFrame(utils::toFrame(x,y, z, rx, ry, rz, descartes_core::utils::EulerConventions::XYZ),\n+             ToleranceBase::zeroTolerance<PositionTolerance>(x, y, z),\n+             ToleranceBase::createSymmetric<OrientationTolerance>(rx, ry, 0, 0, 0, 2.0 * M_PI)),\n+            0.0, angle_discretization));\n+  }\n+\n+  // Create an axial trajectory pt from a given tf transform\n+  descartes_core::TrajectoryPtPtr tfToAxialTrajectoryPt(const tf::Transform& nominal, double discretization)\n+  {\n+    Eigen::Affine3d eigen_pose;\n+    tf::poseTFToEigen(nominal,eigen_pose);\n+    Eigen::Vector3d rpy =  eigen_pose.rotation().eulerAngles(0,1,2);\n+\n+    double rx = rpy(0);\n+    double ry = rpy(1);\n+    double rz = rpy(2);\n+    double x = eigen_pose.translation()(0);\n+    double y = eigen_pose.translation()(1);\n+    double z = eigen_pose.translation()(2);\n+\n+    return pointAxisTrajectoryFactory(x, y, z , rx, ry, rz, discretization);\n+  }\n+\n+  // Translates a point relative to a reference pose to an absolute transformation\n+  // Also flips the z axis of the orientation to point INTO the plane specified by the\n+  // reference frame.\n+  tf::Transform createNominalTransform(const geometry_msgs::Pose& ref_pose, const geometry_msgs::Point& point)\n+  {\n+    // To plane\n+    tf::Transform marker_pose;\n+    tf::poseMsgToTF(ref_pose, marker_pose);\n+    // From plane to point\n+    tf::Transform to_point = tf::Transform::getIdentity();\n+    to_point.setOrigin(tf::Vector3(point.x, point.y, point.z));\n+    // Reverse orientation of z axis\n+    tf::Quaternion quat;\n+    quat.setEuler(0.0, 3.14159, 0.0); // yaw, pitch roll\n+    tf::Transform flip_z (quat);\n+    // Calculate transform\n+    tf::Transform in_world = marker_pose * to_point * flip_z;\n+\n+    return in_world;\n+  }\n+\n+  bool populateTrajectoryMsg(const std::vector<descartes_core::TrajectoryPtPtr>& solution,\n+                             const descartes_core::RobotModel& robot_model,\n+                             double interpoint_delay,\n+                             double time_offset,\n+                             trajectory_msgs::JointTrajectory& trajectory)\n+  {\n+    typedef std::vector<descartes_core::TrajectoryPtPtr>::const_iterator JointSolutionIterator;\n+    \n+    // For calculating the time_from_start field of the trajectoryPoint\n+    ros::Duration time_from_start(time_offset);\n+\n+    for (JointSolutionIterator it = solution.begin(); it != solution.end(); ++it)\n+    {\n+      // Retrieve actual target joint angles from the polymorphic interface function\n+      std::vector<std::vector<double> > joint_angles;\n+      it->get()->getJointPoses(robot_model, joint_angles);\n+      const std::vector<double>& sol = joint_angles[0];\n+      \n+      trajectory_msgs::JointTrajectoryPoint point;\n+      point.positions = sol;\n+      point.velocities.resize(sol.size(), 0.0); // Fill extra fields with zeroes for now\n+      point.accelerations.resize(sol.size(), 0.0);\n+      point.effort.resize(sol.size(), 0.0);\n+      point.time_from_start = time_from_start;\n+\n+      // add trajectory point to array\n+      trajectory.points.push_back(point);\n+\n+      // increment time so far by next duration\n+      time_from_start += ros::Duration(interpoint_delay);\n+    }\n+\n+    return true;","path":"godel_path_planning/src/trajectory_planning.cpp","position":117,"original_position":117,"commit_id":"88c9fb78c68fd75b33307733efae8fde76fbd870","original_commit_id":"24b9a915755daeebc31a8ac77a99550f1fdd62c4","user":{"login":"Jmeyer1292","id":4960180,"avatar_url":"https://avatars.githubusercontent.com/u/4960180?v=3","gravatar_id":"","url":"https://api.github.com/users/Jmeyer1292","html_url":"https://github.com/Jmeyer1292","followers_url":"https://api.github.com/users/Jmeyer1292/followers","following_url":"https://api.github.com/users/Jmeyer1292/following{/other_user}","gists_url":"https://api.github.com/users/Jmeyer1292/gists{/gist_id}","starred_url":"https://api.github.com/users/Jmeyer1292/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/Jmeyer1292/subscriptions","organizations_url":"https://api.github.com/users/Jmeyer1292/orgs","repos_url":"https://api.github.com/users/Jmeyer1292/repos","events_url":"https://api.github.com/users/Jmeyer1292/events{/privacy}","received_events_url":"https://api.github.com/users/Jmeyer1292/received_events","type":"User","site_admin":false},"body":"As it is now, probably not. It originally did velocity calculations. It's also possible that for a generic TrajectoryPt we might not have a valid solution when converting to joint positions. In our case the planner fails first. Should I change to void?","created_at":"2015-02-19T21:08:27Z","updated_at":"2015-02-19T21:08:27Z","html_url":"https://github.com/ros-industrial-consortium/godel/pull/39#discussion_r25025643","pull_request_url":"https://api.github.com/repos/ros-industrial-consortium/godel/pulls/39","_links":{"self":{"href":"https://api.github.com/repos/ros-industrial-consortium/godel/pulls/comments/25025643"},"html":{"href":"https://github.com/ros-industrial-consortium/godel/pull/39#discussion_r25025643"},"pull_request":{"href":"https://api.github.com/repos/ros-industrial-consortium/godel/pulls/39"}}},"pull_request":{"url":"https://api.github.com/repos/ros-industrial-consortium/godel/pulls/39","id":29230605,"html_url":"https://github.com/ros-industrial-consortium/godel/pull/39","diff_url":"https://github.com/ros-industrial-consortium/godel/pull/39.diff","patch_url":"https://github.com/ros-industrial-consortium/godel/pull/39.patch","issue_url":"https://api.github.com/repos/ros-industrial-consortium/godel/issues/39","number":39,"state":"open","locked":false,"title":"Descartes integration with Godel","user":{"login":"Jmeyer1292","id":4960180,"avatar_url":"https://avatars.githubusercontent.com/u/4960180?v=3","gravatar_id":"","url":"https://api.github.com/users/Jmeyer1292","html_url":"https://github.com/Jmeyer1292","followers_url":"https://api.github.com/users/Jmeyer1292/followers","following_url":"https://api.github.com/users/Jmeyer1292/following{/other_user}","gists_url":"https://api.github.com/users/Jmeyer1292/gists{/gist_id}","starred_url":"https://api.github.com/users/Jmeyer1292/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/Jmeyer1292/subscriptions","organizations_url":"https://api.github.com/users/Jmeyer1292/orgs","repos_url":"https://api.github.com/users/Jmeyer1292/repos","events_url":"https://api.github.com/users/Jmeyer1292/events{/privacy}","received_events_url":"https://api.github.com/users/Jmeyer1292/received_events","type":"User","site_admin":false},"body":"This code is not done by any means, but I wanted to put it up here to get feedback and discussion.\r\n\r\nThis pull request uses Descartes to solve paths generated by the tool-path-planner. Most of the changes are in godel_path_planning but there are a few in other locations such as in surface_blending_service.cpp.\r\n\r\nAgain this under active development. I will be testing on actual hardware as soon as tomorrow. Please forgive hard-coded or magic values. ","created_at":"2015-02-13T00:31:52Z","updated_at":"2015-02-19T21:08:27Z","closed_at":null,"merged_at":null,"merge_commit_sha":"f0f05c6ed441c273383c37bad7484fb24d1293b1","assignee":null,"milestone":null,"commits_url":"https://api.github.com/repos/ros-industrial-consortium/godel/pulls/39/commits","review_comments_url":"https://api.github.com/repos/ros-industrial-consortium/godel/pulls/39/comments","review_comment_url":"https://api.github.com/repos/ros-industrial-consortium/godel/pulls/comments/{number}","comments_url":"https://api.github.com/repos/ros-industrial-consortium/godel/issues/39/comments","statuses_url":"https://api.github.com/repos/ros-industrial-consortium/godel/statuses/88c9fb78c68fd75b33307733efae8fde76fbd870","head":{"label":"Jmeyer1292:upgrade_descartes_integration","ref":"upgrade_descartes_integration","sha":"88c9fb78c68fd75b33307733efae8fde76fbd870","user":{"login":"Jmeyer1292","id":4960180,"avatar_url":"https://avatars.githubusercontent.com/u/4960180?v=3","gravatar_id":"","url":"https://api.github.com/users/Jmeyer1292","html_url":"https://github.com/Jmeyer1292","followers_url":"https://api.github.com/users/Jmeyer1292/followers","following_url":"https://api.github.com/users/Jmeyer1292/following{/other_user}","gists_url":"https://api.github.com/users/Jmeyer1292/gists{/gist_id}","starred_url":"https://api.github.com/users/Jmeyer1292/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/Jmeyer1292/subscriptions","organizations_url":"https://api.github.com/users/Jmeyer1292/orgs","repos_url":"https://api.github.com/users/Jmeyer1292/repos","events_url":"https://api.github.com/users/Jmeyer1292/events{/privacy}","received_events_url":"https://api.github.com/users/Jmeyer1292/received_events","type":"User","site_admin":false},"repo":{"id":30308164,"name":"godel","full_name":"Jmeyer1292/godel","owner":{"login":"Jmeyer1292","id":4960180,"avatar_url":"https://avatars.githubusercontent.com/u/4960180?v=3","gravatar_id":"","url":"https://api.github.com/users/Jmeyer1292","html_url":"https://github.com/Jmeyer1292","followers_url":"https://api.github.com/users/Jmeyer1292/followers","following_url":"https://api.github.com/users/Jmeyer1292/following{/other_user}","gists_url":"https://api.github.com/users/Jmeyer1292/gists{/gist_id}","starred_url":"https://api.github.com/users/Jmeyer1292/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/Jmeyer1292/subscriptions","organizations_url":"https://api.github.com/users/Jmeyer1292/orgs","repos_url":"https://api.github.com/users/Jmeyer1292/repos","events_url":"https://api.github.com/users/Jmeyer1292/events{/privacy}","received_events_url":"https://api.github.com/users/Jmeyer1292/received_events","type":"User","site_admin":false},"private":false,"html_url":"https://github.com/Jmeyer1292/godel","description":"ROS-Industrial Focused Technical Project: Robotic Blending","fork":true,"url":"https://api.github.com/repos/Jmeyer1292/godel","forks_url":"https://api.github.com/repos/Jmeyer1292/godel/forks","keys_url":"https://api.github.com/repos/Jmeyer1292/godel/keys{/key_id}","collaborators_url":"https://api.github.com/repos/Jmeyer1292/godel/collaborators{/collaborator}","teams_url":"https://api.github.com/repos/Jmeyer1292/godel/teams","hooks_url":"https://api.github.com/repos/Jmeyer1292/godel/hooks","issue_events_url":"https://api.github.com/repos/Jmeyer1292/godel/issues/events{/number}","events_url":"https://api.github.com/repos/Jmeyer1292/godel/events","assignees_url":"https://api.github.com/repos/Jmeyer1292/godel/assignees{/user}","branches_url":"https://api.github.com/repos/Jmeyer1292/godel/branches{/branch}","tags_url":"https://api.github.com/repos/Jmeyer1292/godel/tags","blobs_url":"https://api.github.com/repos/Jmeyer1292/godel/git/blobs{/sha}","git_tags_url":"https://api.github.com/repos/Jmeyer1292/godel/git/tags{/sha}","git_refs_url":"https://api.github.com/repos/Jmeyer1292/godel/git/refs{/sha}","trees_url":"https://api.github.com/repos/Jmeyer1292/godel/git/trees{/sha}","statuses_url":"https://api.github.com/repos/Jmeyer1292/godel/statuses/{sha}","languages_url":"https://api.github.com/repos/Jmeyer1292/godel/languages","stargazers_url":"https://api.github.com/repos/Jmeyer1292/godel/stargazers","contributors_url":"https://api.github.com/repos/Jmeyer1292/godel/contributors","subscribers_url":"https://api.github.com/repos/Jmeyer1292/godel/subscribers","subscription_url":"https://api.github.com/repos/Jmeyer1292/godel/subscription","commits_url":"https://api.github.com/repos/Jmeyer1292/godel/commits{/sha}","git_commits_url":"https://api.github.com/repos/Jmeyer1292/godel/git/commits{/sha}","comments_url":"https://api.github.com/repos/Jmeyer1292/godel/comments{/number}","issue_comment_url":"https://api.github.com/repos/Jmeyer1292/godel/issues/comments/{number}","contents_url":"https://api.github.com/repos/Jmeyer1292/godel/contents/{+path}","compare_url":"https://api.github.com/repos/Jmeyer1292/godel/compare/{base}...{head}","merges_url":"https://api.github.com/repos/Jmeyer1292/godel/merges","archive_url":"https://api.github.com/repos/Jmeyer1292/godel/{archive_format}{/ref}","downloads_url":"https://api.github.com/repos/Jmeyer1292/godel/downloads","issues_url":"https://api.github.com/repos/Jmeyer1292/godel/issues{/number}","pulls_url":"https://api.github.com/repos/Jmeyer1292/godel/pulls{/number}","milestones_url":"https://api.github.com/repos/Jmeyer1292/godel/milestones{/number}","notifications_url":"https://api.github.com/repos/Jmeyer1292/godel/notifications{?since,all,participating}","labels_url":"https://api.github.com/repos/Jmeyer1292/godel/labels{/name}","releases_url":"https://api.github.com/repos/Jmeyer1292/godel/releases{/id}","created_at":"2015-02-04T16:20:33Z","updated_at":"2015-02-04T16:20:34Z","pushed_at":"2015-02-19T20:30:10Z","git_url":"git://github.com/Jmeyer1292/godel.git","ssh_url":"git@github.com:Jmeyer1292/godel.git","clone_url":"https://github.com/Jmeyer1292/godel.git","svn_url":"https://github.com/Jmeyer1292/godel","homepage":"","size":14559,"stargazers_count":0,"watchers_count":0,"language":"C++","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":"hydro-devel"}},"base":{"label":"ros-industrial-consortium:hydro-devel","ref":"hydro-devel","sha":"ffa694e2caa15c870c98cdc34c9668cea914521e","user":{"login":"ros-industrial-consortium","id":6336074,"avatar_url":"https://avatars.githubusercontent.com/u/6336074?v=3","gravatar_id":"","url":"https://api.github.com/users/ros-industrial-consortium","html_url":"https://github.com/ros-industrial-consortium","followers_url":"https://api.github.com/users/ros-industrial-consortium/followers","following_url":"https://api.github.com/users/ros-industrial-consortium/following{/other_user}","gists_url":"https://api.github.com/users/ros-industrial-consortium/gists{/gist_id}","starred_url":"https://api.github.com/users/ros-industrial-consortium/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/ros-industrial-consortium/subscriptions","organizations_url":"https://api.github.com/users/ros-industrial-consortium/orgs","repos_url":"https://api.github.com/users/ros-industrial-consortium/repos","events_url":"https://api.github.com/users/ros-industrial-consortium/events{/privacy}","received_events_url":"https://api.github.com/users/ros-industrial-consortium/received_events","type":"Organization","site_admin":false},"repo":{"id":17028696,"name":"godel","full_name":"ros-industrial-consortium/godel","owner":{"login":"ros-industrial-consortium","id":6336074,"avatar_url":"https://avatars.githubusercontent.com/u/6336074?v=3","gravatar_id":"","url":"https://api.github.com/users/ros-industrial-consortium","html_url":"https://github.com/ros-industrial-consortium","followers_url":"https://api.github.com/users/ros-industrial-consortium/followers","following_url":"https://api.github.com/users/ros-industrial-consortium/following{/other_user}","gists_url":"https://api.github.com/users/ros-industrial-consortium/gists{/gist_id}","starred_url":"https://api.github.com/users/ros-industrial-consortium/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/ros-industrial-consortium/subscriptions","organizations_url":"https://api.github.com/users/ros-industrial-consortium/orgs","repos_url":"https://api.github.com/users/ros-industrial-consortium/repos","events_url":"https://api.github.com/users/ros-industrial-consortium/events{/privacy}","received_events_url":"https://api.github.com/users/ros-industrial-consortium/received_events","type":"Organization","site_admin":false},"private":false,"html_url":"https://github.com/ros-industrial-consortium/godel","description":"ROS-Industrial Focused Technical Project: Robotic Blending","fork":false,"url":"https://api.github.com/repos/ros-industrial-consortium/godel","forks_url":"https://api.github.com/repos/ros-industrial-consortium/godel/forks","keys_url":"https://api.github.com/repos/ros-industrial-consortium/godel/keys{/key_id}","collaborators_url":"https://api.github.com/repos/ros-industrial-consortium/godel/collaborators{/collaborator}","teams_url":"https://api.github.com/repos/ros-industrial-consortium/godel/teams","hooks_url":"https://api.github.com/repos/ros-industrial-consortium/godel/hooks","issue_events_url":"https://api.github.com/repos/ros-industrial-consortium/godel/issues/events{/number}","events_url":"https://api.github.com/repos/ros-industrial-consortium/godel/events","assignees_url":"https://api.github.com/repos/ros-industrial-consortium/godel/assignees{/user}","branches_url":"https://api.github.com/repos/ros-industrial-consortium/godel/branches{/branch}","tags_url":"https://api.github.com/repos/ros-industrial-consortium/godel/tags","blobs_url":"https://api.github.com/repos/ros-industrial-consortium/godel/git/blobs{/sha}","git_tags_url":"https://api.github.com/repos/ros-industrial-consortium/godel/git/tags{/sha}","git_refs_url":"https://api.github.com/repos/ros-industrial-consortium/godel/git/refs{/sha}","trees_url":"https://api.github.com/repos/ros-industrial-consortium/godel/git/trees{/sha}","statuses_url":"https://api.github.com/repos/ros-industrial-consortium/godel/statuses/{sha}","languages_url":"https://api.github.com/repos/ros-industrial-consortium/godel/languages","stargazers_url":"https://api.github.com/repos/ros-industrial-consortium/godel/stargazers","contributors_url":"https://api.github.com/repos/ros-industrial-consortium/godel/contributors","subscribers_url":"https://api.github.com/repos/ros-industrial-consortium/godel/subscribers","subscription_url":"https://api.github.com/repos/ros-industrial-consortium/godel/subscription","commits_url":"https://api.github.com/repos/ros-industrial-consortium/godel/commits{/sha}","git_commits_url":"https://api.github.com/repos/ros-industrial-consortium/godel/git/commits{/sha}","comments_url":"https://api.github.com/repos/ros-industrial-consortium/godel/comments{/number}","issue_comment_url":"https://api.github.com/repos/ros-industrial-consortium/godel/issues/comments/{number}","contents_url":"https://api.github.com/repos/ros-industrial-consortium/godel/contents/{+path}","compare_url":"https://api.github.com/repos/ros-industrial-consortium/godel/compare/{base}...{head}","merges_url":"https://api.github.com/repos/ros-industrial-consortium/godel/merges","archive_url":"https://api.github.com/repos/ros-industrial-consortium/godel/{archive_format}{/ref}","downloads_url":"https://api.github.com/repos/ros-industrial-consortium/godel/downloads","issues_url":"https://api.github.com/repos/ros-industrial-consortium/godel/issues{/number}","pulls_url":"https://api.github.com/repos/ros-industrial-consortium/godel/pulls{/number}","milestones_url":"https://api.github.com/repos/ros-industrial-consortium/godel/milestones{/number}","notifications_url":"https://api.github.com/repos/ros-industrial-consortium/godel/notifications{?since,all,participating}","labels_url":"https://api.github.com/repos/ros-industrial-consortium/godel/labels{/name}","releases_url":"https://api.github.com/repos/ros-industrial-consortium/godel/releases{/id}","created_at":"2014-02-20T17:20:25Z","updated_at":"2015-02-19T00:27:50Z","pushed_at":"2015-02-19T00:27:50Z","git_url":"git://github.com/ros-industrial-consortium/godel.git","ssh_url":"git@github.com:ros-industrial-consortium/godel.git","clone_url":"https://github.com/ros-industrial-consortium/godel.git","svn_url":"https://github.com/ros-industrial-consortium/godel","homepage":"","size":14792,"stargazers_count":0,"watchers_count":0,"language":"C++","has_issues":true,"has_downloads":true,"has_wiki":true,"has_pages":false,"forks_count":7,"mirror_url":null,"open_issues_count":9,"forks":7,"open_issues":9,"watchers":0,"default_branch":"hydro-devel"}},"_links":{"self":{"href":"https://api.github.com/repos/ros-industrial-consortium/godel/pulls/39"},"html":{"href":"https://github.com/ros-industrial-consortium/godel/pull/39"},"issue":{"href":"https://api.github.com/repos/ros-industrial-consortium/godel/issues/39"},"comments":{"href":"https://api.github.com/repos/ros-industrial-consortium/godel/issues/39/comments"},"review_comments":{"href":"https://api.github.com/repos/ros-industrial-consortium/godel/pulls/39/comments"},"review_comment":{"href":"https://api.github.com/repos/ros-industrial-consortium/godel/pulls/comments/{number}"},"commits":{"href":"https://api.github.com/repos/ros-industrial-consortium/godel/pulls/39/commits"},"statuses":{"href":"https://api.github.com/repos/ros-industrial-consortium/godel/statuses/88c9fb78c68fd75b33307733efae8fde76fbd870"}}}}