New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Chomp fix goalstate check #899

Open
wants to merge 2 commits into
base: kinetic-devel
from

Conversation

Projects
None yet
6 participants
@bmagyar
Copy link
Member

bmagyar commented May 17, 2018

Description

Fixes #822 originating from not loading the RobotState object properly with joint values.

I also took the opportunity to add *_NAMED prints back which were removed in the console_bridge cleanup and managed to slip through the review.

@Gene0213 please confirm that this fixes the issue you are seeing.

Checklist

  • Required: Code is auto formatted using clang-format
  • Optional: Created tests, which fail without this PR reference
  • Optional: Decide if this should be cherry-picked to other current ROS branches (Indigo, Jade, Kinetic)

@bmagyar bmagyar force-pushed the bmagyar:chomp-fix-goalstate-check branch from 2d78d68 to 3c579f2 May 17, 2018

@bmagyar

This comment has been minimized.

Copy link
Member

bmagyar commented May 17, 2018

@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.

@davetcoleman
Copy link
Member

davetcoleman left a comment

Approve except minor formatting

const Eigen::MatrixXd goal_state = trajectory.getTrajectoryPoint(goal_index);

if (not planning_scene->getRobotModel()->satisfiesPositionBounds(goal_state.data()))
moveit::core::RobotState goal_robotstate = planning_scene->getCurrentState();

This comment has been minimized.

@davetcoleman

davetcoleman May 17, 2018

Member

goal_robot_state

This comment has been minimized.

@bmagyar

@bmagyar bmagyar force-pushed the bmagyar:chomp-fix-goalstate-check branch from 3c579f2 to 5dd87d1 May 17, 2018

@bmagyar

This comment has been minimized.

Copy link
Member

bmagyar commented May 17, 2018

I'd like to confirm with @raghavendersahdev and @Gene0213 that this fixes the issue they've found.

@juanluislm

This comment has been minimized.

Copy link

juanluislm commented May 22, 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.
[ INFO] [1527013789.752135949]: Setting joint yumi_joint_1_r to position 1.41205
[ INFO] [1527013789.752194509]: Setting joint yumi_joint_2_r to position -2.04933
[ INFO] [1527013789.752223264]: Setting joint yumi_joint_7_r to position -0.534935
[ INFO] [1527013789.752257314]: Setting joint yumi_joint_3_r to position 0.740543
[ INFO] [1527013789.752286650]: Setting joint yumi_joint_4_r to position -2.466
[ INFO] [1527013789.752307781]: Setting joint yumi_joint_5_r to position 1.89479
[ INFO] [1527013789.752332489]: Setting joint yumi_joint_6_r to position 0.198166
[ INFO] [1527013789.752422188]: The following collision detectors are active in the planning scene.
[ INFO] [1527013789.752447142]: HYBRID
[ INFO] [1527013789.752485704]: Active collision detector is: HYBRID
[ INFO] [1527013789.905760955]: First coll check took 0.153223535
[ INFO] [1527013789.920665976]: Forward kinematics took 0.003211630
[ INFO] [1527013789.920930903]: iteration: 0
[ INFO] [1527013789.930471379]: Chomp Got mesh to mesh safety at iter 0. Breaking out early.
[ INFO] [1527013789.930506111]: Chomp path is collision free
[ INFO] [1527013789.930524812]: Terminated after 1 iterations, using path from iteration -1
[ INFO] [1527013789.930539497]: Optimization core finished in 0.013103 sec
[ INFO] [1527013789.930560782]: Time per iteration 0.013123
[ WARN] [1527013789.951594586]: Fail: ABORTED: Unknown event

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.
[ INFO] [1526935866.086067507]: Looking around: no
[ INFO] [1526935866.086097182]: Replanning: no
[ INFO] [1526935877.838049979]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1526935877.846752955]: Setting joint gripper_r_joint to position 0.00816195
[ INFO] [1526935877.846814605]: Setting joint gripper_r_joint_m to position 0.00816195
[ INFO] [1526935877.846916161]: The following collision detectors are active in the planning scene.
[ INFO] [1526935877.846936137]: HYBRID
[ INFO] [1526935877.846951168]: Active collision detector is: HYBRID
[ INFO] [1526935877.963768703]: First coll check took 0.116767776
[move_group-4] process has died [pid 18966, exit code -11, cmd /home/apollo/ws_moveit/devel/lib/moveit_ros_move_group/move_group --debug __name:=move_group __log:=/home/apollo/.ros/log/9bbacbea-5d38-11e8-bcd7-3497f65a3b74/move_group-4.log].
log file: /home/apollo/.ros/log/9bbacbea-5d38-11e8-bcd7-3497f65a3b74/move_group-4*.log

