ExecuteTrajectoryAction is ABORTED when executing robot trajectory
I am trying to move a robot through a set of waypoints with using computeCartesianPath
and ExecuteTrajectoryAction
, but it gets ABORTED
. Below, the code I use:
moveit_msgs::RobotTrajectory trajectory;
std::vector<geometry_msgs::Pose> waypoints;
geometry_msgs::Pose target_pose = current_pose;
waypoints.push_back(current_pose);
target_pose.position.x += 0.02;
waypoints.push_back(target_pose);
waypoints.push_back(current_pose);
double fraction = move_group.computeCartesianPath(waypoints, 0.01, 0.0, trajectory);
// send a goal to the action
moveit_msgs::ExecuteTrajectoryActionGoal goal;
goal.goal.trajectory = trajectory;
ac.sendGoal(goal.goal);
In order to verify if robot can reach every position in generated trajectory, I use setJointValueTarget
for each joint position, then plan
and move
and it works properly. Here part of generated trajectory:
joint_trajectory:
header:
seq: 0
stamp: 0.000000000
frame_id: /base_arm_link
joint_names[]
joint_names[0]: joint_1
joint_names[1]: joint_2
joint_names[2]: joint_3
points[]
...
points[4]:
positions[]
positions[0]: 0.0302061
positions[1]: 0.762825
positions[2]: -0.00153398
velocities[]
velocities[0]: -0.115171
velocities[1]: 0.128867
velocities[2]: 1.6198e-18
accelerations[]
accelerations[0]: -0.00539319
accelerations[1]: -0.00836068
accelerations[2]: -3.29225e-15
effort[]
time_from_start: 0.781236721
Why does ExecuteTrajectoryAction
fail?
Do you have any additional log from move_group node? I guess the time parametrization is off, hence executor is rejecting the trajectory.