How to constraint end-effector's orientation in base frame coordinates
Hi, using Ros2 humble, moveit2 and I need to constraint end-effector's orientation in base frame coordinates. For example effector should be horizontal or vertical to ground, but in both cases i want to set Z absolute tolerance to 6.28 and XY to 0.17, so that effector stays in it's orientation and arm can rotate around base.
How can I do that, because in current state depending on effector orientation I need to change tolerances depending on effector oreintation. I tried transforming tolerances from base frame to effector frame, but the numbers didn't make sense.
moveit_msgs::msg::OrientationConstraint orientation_constraint;
orientation_constraint.header.frame_id = rpi->move_group_->getPoseReferenceFrame();
orientation_constraint.link_name = rpi->move_group_->getEndEffectorLink();
orientation_constraint.orientation = start_orientation_constraint;
orientation_constraint.absolute_x_axis_tolerance = deg_to_rad*tcp_pose_cmd->get_roll_tolerance();
orientation_constraint.absolute_y_axis_tolerance = deg_to_rad*tcp_pose_cmd->get_pitch_tolerance();
orientation_constraint.absolute_z_axis_tolerance = deg_to_rad*tcp_pose_cmd->get_yaw_tolerance();
/*orientation_constraint.absolute_x_axis_tolerance = deg_to_rad*10;
orientation_constraint.absolute_y_axis_tolerance = deg_to_rad*10;
orientation_constraint.absolute_z_axis_tolerance = deg_to_rad*360;*/
orientation_constraint.weight = 1.0;