Invalid Trajectory: start point deviates from current robot state more than 0.01

[dan9thsense]

In branch 2024_dev, I get the error in the title about 80% of the time that I try to move to specified joints using this method from the tutorials. The method generally works when used right at the start of the simulation. But once the floor robot arm is moved, then trying to use this method causes the error to occur.

For example, when the current elbow joint is at 1.57 and I try to move it to 1.0, the error indicates that it is taking the goal (1.0) as the current start point. I saw similar behavior with Cartesian goals and fixed it by using the current pose as the first waypoint. But I don't know how to fix it here.

Invalid Trajectory: start point deviates from current robot state more than 0.01 joint 'floor_elbow_joint': expected: 1.57, current: 0.99786

def floor_robot_move_joints_dict(self, dict_positions : dict):
    new_joint_position = self._create_floor_joint_position_dict(dict_positions)
    with self._planning_scene_monitor.read_write() as scene:
        self._floor_robot.set_start_state_to_current_state()
        scene.current_state.joint_positions = new_joint_position
        joint_constraint = construct_joint_constraint(
                robot_state=scene.current_state,
                joint_model_group=self._ariac_robots.get_robot_model().get_joint_model_group("floor_robot"),
        )
        self._floor_robot.set_goal_state(motion_plan_constraints=[joint_constraint])
    self._plan_and_execute(self._ariac_robots,self._floor_robot, self.get_logger())

[jfernandez37]

For some reason, I could not get the self._floor_robot.set_start_state_to_current_state() working. It has now been updated to self._floor_robot.set_start_state(robot_state = scene.current_state). This seemed to fix the issue.

[dan9thsense]

That fixed it. Thanks.

[dan9thsense]

The problem has returned whenever the arm gets bumped or deviated from its path, for example, by pushing down too hard when trying to pick up a part. Is there a way to reset the state or something so that it recognizes where the current joints actually are? Alternatively, is there a way to turn off this check and/or increase the tolerance? Here is an example:

Invalid Trajectory: start point deviates from current robot state more than 0.01 joint 'linear_actuator_joint': expected: -2.44761, current: -2.43541

[jaybrecht]

Just to clarify you have this issue even when adding code like this before planning the trajectory?

with self._planning_scene_monitor.read_write() as scene:
    self._floor_robot.set_start_state(robot_state=scene.current_state)

[dan9thsense]

Yes. That fixed it before but when the arm has been jostled, it doesn't work.

[dan9thsense]

Here is a discussion about it, but no resolution: https://github.com/jacknlliu/development-issues/issues/67

[dan9thsense]

Along the same lines, when the arm is jostled, this error sometimes appears. Of course, the best thing is to not jostle the arm, but the problem is that, when you occasionally do, there seems to be no way to clear the error. How do you get the gripper back within limits if being out of limits makes it refuse to move? I cannot find a way to set the 'start_state_max_bounds_error' parameter at all, much less while the sim is running.

Joint 'floor_gripper_joint' from the starting state is outside bounds by a significant margin: [0.222733 ] should be in the range [-0.001 ], [0.001 ] but the error above the 'start_state_max_bounds_error' parameter (currently set to 0.050000)

[jaybrecht]

Hi Dan, sorry for the delayed response. I was digging through the moveit_py documentation and saw there is a method for the PlanningSceneMonitorobject called wait_for_current_robot_state(). I don't know if it would fix your issue but I would try adding it like this. This might help make sure that the values from the joint state topic are up to date before setting the initial state.

with self._planning_scene_monitor.read_write() as scene:
    scene.wait_for_current_robot_state()
    self._floor_robot.set_start_state(robot_state=scene.current_state)

Another possible suggestion I have is, if you are running your system similarly to ariac_tutorials, changing the executor to be of type SingleThreadedExecutor() instead of MultiThreadedExecutor(). I have recently discovered that the multi-threaded executor can occasionally lead to topics not receiving callbacks at a consistent rate, especially on faster systems.

[dan9thsense]

That's a good find; I will try it.

Yes, the ROS2 approach to threading is, simply put, poor. The issue with blocking causes all sorts of problems.

[jaybrecht]

@dan9thsense Any update on this? Does waiting for the current state fix the start point deviation error?

[dan9thsense]

Waiting blocks right now. I have to learn more about executors.......

[zeidk]

is self._planning_scene_monitor a class member that you initialize in the init? You can try to put it in a separate mutually exclusive callback group and see if see solves the issue. It seems this a synchronous execution which is blocking the current thread.

[dan9thsense]

OK, I'll try that, thanks. Or maybe put the wait in its own executor?

[dan9thsense]

Since the call is to Moveit, based on the original Moveit call (MoveItPy) where MoveIt creates a node, I don't see a way to put that in a callback group that I make.

[jaybrecht]

Dan, can you try changing the start state tolerance directly in the MoveIt config and see if that fixes the error?

[jaybrecht]

Actually it might be this part of the launch file that needs to be changed

[dan9thsense]

I think that will help. I'll try it. So far, I have not found another solution.

[jaybrecht]

I have been doing some work with MoveItPy myself recently and I found that it was difficult to not jostle the robot when doing full speed cartesian motions. Looking into it, the most recent version of MoveItPy has added the ability to scale trajectories with time-optimal trajectory generation (TOTG) which is a feature I used extensively for nist_competitor. It also has the TrajectoryExecutionManager which allows for async execution of trajectories which is very helpful for pick operations. I was able to build the current version of MoveIt from source on Iron. Would you be interested in using these features? I think it could help with not jostling the arm as much. If so we can look at how to properly integrate this with the automated evaluation. We may create a separate image that has MoveIt built from source and competitors can select that image in the evaluation configuration file.

[dan9thsense]

That's interesting. Now that it's April and quals are just a few weeks away, I'm focusing on testing and debugging rather than introducing new features or approaches. Those are good things for next year. Also, given the track record of both ROS2 and MoveItPy, I'd be concerned about some subtle bug rearing its head at the last minute.

[dan9thsense]

However, I will try changing the tolerance and see if that helps. I expect it will.