acmerobotics / road-runner

Wheeled mobile robot motion planning library designed for FTC
MIT License
209 stars 75 forks source link

The curve for x linear velocity from timeTrajectory from SplineTest is not smooth? #107

Open hallw opened 1 week ago

hallw commented 1 week ago

Tuning RoadRunner following the instructions, all seem went well until the final step with the SplineTest. The robot changes heading drastically for (~half a second) in the middle of movement. At the beginning I thought it is due to incorrect heading gain, but after I remove all gains (no feedback), I still see the same.

I finally decided to draw graph with command.linearVel.x.value(), command.linearVel.y.value() and command.angVel.value(), command being the return value of below:

        PoseVelocity2dDual<Time> command = new HolonomicController(
                PARAMS.axialGain, PARAMS.lateralGain, PARAMS.headingGain,
                PARAMS.axialVelGain, PARAMS.lateralVelGain, PARAMS.headingVelGain
        )
                .compute(txWorldTarget, pose, robotVelRobot);

In my understanding, since the trajectory is roughly a half circle, for most of the time x linear velocity should be a constant, except or a short period of ramp up and ramp down, while both y linear velocity and angular velocity should be both small. But what I'm surprised to see the curve for x linear velocity has a "sharp angle" in the middle, around when 2 spline path connects. I think this is causing the robot to change heading drastically?

The trajectory is like below (same as SplineTest from QuickStart except left/right flipped):

            drive.actionBuilder(beginPose)
                    .splineTo(new Vector2d(30, 30 * direction()), Math.PI / 2 * direction())
                    .splineTo(new Vector2d(0, 60 * direction()), Math.PI)
                    .build());

Screen recording of FTC Dashboard attached.

https://github.com/acmerobotics/road-runner/assets/12581739/0f119c07-ee1a-46e0-a7cd-d9a1915a2cc0