Get the generated grasp pose associated with a planned solution #379

Hello,

I am trying to use MTC in a task planner for a combined task and motion planning approach. I need a way to get the chosen grasp pose from a planned solution.

For example in a Pick task, the GenerateGraspPose stage generates multiple grasps, each valid grasp leads to a solution. In my case I fix the number of planned solutions to the pick task to 1, and I want to access the grasp pose (as a Pose msg or Eigen::Isometry3d) leading to that solution after the planning process, something like this:

grasp_pose = task_->solutions()->front()->stages()["generate grasp pose"]->pose

Is there a way to do this currently ?

Thanks in advance !