Determining robot-relative velocity with odometry & field relative speeds on swerve

How would I go about determining robot-relative velocities, given that we know the angle of the robot through odometry/gyro, and the field-relative speeds, with swerve kinematics?

For example, if, from the driver station, the robot is facing left, and I push the left stick down so it travels straight towards me, I want it to read, i.e. 1 meter per second. I’d also want it to read 1m/s if I was traveling at a 45 degree angle, or 30, or whatever.

There is probably some math I’m missing here; any ideas?

(Another way to think about this would be determining the robot’s angular velocity relative to the goal. In that case, using Limelight measurements would be viable as well.)

EDIT: Actually, if you use forward kinematics to determine the chassis speeds, does vyMetersPerSecond represent this, relative to the robot frame?

4 Likes

Sorry, I might be reading this wrong. When you say “45 degree angle, or 30, or whatever”, are you saying that the robot is moving towards you while being pointed at an arbitrary direction, in which case that would be field-relative and you don’t need to convert to robot-relative velocities, or are you saying that it is moving towards an arbitrary direction at 1 meter per second, then you want the magnitude of the velocity (i.e. speed) regardless of the heading of the robot, which would simply be the square root of (Vx^2 + Vy^2).

What I’m looking for is the robot going in an arbitrary direction at an arbitrary speed, but determining, relative from the robot, how fast it is travelling to the side. This can also be thought of as determining the robot’s (angular?) velocity relative to the goal.

I’m not really looking to include moving AWAY from the goal into the calculation. That’s already being accounted for in what I’m attempting to do (shooting on the move).

OH, ok I think I got it. We’re trying to find the tangential velocity. Anyone feel free to correct me if I’m wrong. Using the angle between the horizontal and the positional vector of the robot relative to the target (which is in the center of the field), you can find a unit vector pointing in the tangential direction. You can then find the component of the field-relative velocity along that unit vector using the equation in the attached image. This is a theoretical approach, feel free to correct me if I made a mistake!
image

I’m not entirely sure of a way to implement that in Java code, as I don’t really know what those arrows mean. And honestly I don’t understand some of those words…

So for clarification:

  • Does “horizontal and positional vector” essentially mean the absolute distance of the robot to the center of the goal (i.e. from odometry)?
  • What do those arrows above a and b represent? How are the implemented in java?

NEVERMIND, I’ve got it. The trick is to take the field-relative speeds from forward kinematics (kinematics.toChassisSpeeds), and then pass the values from that into ChassisSpeeds.fromFieldRelativeSpeeds(). This gives you robot-relative speeds, taking into account the gyro angle.

Thanks for the help though @Inverse_Laplace!

4 Likes

Great to hear you figured it out!

1 Like

Keep in mind that this solution will not give you the actual velocities of the robot, since you’re getting the field relative velocities from your joystick inputs, not encoder velocities- it will only give you what you want/expect the robot to be doing

This is using kinematics.toChassisSpeeds() and passing in the module states with the encoder velocities, so it is the actual velocity. Are you thinking of using fromFieldRelativeSpeeds from joystick inputs? Because that’s the only thing really in the docs.

2 Likes

Yes, that’s what I thought you were doing for some reason.

Forward kinematics with kinematics.toChassisSpeeds should return robot relative speeds. The ChassisSpeeds object should always be used to represent robot relative speeds per the documentation.

You should only need to use forward kinematics to get what you want, no need to deal with field relative speeds.

That makes sense however I’ve been told it’s not the case. Considering that the gyro angle isn’t passed to toChassisSpeeds, it shouldn’t be field relative. I will probably have access to the robot again on Monday so I can test… I could also test in simulation tomorrow.

You’re correct- toChassisSpeeds is not field relative - that’s what is meant by “ChassisSpeeds”

You don’t need a gyro angle to determine chassis speeds from swerve module states. The swerve module states are also robot relative.

Yeah so I misread what I was told–either way it should be robot-relative, as the fromFieldRelativeSpeeds passed into inverse kinematics also return robot-relative speeds. So I don’t need the fromFieldRelativeSpeeds when determining robot velocity.

Also, I meant that you would need the gyro angle to be able to go back to field-relative speed.

1 Like

I want to confirm after reading over this topic. Would code like this would give me the “actual” field relative speed?

var chasisSpeeds = kinemetics.toChasisSpeeds(drivetrain.getModuleStates());
var fieldRelativeSpeed = ChasisSpeeds.fromFieldRelativeSpeeds(
        chasisSpeeds,
        drivetrain.getGyroAngle());

I ran a test and it seems to be right, but it’s difficult to confirm.

I want to use these values to reset the slew rate limiters when the teleop command starts, for cases when the robot is already moving. Teleop drive is field relative, so the rate limiters are field relative.

fromFieldRelativeSpeeds() converts from field relative to robot relative, not the other way around (atleast to my understanding).

If you want field relative velocities, one solution would be to use the SwerveOdometry class. Keep track of the previous robot position 20 ms prior, subtract it from the current position returned by the SwerveOdometry class, and divide by the elapsed time (20 ms). Though maybe someone else has an easier solution (possibly a method of some class I’m not aware of).

This is how I ended up doing it.

1 Like

Thanks! This took more brain power than it should have, but I think I got it. To go from field-relative to robot-relative fromFieldRelativeSpeeds rotates the vector clockwise.

Source from ChassisSpeeds:

  public static ChassisSpeeds fromFieldRelativeSpeeds(
      double vxMetersPerSecond,
      double vyMetersPerSecond,
      double omegaRadiansPerSecond,
      Rotation2d robotAngle) {
    return new ChassisSpeeds(
        vxMetersPerSecond * robotAngle.getCos() + vyMetersPerSecond * robotAngle.getSin(),
        -vxMetersPerSecond * robotAngle.getSin() + vyMetersPerSecond * robotAngle.getCos(),
        omegaRadiansPerSecond);
  }

Your code rotates the vector counterclockwise, that makes perfect sense since it’s the inverse. Using your method, my code looks like this:

    var chassisSpeeds = drivetrainSubsystem.getChassisSpeeds();
    var robotSpeeds = new ChassisSpeeds(
        chassisSpeeds.vxMetersPerSecond * robotAngle.getCos() - chassisSpeeds.vyMetersPerSecond * robotAngle.getSin(),
        chassisSpeeds.vyMetersPerSecond * robotAngle.getCos() + chassisSpeeds.vxMetersPerSecond * robotAngle.getSin(),
        chassisSpeeds.omegaRadiansPerSecond);

Would an alternate way to do this be to negate the robot angle passed to fromFieldRelativeSpeeds? I see how this might be confusing, given the name of the method, but would this be give the correct result:

The alternative solution you gave should work too. All fromFieldRelativeSpeeds does is multiply \begin{pmatrix} v_x\\ v_y \end{pmatrix} by the inverse of the rotation matrix. So passing in the opposite of the gyro angle should have the same effect as @swurl code.

1 Like

The original transformation is:

x cos θ + y sin θ

y cos θ - x sin θ

cos -θ = cos θ, and sin -θ = - sin θ

Therefore, plugging in -θ results in new transformation

x cos θ - y sin θ
x sin θ + y cos θ

So yes you are correct–good find!