Controller aborts trajectory goal with GOAL_TOLERANCE_VIOLATION after execution
I am simulating denso arm in Gazebo 2 on Indigo.
denso_control.yaml
vs087:
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 125
arm_controller:
type: "position_controllers/JointTrajectoryController"
joints:
- joint_1
- joint_2
- joint_3
- joint_4
- joint_5
- joint_6
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
joint_1: {trajectory: 0.1, goal: 0.1}
joint_2: {trajectory: 0.1, goal: 0.1}
joint_3: {trajectory: 0.1, goal: 0.1}
joint_4: {trajectory: 0.1, goal: 0.1}
joint_5: {trajectory: 0.1, goal: 0.1}
joint_6: {trajectory: 0.1, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 125
action_monitor_rate: 10
I am setting the following parameters while initializing my move_group:
group.set_planner_id("RRTConnectkConfigDefault")
group.set_goal_tolerance(0.1)
group.set_num_planning_attempts(10)
group.set_planning_time(5.0)
group.set_max_velocity_scaling_factor(1.0)
group.set_max_acceleration_scaling_factor(1.0)
After executing a reachable target (whether named or pose target), the controller throws an error:
[ WARN] [1508768453.519348029, 51.904000000]: Dropping first 1 trajectory point(s) out of 10, as they occur before the current time.
First valid point will be reached in 0.208s.
[ WARN] [1508768455.123441445, 53.504000000]: Controller vs087/arm_controller failed with error code GOAL_TOLERANCE_VIOLATED
[ WARN] [1508768455.123507438, 53.504000000]: Controller handle vs087/arm_controller reports status ABORTED
This is the output of rostopic echo /vs087/arm_controller/state
which seems to be within the set goal tolerance (even though joint 2 has much higher error than the others, which is always the case)
error:
positions: [-1.760044554544038e-09, -0.001160522929334995, -9.655529709107213e-10, 1.208886324377545e-09, 3.4927438719023485e-10, 3.33908012351003e-10]
velocities: [-1.7600773791239206e-06, -1.1605230596161191, -9.655595170077191e-07, 1.209310674844269e-06, 3.492708007257761e-07, 3.342539573532484e-07]
Sometimes I also get an error:
Controller is taking too long to execute trajectory (the expected upper bound for the trajectory execution was 1.2 seconds). Stopping trajectory.
I am able to plan and execute in RViz most of times. What am I missing here? I think I have set the nexessary parameters according to previous similar issues.
I am able to plan and execute in RViz most of the times for random positions
Just noticed velocity error for joint 2 is always quite high
I'm facing the same problem. I noticed one thing though. On the topic
/<robot_namespace>/arm_controller/follow_joint_trajectory/goal
the values ofpath_tolerance
,goal_tolerance
andgoal_time_tolerance
are empty and not set by the server. Can you please check and confirm?What did you do @cgnarendiran ? I think I have exactly the same error.