Controller failed with error code PATH_TOLERANCE_VIOLATED.
Hey, I hope you guys are doing well! I am trying to move my robotic arm in a gazebo simulation environment using ROS framework, but unfortunately getting below error:
[ INFO] [1587368371.019352158, 1094.713000000]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1587368371.031432827, 1094.721000000]: RRTConnect: Created 5 states (2 start + 3 goal)
[ INFO] [1587368371.031485894, 1094.721000000]: Solution found in 0.012215 seconds
[ INFO] [1587368371.048440007, 1094.731000000]: SimpleSetup: Path simplification took 0.016895 seconds and changed from 4 to 2 states
[ WARN] [1587368371.197235055, 1094.839000000]: Controller ow5/arm_manipulator_controller failed with error code PATH_TOLERANCE_VIOLATED
[ WARN] [1587368371.197371671, 1094.839000000]: Controller handle ow5/arm_manipulator_controller reports status ABORTED
[ INFO] [1587368371.197427331, 1094.839000000]: Completed trajectory execution with status ABORTED ...
[ INFO] [1587368371.356938233, 1094.938000000]: Received event 'stop'
I am modifying the parameters in controllers.yaml
file but nothing seems to work.
controllers.yaml
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
arm_manipulator_controller:
type: "position_controllers/JointTrajectoryController"
joints:
- j1
- j2
- j3
- j4
- j5
constraints:
goal_time: 10.0 # Override default
stopped_velocity_tolerance: 0.05 # Override default
j1: {trajectory: 0.1, goal: 0.05} # Override default
j2: {trajectory: 0.1, goal: 0.05} # Override default
j3: {trajectory: 0.1, goal: 0.05} # Override default
j4: {trajectory: 0.1, goal: 0.05} # Override default
j5: {trajectory: 0.1, goal: 0.05} # Override default
stop_trajectory_duration: 0.5
state_publish_rate: 50 # Override default
action_monitor_rate: 30 # Override default
allow_partial_joints_goal: true # Override default
code:
class MoveGroupPythonInteface(object):
def __init__(self):
super(MoveGroupPythonInteface, self).__init__()
# initialize `moveit_commander`_ and a `rospy`_ node:
joint_state_topic = ['joint_states:=/ow5/joint_states']
moveit_commander.roscpp_initialize(joint_state_topic)
rospy.init_node('move_group_interface', anonymous=True)
group_name = "arm_manipulator"
self.move_group = moveit_commander.MoveGroupCommander(group_name)
def go_to_home_pose(self):
# Planning to home position
joint_goal = self.move_group.get_current_joint_values()
joint_goal[0] = 1.95118
joint_goal[1] = 1.11642
joint_goal[2] = -1.41584
joint_goal[3] = -0.21243
joint_goal[4] = 3.13971
self.move_group.go(joint_goal, wait=True)
self.move_group.stop()
def go_to_joint_state(self, joint_values):
# We can get the joint values from the group and adjust some of the values
# Here, joint_values is a map containing robot joint values
joint_goal = self.move_group.get_current_joint_values()
joint_goal = list(joint_values)
self.move_group.go(joint_goal, wait=True)
self.move_group.stop()
def main():
try:
interface = MoveGroupPythonInteface()
interface.go_to_home_pose()
# position A
interface.go_to_joint_state([-2.81659, 0.28823, -0.87248, -1.00072, -2.67695])
# position B
interface.go_to_joint_state([0.32682, -0.42397, 0.90111, 0.30738, 3.00750])
except rospy.ROSInterruptException:
return
if __name__=='__main__':
main()
On running above code, the robot is successfully moving to home position and then going to position B by skipping position A, giving me below messages in the terminal:
[ INFO] [1587368363.488147562, 1089.525000000]: ABORTED: Solution found but controller failed during execution
[ INFO] [1587368371.356526641, 1094.937000000]: ABORTED: Solution found but controller failed during execution
I found the above joint values of position A and B by moving the markers in RViz. I am using ros kinetic and gazebo 9. I would really appreciate someone's help/guidance on this as I am not able to figure out the root cause of ...
Either your controllers are not tuned correctly or your path tolerance is too tight for the velocity limits that you've set. The PATH TOLERANCE VIOLATED is triggered when the controller exceeds the distance to an intermediate waypoint by the amount defined in each of the trajectory constraints (in your case set to 0.1 [radian]). Try temporarily setting those constraints to higher values and check if it still fails. If it doesn't, tune your controller or make your velocity limits match the actual values.