search

acmerobotics / road-runner

Wheeled mobile robot motion planning library designed for FTC
MIT License
224 stars 77 forks source link

Mecanum Kinematics Seem Broken In X-Axis #4

Closed markdmatthews closed 5 years ago

markdmatthews commented 5 years ago

Hi - mentor for FTC#9929 here.

Our software team is experimenting with Road Runner (and the quickstart), and we're finding that when using SplineFollowOpMode everything works okay for point turns, splines and driving in the y axis with our mecanum drive base.

However, when the path includes movement in the x axis, the robot only moves forward, example a trajectory like:

Trajectory trajectory = new TrajectoryBuilder(new Pose2d(0, 0, 0), constraints)
.lineTo(new Vector2d(

The team added some logging of power, pose and positions to the mecanum drive adapter to road runner (which is just a copy of the one in the quickstart with the hardware map names changed to use our existing names).

What we notice is that even when the path should be completely in the x axis, the power sent to the drive base is all in the positive direction (lf=left front, lr = left rear, etc), when we'd expect the motors on each side to be counter-rotating, and thus have opposite power:

10-17 12:02:17.596 29117 29613 V Robocol : sending CMD_NOTIFY_INIT_OP_MODE(222), attempt: 0 10-17 12:02:18.121 29117 29622 D TNT : Trajectory duration: 0.8944271909999153 10-17 12:02:18.438 29117 29314 V Robocol : received command: CMD_RUN_OP_MODE(10094) Spline follow 10-17 12:02:18.441 29117 29313 I BatteryChecker: percent remaining: 12 is charging: false 10-17 12:02:18.452 29117 29622 D TNT : Pose: 0.0, 0.0, 0.0 10-17 12:02:18.464 29117 29622 D TNT : Power lf: 0.006261081282002851, lr: 0.006261081282002851, rr: 0.006261081282002851, rf:0.006261081282002851 10-17 12:02:18.567 29117 29622 D TNT : Pos: lf: 1, lr: 1, rr: -2, rf: 0

10-17 12:02:18.568 29117 29622 D TNT : Pose: 0.0, 0.0, 0.0 10-17 12:02:18.577 29117 29622 D TNT : Power lf: 0.09354983411813153, lr: 0.09354983411813153, rr: 0.09354983411813153, rf:0.09354983411813153 10-17 12:02:18.645 29117 29622 D TNT : Pos: lf: 4, lr: 2, rr: -1, rf: 2

10-17 12:02:18.647 29117 29622 D TNT : Pose: 0.0, 0.0, 0.0 10-17 12:02:18.653 29117 29622 D TNT : Power lf: 0.15320404791953038, lr: 0.15320404791953038, rr: 0.15320404791953038, rf:0.15320404791953038 10-17 12:02:18.706 29117 29622 D TNT : Pos: lf: 14, lr: 13, rr: 12, rf: 27 10-17 12:02:18.708 29117 29622 D TNT : Pose: 0.0, 0.0, 0.0

...

10-17 12:02:19.064 29117 29622 D TNT : Remaining duration: 0.8944271909999153 10-17 12:02:19.069 29117 29622 D TNT : Power lf: 0.2143364517184655, lr: 0.2143364517184655, rr: 0.2143364517184655, rf:0.2143364517184655 10-17 12:02:19.117 29117 29622 D TNT : Pos: lf: 424, lr: 422, rr: 417, rf: 427 10-17 12:02:19.119 29117 29622 D TNT : Pose: 11.003564728653075, 0.045831693693380166, 1.3877787807814457E-17 ...

10-17 12:02:19.235 29117 29622 D TNT : Power lf: 0.0864297984367916, lr: 0.0864297984367916, rr: 0.0864297984367916, rf:0.0864297984367916 10-17 12:02:19.286 29117 29622 D TNT : Pos: lf: 496, lr: 489, rr: 481, rf: 491 10-17 12:02:19.288 29117 29622 D TNT : Pose: 12.7638536488942, 0.2936907325589464, -0.016666666666666663

We can use our existing tele-op and autonomous code to strafe with the drive base, so we're pretty sure it's all wired correctly.

For reference, here are the constraints that we've used in SplineFollowOpMode (track width is 18, wheel base is 12, we're using a kV of .01283 as calibrated by the FeedForward tuner in the quick start):

DriveConstraints baseConstraints = new DriveConstraints(40.0, 60.0, Math.PI / 2, Math.PI / 2);
MecanumConstraints constraints = new MecanumConstraints(baseConstraints, drive.getTrackWidth(), drive.getWheelBase());
rbrott commented 5 years ago

Hello!

I'm glad to see you're trying out road runner. Road runner's convention is that the x-axis of the robot points forward and the y-axis points left with the origin at the center of the robot. By default, it assumes that all movements are straight. To strafe, you must use the constant heading interpolator like so:

Trajectory trajectory = new TrajectoryBuilder(new Pose2d(0, 0, 0), constraints)
    .lineTo(new Vector2d(0, 20), new ConstantInterpolator(0))
    .build()

This may seem like a bit of boilerplate (in fact, I may add some methods to TrajectoryBuilder for this purpose) but it facilitates more complicated maneuvers (e.g., turning while traveling in a line).

markdmatthews commented 5 years ago

Thanks for the clarification. I'll have the team test this and figure out our own wrapper to keep x, y consistent with our model, and maybe reduce some of the boilerplate!