Controller failed with error code PATH_TOLERANCE_VIOLATED.

edit

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 ... (more)