Initialze planning graph using the robotstate

[BasJ93]

I am currently using Descartes with an UR5 arm to perform the task of combining a nut and bolt. I like Descartes because in my experience it produces superior Cartesian paths, and in my application the yaw of the end effector is not important.

However, I am running in to the problem that the robot pose changes when a new path is computed. Because the UR can produce 4 valid IK solutions for a given pose in my path, the planner can end up with a trajectory where the robot pose is changed from the actual pose to a different solution for the same point.

When looking through the source for Descartes, I stumbled upon a possible solution:

planning_graph.h
// TODO: add constructor that takes RobotState as param PlanningGraph(descartes_core::RobotModelConstPtr model);

I would think that if this TODO is worked out, this would solve my problem. This should allow me to seed the PlanningGraph::getShortestPath() with the current robot state as the start state. Please correct me if I am wrong here.

Sadly, I can't seem to figure out how to do this myself, so any help is appreciated.

[Jmeyer1292]

Interesting that you get different plans each time. Are they all the "same" cost (e.g. just a twist of joint 6)?

I seed my paths by including the current position of the robot as a point in the trajectory to solve. Specifically, put a JointTrajectoryPt with your robots start pose at the front of your input trajectory. Peel it off of the result trajectory if required.

[BasJ93]

I misread your comment, and misunderstood the JoinTrajectryPt. I now realize that that is actually a point in the trajectory defined with joint angles. That might very well be my solution.

Old response:
Yes, I think that the cost is the same, as the robot pose is the opposite of the current pose e.g. the shoulder joint rotates 180 degrees.

I do start the trajectory with the current pose of the robot as the first trajectory point. This point is of the AxialSymmetricPt type. I don't think this is the source of my problem, as I have noticed that not using the URKinematics plugin, but using KDL instead does not result in this behavior.

I must also add that I am still running on the Indigo version, but I have seen a similar result when using the Kinetic version build from the latest commit. I suspect that in the Indigo version it has to do with the virtual_vertex being added to the graph, as to be able to search all possible solution. This does mean that all the possible starting poses can be selected as the most optimal solution, instead of the actual pose of the robot. I have looked at how to replace the virtual_vertex with the actual vertex that represents the current robot state, but have not been able to come up with the code required.

[BasJ93]

Thank you, seeding the planner by including the current pose solved my problem. I will look into writing a test program to see if the cost is the same for the paths where the robot pose is changed.

[jbeck28]

I realize this was 2 years ago but I am have a seemingly isomorphic problem. Paths planned for my UR5e tend to end up in a joint configuration which causes problems with my tooling. Sometimes the paths plan properly in a configuration where wrist_3 does not point down. Being able to set a first point's joint coordinates would probably cause it to optimize toward the correct solution, but it seems like the planner only takes in a vector descartes_core::trajectory_pt rather than the standard jointtrajectorypt which contains joint state information, rather than the (seemingly) strictly cartesian space information contained in the descartes_core type.

What are you doing differently to prepend a joint-space point to your input trajectory?

Edit
I think this is what I was looking for:
https://github.com/ros-industrial/industrial_training/blob/kinetic/exercises/4.1/src/myworkcell_core/src/descartes_node.cpp#L78