MoveIt! Compute Cartesian path ABORTED: Solution found but controller failed during execution.
I am using this robotic arm. I want to use the cartesian path planning. I start with a current position and gradually decrease the z coordinate and increase the y coordinate little by little, and append those positions to the list which I then pass to the compute_cartesian_path
method. However I get an empty plan. Down below is the output of the terminal with the plan printed and the error message. How to fix this?
[ INFO] [1557217251.857413662, 9.357000000] [/move_group_commander_wrappers_1557217250468349932] [ros.moveit_ros_planning_interface.move_group_interface]: Ready to take commands for planning group arm.
[ INFO] [1557217252.075151786, 9.575000000] [/move_group] [ros.moveit_ros_move_group]: Received request to compute Cartesian path
[ INFO] [1557217252.075479483, 9.575000000] [/move_group] [ros.moveit_ros_move_group]: Attempting to follow 3 waypoints for link 'gripper_link' using a step of 0.010000 m and jump threshold 1.000000 (in global reference frame)
[ INFO] [1557217252.076646554, 9.576000000] [/move_group] [ros.moveit_ros_move_group]: Computed Cartesian path with 1 points (followed 0.000000% of requested trajectory)
joint_trajectory:
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: "/world"
joint_names: [base_forearm_joint, forearm_0_1_joint, forearm_arm_joint, arm_0_1_joint, arm_gripper_joint]
points:
-
positions: [0.0, 0.0, 0.0, 0.0, 0.0]
velocities: []
accelerations: []
effort: []
time_from_start:
secs: 0
nsecs: 0
multi_dof_joint_trajectory:
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: ''
joint_names: []
points: []
[ INFO] [1557217252.079122682, 9.579000000] [/move_group] [ros.moveit_ros_move_group.ExecuteTrajectoryAction]: Execution request received
[ WARN] [1557217252.091157172, 9.590000000] [/move_group] [ros.moveit_simple_controller_manager]: Controller arm_position_controller failed with error code INVALID_GOAL
[ WARN] [1557217252.091245395, 9.591000000] [/move_group] [ros.moveit_ros_planning.trajectory_execution_manager]: Controller handle arm_position_controller reports status FAILED
[ INFO] [1557217252.091298267, 9.591000000] [/move_group] [ros.moveit_ros_planning.trajectory_execution_manager]: Completed trajectory execution with status FAILED ...
[ INFO] [1557217252.091413761, 9.591000000] [/move_group] [ros.moveit_ros_move_group.ExecuteTrajectoryAction]: Execution completed: FAILED
[ INFO] [1557217252.091589255, 9.591000000] [/move_group_commander_wrappers_1557217250468349932] [ros.moveit_ros_planning_interface.move_group_interface]: ABORTED: Solution found but controller failed during execution
EDIT
this is the code responsible for planning. When I print the waypoints
, they are all correct. But the plan
contains just one point at 0.
waypoints = []
wpose = self.move_group.get_current_pose().pose
init_pose = copy.deepcopy(wpose)
wpose.position.z -= 0.05
wpose.position.y += 0.03
waypoints.append(copy.deepcopy(wpose))
wpose.position.z -= 0.05
wpose.position.y += 0.03
waypoints.append(copy.deepcopy(wpose))
wpose.position.z -= 0.05
wpose.position.y += 0.03
waypoints.append(copy.deepcopy(wpose))
print(waypoints)
(plan, fraction) = self.move_group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # end_effector_step, Cartesian path to be interpolated at a resolution of 1 cm
1.0, # jump_threshold
avoid_collisions = True)
You describe a problem with "receiving an empty plan" and the output also indicates that MoveIt was unable to generate a Cartesian trajectory for you.
But your question title does not seem to correspond. Can you please check and correct?
This is the real problem:
not the controller bits.
@gvdhoorn but what to do about it? I append to the list 4 positions starting with the initial position. Why does it return path with 1 point?
@kump the compute cartesian path service usually does not fail when it cannot find a path. Instead, it reports the fraction of the path it succeeded to find. In your case, that is 0%, so for some reason, it didn't find any path.
I'm having problems with this same repository. It's the only "near working" repository using moveit controllers I was able to find, because the documentation and the software itself is so poorly designed and maintained.
Any solution here? I am also facing the same error.
@gonzalocasas, for me, same cartesian goal poses are working fine with Rviz (demo.launch), but not with Gazebo+Rviz. Is there any reason behind that?
PS: I am using my custom robot