how to plan a trajectory in cartesian space using OMPL

edit

There is a MoveIt! tutorial that talks about Cartesian paths here. Have you tried it? Here is the relevant part:

You can plan a cartesian path directly by specifying a list of waypoints for the end-effector to go through.

waypoints = []

# start with the current pose
waypoints.append(group.get_current_pose().pose)

# first orient gripper and move forward (+x)
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
waypoints.append(copy.deepcopy(wpose))

# second move down
wpose.position.z -= 0.10
waypoints.append(copy.deepcopy(wpose))

# third move to the side
wpose.position.y += 0.05
waypoints.append(copy.deepcopy(wpose))

We want the cartesian path to be interpolated at a resolution of 1 cm which is why we will specify 0.01 as the eef_step in cartesian translation. We will specify the jump threshold as 0.0, effectively disabling it.

(plan3, fraction) = group.compute_cartesian_path(
                         waypoints,   # waypoints to follow
                         0.01,        # eef_step
                         0.0)         # jump_threshold

print "============ Waiting while RVIZ displays plan3..."
rospy.sleep(5)