Join GitHub today
GitHub is home to over 28 million developers working together to host and review code, manage projects, and build software together.Sign up
Chomp fix goalstate check #899
Fixes #822 originating from not loading the
I also took the opportunity to add
@Gene0213 please confirm that this fixes the issue you are seeing.
@Gene0213 provided that this works, we should still add a test that makes sure it stays like that. Would you be up for that? You'd need to add a new planning group here that only has one of the joints in it and add a new test function here which queries this new group with a goal. This should trigger the current issue since it originates from the planning group not covering all the joint state.
referenced this pull request
May 17, 2018
@bmagyar I used the code in your pull request in the workspace that @Gene0213 was using, and your fix seems to work for one of the groups (left_arm) but not for the others (right_arm, left_gripper, right_gripper).
This is what I get when planning for the right_arm:
[ INFO] [1527013789.740735276]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
I think there is something off with the way the joint states for the right arm are filled. When I plan for the left arm, I can visualize the path and execute it correctly, but this error pops up:
[ERROR] [1527013267.895048036]: Found empty JointState message
When planning for the left_gripper or right_gripper, I get this:
[ INFO] [1526935866.086006148]: Ready to take commands for planning group right_gripper.
Do you have any advice for these problems?
The issue here is that the joint states for the last position for the right arm, right gripper joints and the left gripper joints is not set properly. I print the robot state in loop after this line by writing the following code:
This causes the condition
Looking into addressing this currently.
The robot state for the right arm, left/right gripper joints is not being set correctly, in fact in the
I printed these variables while debugging and found this info.
A quick fix/check could be to explicitly set the start and goal joint positions values obtained from the start and end point of the trajectory and set that to be for the req.start_state and the last_state explicitly by getting the joint names and setting their positions appropriately. I tried this, see next para.
I explicitly hacked the code to have the correct last_state to contain the correct values for the right joints, turns out the whatever joint state values should be set for the right arm are by default set for the left arm, it seems to me that the planner is unaware about the existence of the right arm. I also forced the code to have the values of the right arm to be the same as the left arm, so atleast the right arm has correct values but again the
A debugging file for the chomp_planner.cpp.txt is attached here whoever might want to try and debug this issue, might be a good starting point, comment out any couts that you may not need
Also CHOMP planning not only fails for right arm of yumi robot but also fails for the fetch robot arm, implying a bigger issue of possibly move_group not being able to discover all joints for a particular group. Maybe the robotModel is not setup correctly, the robot model does not have the joint values correct
Also another NOTE: if this loop is commented everything works, however this loop checks for if the final robot state is within goal tolerances which might be important.
will do some more testing later today on this to try and fix it.
2 cents reply: The start_state should have the full joint state of the robot. Imagine the simple case of a dual arm robot: if you are only planning for one arm, how do you know you won't collide with the other arm? (self collision vs environment collision check).
I'll try to come back later with a more detailed reply.
Thanks for the reply @bmagyar. Yes this start_state has the full joint state of the robot and it produces the correct trajectory also after optimization, everything is correct for the trajctory optimization. However the
When I hack and set the right arm joints to be the same as the left arm joints, this condition
@bmagyar As pointed out in #1165 (comment), the full current state of the robot is encoded in the PlanningScene passed to the planner.