{"action":"created","issue":{"url":"https://api.github.com/repos/ros-industrial-consortium/hamilton/issues/2","labels_url":"https://api.github.com/repos/ros-industrial-consortium/hamilton/issues/2/labels{/name}","comments_url":"https://api.github.com/repos/ros-industrial-consortium/hamilton/issues/2/comments","events_url":"https://api.github.com/repos/ros-industrial-consortium/hamilton/issues/2/events","html_url":"https://github.com/ros-industrial-consortium/hamilton/pull/2","id":92967880,"number":2,"title":"Demo #1 simple main","user":{"login":"madratman","id":6305323,"avatar_url":"https://avatars.githubusercontent.com/u/6305323?v=3","gravatar_id":"","url":"https://api.github.com/users/madratman","html_url":"https://github.com/madratman","followers_url":"https://api.github.com/users/madratman/followers","following_url":"https://api.github.com/users/madratman/following{/other_user}","gists_url":"https://api.github.com/users/madratman/gists{/gist_id}","starred_url":"https://api.github.com/users/madratman/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/madratman/subscriptions","organizations_url":"https://api.github.com/users/madratman/orgs","repos_url":"https://api.github.com/users/madratman/repos","events_url":"https://api.github.com/users/madratman/events{/privacy}","received_events_url":"https://api.github.com/users/madratman/received_events","type":"User","site_admin":false},"labels":[],"state":"open","locked":false,"assignee":null,"milestone":null,"comments":2,"created_at":"2015-07-04T03:55:08Z","updated_at":"2015-07-04T16:24:10Z","closed_at":null,"pull_request":{"url":"https://api.github.com/repos/ros-industrial-consortium/hamilton/pulls/2","html_url":"https://github.com/ros-industrial-consortium/hamilton/pull/2","diff_url":"https://github.com/ros-industrial-consortium/hamilton/pull/2.diff","patch_url":"https://github.com/ros-industrial-consortium/hamilton/pull/2.patch"},"body":"```\r\ncatkin_make hamilton hamilton_demo \r\nsource devel/setup.bash\r\nroslaunch descartes_tutorial_support motoman_sim.launch \r\nrosrun hamilton hamilton_demo \r\n```\r\nAlso required is ~~[motoman_sia20d_moveit_config](https://github.com/ros-industrial/motoman/tree/hydro-devel/motoman_sia20d_moveit_config)~~, [motoman metapackage](https://github.com/ros-industrial/motoman) which I am not including because of its size. Clone in `catkin_ws/src` and do `catkin_make motoman` \r\nI am adding some more commits to clean up and add comments within this hour. But you can do the above to run the demo right now and play around with waypoints. I'll add some more examples as well. \r\nIt is built over [descartes_tutorial](https://github.com/ros-industrial-consortium/descartes_tutorials/blob/master/descartes_tutorial1/src/tutorial1.cpp) and hence follows a joint trajectory action goal\r\nThis is what the code does(or tries to do):\r\n- Moves to home\r\n- Moves to a Pose target `0.25, 0.25, 1.0`  \r\n- MoveIt traj(goes up):\r\n```\r\n  for(int i = 0; i < 8; ++i)\r\n  {\r\n    target_pose.position.z += 0.05;\r\n    waypoints.push_back(target_pose); \r\n  }\r\n```\r\n- Descartes traj(comes down):\r\n```\r\n for (int i = 0; i < 10; ++i)\r\n  {\r\n    pose = Eigen::Translation3d(0.25, 0.25, 1.4 - 0.05 * i);\r\n    descartes_core::TrajectoryPtPtr pt = makeTolerancedCartesianPoint(pose);\r\n    points.push_back(pt);\r\n  }\r\n```\r\n- Planning process:\r\n  - MoveIt\r\n\r\n    Does a `group.computeCartesianPath(...)`, followed by a `iptp`. Saves in `moveit_robot_traj`. \r\n    Extract joint solution by `moveit_joint_traj = moveit_robot_traj.joint_trajectory`\r\n  - Descartes\r\n\r\n    same as tut - uses `densePlanner`. Save in `descartes_joint_solution` \r\n\r\n- Stitching and execution\r\nHere onwards, `something_robot_traj` implies a [robot_trajectory::RobotTrajectory](http://docs.ros.org/hydro/api/moveit_core/html/classrobot__trajectory_1_1RobotTrajectory.html) object, and `something_joint_traj` refers to a `trajectory_msgs::JointTrajectory` object.  The following snippet should be readable\r\n```\r\n  overall_robot_traj(robotmodel, \"manipulator\");\r\n  descartes_robot_traj(robotmodel, \"manipulator\");\r\n  overall_robot_traj.setRobotTrajectoryMsg(kinematicstate, moveit_robot_traj);\r\n  descartes_robot_traj.setRobotTrajectoryMsg(kinematicstate, descartes_joint_solution);\r\n  overall_robot_traj.append(descartes_robot_traj, descartes_robot_traj.getWaypointDurationFromStart(descartes_robot_traj.getWayPointCount()));\r\n  moveit_msgs::RobotTrajectory combined;\r\n  overall_robot_traj.getRobotTrajectoryMsg(combined);\r\n  trajectory_msgs::JointTrajectory joints_combined = combined.joint_trajectory;\r\n  executeTrajectory(joints_combined) //defined in descartes_tutorial. \r\n //should expand robot_trajectory::RobotTrajectory API to get combined_joint directly\r\n```\r\n- Issues\r\n  - The stitch isn’t “nice”. Descartes plans to the first waypoint again and hence the robot changes poses in an arbit fashion -> which makes me realize the obvious things -> constrain the joint pose at \"segment transitions\" to be the same. \r\n  - If there's a slight offset in consecutive segments - citing an example I ran z = 1.25 at the end of free space segment, and z = 1.4 at the beginning of Descartes segment, instead of intuitively going just upwards, we get some fancy trajectory. \r\nHence, in the hybrid planner, we need to interpolate for such situations, which could be either due to developer error or just one would expect such interpolations.   \r\nSo, we have two scenarios and variable spaces - joint vs cartesian. How to decide when to interpolate in what? Should we have a “combined” distance function over the joint and cartesian spaces. So we have three distance metrics. From this inference, we need to come up with a solution which results in intuitive (and non-human killing motion, in the context of the recent news). \r\nBut there is orientation as well - so we got to interpolate in quaternions or in SO(3). I don't know about Euler angles. Maybe if we are far from gimbal lock, euler interpolation is the easiest to implement.    \r\nQuaternions -> have to handle antipodal symmetry.    \r\nJoint space -> distance metric?   \r\nOr we need a combination of these\r\n\r\n`if(person.getPatienceLeft() == 1)`\r\n`{`\r\n&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;[read some issues encountered during running this and thoughts on possible solutions to interpolation] (https://docs.google.com/document/d/1aM8Awzb0Kao-4yxRY8sGc6Kabnw0cqWh1IQ1SllK3Yw/edit?usp=sharing)\r\n`}`\r\n\r\n- Questions:\r\n  - Will Descartes always return the same solution? It is Djikstra behind the scenes, but is sampling also involved here?\r\n```\r\n[ INFO] [1435978791.007538818]: CartID: ID7 JointPoses count: 47\r\n[ INFO] [1435978791.017452068]: Found 10 joint solutions out of 10 iterations\r\n[ INFO] [1435978791.041518214]: Found 10 joint solutions out of 10 iterations\r\n[ INFO] [1435978791.066464410]: Found 8 joint solutions out of 10 iterations\r\n[ INFO] [1435978791.084312710]: Found 10 joint solutions out of 10 iterations\r\n[ INFO] [1435978791.092721533]: Found 10 joint solutions out of 10 iterations\r\n[ INFO] [1435978791.092805969]: CartID: ID8 JointPoses count: 48\r\n[ INFO] [1435978791.122350909]: Found 10 joint solutions out of 10 iterations\r\n[ INFO] [1435978791.126422228]: Found 10 joint solutions out of 10 iterations\r\n[ INFO] [1435978791.140391013]: Found 9 joint solutions out of 10 iterations\r\n[ INFO] [1435978791.156190742]: Found 10 joint solutions out of 10 iterations\r\n[ INFO] [1435978791.192315303]: Found 9 joint solutions out of 10 iterations\r\n```\r\nWhy 5*10 iterations? How can these be changed and their effects?    \r\n"},"comment":{"url":"https://api.github.com/repos/ros-industrial-consortium/hamilton/issues/comments/118527817","html_url":"https://github.com/ros-industrial-consortium/hamilton/pull/2#issuecomment-118527817","issue_url":"https://api.github.com/repos/ros-industrial-consortium/hamilton/issues/2","id":118527817,"user":{"login":"jrgnicho","id":3279718,"avatar_url":"https://avatars.githubusercontent.com/u/3279718?v=3","gravatar_id":"","url":"https://api.github.com/users/jrgnicho","html_url":"https://github.com/jrgnicho","followers_url":"https://api.github.com/users/jrgnicho/followers","following_url":"https://api.github.com/users/jrgnicho/following{/other_user}","gists_url":"https://api.github.com/users/jrgnicho/gists{/gist_id}","starred_url":"https://api.github.com/users/jrgnicho/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/jrgnicho/subscriptions","organizations_url":"https://api.github.com/users/jrgnicho/orgs","repos_url":"https://api.github.com/users/jrgnicho/repos","events_url":"https://api.github.com/users/jrgnicho/events{/privacy}","received_events_url":"https://api.github.com/users/jrgnicho/received_events","type":"User","site_admin":false},"created_at":"2015-07-04T16:24:10Z","updated_at":"2015-07-04T16:24:10Z","body":"Thank you for submitting this PR.  I'll be reviewing it soon."}}