Revision history [back]

There is multiple things wrong with this question / code.

1. Please write in full sentences and explain what you want to achieve and what moveit configuration you use. Also include links to the repositories. Otherwise nobody understands what you mean.

I believe you are using https://github.com/frankaemika/franka_ros and https://github.com/ros-planning/panda_moveit_config .

I have no idea what you are trying to achieve.

The code is overly redundant and can be reduced to less lines of code.

1. you apparently try to control the hand group of the robot, which consists of two prismatic joints defined here. As can be seen in the line pointed out in the link, the upper joint limits of both are 0.04m. You specify a target position of 0.2m per joint. The gripper cannot open that wide, so the plan request fails. You probably see an output like "No valid goal state sampled" in the terminal where you started moveit.

2. You don't need current_state at all - nor do you need a moveit::core::RobtState anywhere. You could just specify your target as hand_group.setJointValueTarget(std::vector<double> {0.04,0.04}) or similar.

3. The way you use plan and move suggests you expect move to execute the plan you generated before. This is not the case. As a matter of fact the logic you implement here is equivalent to just calling move() without plan before.