sanity check: python tutorial cartesian path about moveit_tutorials

[jwhendy]

Just printed waypoints and they appear correct, so that's working, presuming waypoints should be a list of poses (just showing the positions; removed identical orientations for brevity):

============ waypoints:
[
## starting position
position: 
  x: 0.705314384552
  y: 0.188
  z: 1.25001004169
..., 
## +0.1x
position: 
  x: 0.805314384552
  y: 0.188
  z: 1.25001004169
...,
## -0.1z
position: 
  x: 0.805314384552
  y: 0.188
  z: 1.15001004169
...,
## +0.05y
position: 
  x: 0.805314384552
  y: 0.238
  z: 1.15001004169
]

Edit: after posting the below... dangit. I should have included the orientations after all, as they might have shown why this wasn't working.

[jwhendy]

Progress! I found this code for the pi robot and implemented the loop which prints out the success of compute_cartesian_path. Initially my matching of that code succeeded (three phase path), and after a bunch of attempts I narrowed it down.

First off, here's the output with the current tutorial, but with the pi robot status loop implemented (file is here):

roslaunch pr2_moveit_config demo.launch

[ INFO] [1501801861.002583628]: Attempting to follow 4 waypoints for link 'l_wrist_roll_link' using a step of 0.010000 m and jump threshold 0.000000 (in global reference frame)
[ INFO] [1501801861.011129435]: Computed Cartesian path with 7 points (followed 37.500000% of requested trajectory)
[ INFO] [1501801861.012360833]: Received request to compute Cartesian path

rosrun moveit_tutorials move_group_python_interface_tutorial_debug.py

============ Waiting while RVIZ displays plan2...
Still trying after 10 attempts
Still trying after 20 attempts
[...]
Still trying after 100 attempts
path planning failed with 0.375 success after 100 attempts

The current code gets the initial pose, then creates and empty Pose() message, clones position.y and position.z, and updates the orientation.w and position.x components. This won't find a valid path:

wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 1.0
wpose.position.x = waypoints[0].position.x + 0.1
wpose.position.y = waypoints[0].position.y
wpose.position.z = waypoints[0].position.z

In the process of looking at the pi robot code, I noticed the code clones the starting pose rather than creating a fresh pose. Kind of by accident I found out it worked:

wpose = copy.deepcopy(start_pose)
wpose.orientation.w = 1.0
wpose.position.x = waypoints[0].position.x + 0.1

After a lot of mucking about, I realized that the above was what was doing the magic and found that this also works:

wpose = geometry_msgs.msg.Pose()
wpose.orientation.x = waypoints[0].orientation.x
wpose.orientation.y = waypoints[0].orientation.y
wpose.orientation.z = waypoints[0].orientation.z
wpose.orientation.w = 1.0
wpose.position.x = waypoints[0].position.x + 0.1
wpose.position.y = waypoints[0].position.y
wpose.position.z = waypoints[0].position.z

Comparing my messages above of before/after pose, I think the issue is it's not finding a solution from this orientation:

orientation: 
  x: -0.0
  y: -0.887929895957
  z: -0.0
  w: 0.459978803714

To this, which is what gets set in the tutorial:

orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 1.0

I'm not good enough with robots to know what was intended, but at least I feel good having narrowed down what was going on.

[felixvd]

Since the link in the original post is broken and this seem to be stale, I'll close this. Please reopen if need be.