Fix for reject waypoint trajectories mismatch joints

[hello-binit]

When a joint is setting up execution of a waypoint trajectory, it will move to the first point of the trajectory using move_to() if follow_trajectory(move_to_first_point=True). move_to_first_point defaults to true. But it takes some time for the joint to reach the first point. With motion planning, you often incoporate the joint's initial position into the optimization. Furthermore, for model predictive control, you want the robot to start tracking the trajectory immediately. So you'd set move_to_first_point=False.

There is an bug where the user-provided trajectory's first waypoint has a position that doesn't match the joint's actual position. In this bug, the trajectory is still accepted and the joint begins servo-ing fast to track the trajectory. Often, servo-ing fast causes the motor's current draw to spike and this trips motor protection filters at the low level. To the user, this looks like the joint jerks forward and suddenly stops. In this fix, we check that the first waypoint's position is within tolerance of the actual position of the joint, and reject the trajectory if it is not.

Part of the reason for this bug is that the firmware was handling this check in TrajectoryManager.is_segment_valid(), but that logic was eventually commented out because the extra computational time of the check caused communication with the uC to stutter.

Added two additional fixes while writing unit tests:

• req_calibration shouldn't be an argument of Dynamixels. It's part of the parameters
• Trajectories are cancelled if any joint reports issue with initiating tracking.

Testing

Use python3 -m unittest test.test_robot. Tested on 3004.