Do you have any advice for these problems?

@bmagyar

This comment has been minimized.

Copy link
Member

bmagyar commented May 22, 2018

Unfortunately I can't tell anything without more info. Could you try to run it with gdb please? That should give us more hints. Make sure you've compiled things in debug mode with -DCMAKE_BUILD_TYPE=Debug.

@juanluislm

This comment has been minimized.

Copy link

juanluislm commented May 22, 2018

@bmagyar am on it! I will let you know what I get!

@raghavendersahdev

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented May 22, 2018

Hi @juanluislm , I also get the same error (fails for right arm, works for left) see #822 . I will be trying to see why this happens later this week.

@raghavendersahdev

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented Jun 13, 2018

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:

std::ostringstream temp;
last_state.printStatePositions(temp);
std::cout << temp.str() << " last_state.printStatePositions" <<std::endl;

The output is something like:
selection_030

This causes the condition jc.decide(last_state).satisfied; in this line to evaluate to false, hence it returns False and terminates for the right arm.

Looking into addressing this currently.

@raghavendersahdev

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented Jun 13, 2018

The robot state for the right arm, left/right gripper joints is not being set correctly, in fact in the moveit_msgs::MotionPlanRequest& req , the req.start_state.joint_state.name string array variable contains joints like yumi_joint_1_l,...yumi_joint_7_l for req.group_name = right_arm which is a bit weird and incorrect.

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 jc.decide(last_state).satisfied returned false, which is weird.
I added the following code after this line.

for(int i=0; i<trajectory.getNumJoints() ; i++)
  {
    Eigen::MatrixXd temp_trajectory = trajectory.getTrajectory();
    double temp_end_state = temp_trajectory(trajectory.getEndIndex()-1,i);
    std::string temp_joint_name = req.start_state.joint_state.name[i];
    temp_joint_name[13] = 'r';                               // hack for testing, coz the joints are always for the left arm, forcing here to verify
    std::cout << temp_end_state << " temp_end state " << std::endl;
    std::cout << temp_joint_name << " temp_joint_name " << std::endl;
    last_state.setJointPositions(temp_joint_name, &temp_end_state);
  }

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.

@bmagyar

This comment has been minimized.

Copy link
Member

bmagyar commented Jun 13, 2018

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.

@raghavendersahdev

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented Jun 13, 2018

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 last_state here is loaded in an incorrect manner as it loads into the left arm joints the positions of the right arm and the right arm joints have all 0s,numbers like 6.926e-210, etc. written in it.

When I hack and set the right arm joints to be the same as the left arm joints, this condition jc.decide(last_state).satisfied still returns false, possibly because the when planning for the right arm, since the left arm has the coordinates of the right, the last state goal constraints would pass for the right arm, but fail for the left arm as left_arm joint states is wrong at that point in the last_state variable.

Setting the last_state correctly here should make the planning correct for both arms.

@rhaschke

This comment has been minimized.

Copy link
Contributor

rhaschke commented Nov 28, 2018

The start_state should have the full joint state of the robot.

@bmagyar As pointed out in #1165 (comment), the full current state of the robot is encoded in the PlanningScene passed to the planner. MotionPlanRequest::start_state should only encode deviations from this - if desired.

@bmagyar @raghavendersahdev What is the status of this PR?

@rhaschke

This comment has been minimized.

Copy link
Contributor

rhaschke commented Dec 9, 2018

Looks like this change-set was already merged into Melodic in #960.

@v4hn v4hn added the state unclear label Dec 18, 2018

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment