computeCartesianPath() returns -21 (FRAME_TRANSFORM_FAILURE)
I'm trying to perform a linear motion to pick an object. This is code:
bool RoboticArm::linearMove(geometry_msgs::Pose pose) {
move_group_ptr->setPlanningTime(10.0);
robot_state::RobotState start_state(*move_group_ptr->getCurrentState());
move_group_ptr->setStartState(start_state);
std::vector<geometry_msgs::Pose> waypoints;
geometry_msgs::Pose target_pose = pose;
target_pose.position.z -= 0.15;
waypoints.push_back(target_pose); // down
move_group_ptr->setPoseTarget(target_pose);
move_group_ptr->setMaxVelocityScalingFactor(0.1);
move_group_ptr->setPoseReferenceFrame("/camera_rgb_optical_frame");
moveit_msgs::RobotTrajectory trajectory;
const double jump_threshold = 0.0;
const double eef_step = 0.01;
moveit_msgs::MoveItErrorCodes error_code;
double fraction = move_group_ptr->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, true, &error_code);
ROS_INFO_NAMED("tutorial", "Visualizing plan 4 (Cartesian path) (%.2f%% acheived) err(%d)", fraction * 100.0, error_code.val);
if (move_group_ptr->execute(trajectory) != moveit::planning_interface::MoveItErrorCode::SUCCESS) {
ROS_ERROR("Failed to plan linear motion");
move_group_ptr->clearPoseTargets();
move_group_ptr->setMaxVelocityScalingFactor(1.0);
return false;
}
cout << "Linear Motion Completed" << endl;
move_group_ptr->setMaxVelocityScalingFactor(1.0);
return true;
}
We don't understand why the error is -21. Note that knowsFrameTransform() returns 1. Can someone help me?
is that leading
*
in*move_group_ptr->getCurrentState()
correct? Shouldn't it either be*(move_group_ptr).getCurrentState()
ormove_group_ptr->getCurrentState()
?