Help with kinematics (and eventually Odometry)

Hello,
I have spent a bit of time trying to create Kinematics objects in Python, and I am struggling with how to do so effectively.

I am wondering if anyone has gotten this to work yet, and/ or knows what I am missing. I have been through the documentation Here and looked at the example Here. I have also looked at the help files of each method, but am still doing some things wrong.

But I keep getting stuck. I am starting with a simple 2 wheel tank drive (with the hopes that it will work in sim) just to get my bearings.

Here is the code I have thus far…


import wpilib
from wpilib.kinematics import DifferentialDriveKinematics
from wpilib.kinematics import DifferentialDriveWheelSpeeds
from wpilib.kinematics import ChassisSpeeds
import wpilib.drive

if wpilib.RobotBase.isSimulation():
    is_sim = True
    import physics
    import time
else:
    is_sim = False


class MyRobot(wpilib.TimedRobot):
    """Main robot class"""

    def robotInit(self):
        """Robot-wide initialization code should go here"""

        self.lstick = wpilib.Joystick(0)
        self.rstick = wpilib.Joystick(1)

        self.l_motor = wpilib.Jaguar(1)
        self.r_motor = wpilib.Jaguar(2)

        # Position gets automatically updated as robot moves
        self.gyro = wpilib.AnalogGyro(1)


        self.drive = wpilib.drive.DifferentialDrive(self.l_motor, self.r_motor)

        self.motor = wpilib.Jaguar(4)

        self.limit1 = wpilib.DigitalInput(1)
        self.limit2 = wpilib.DigitalInput(2)

        self.position = wpilib.AnalogInput(2)

        self.Kine = DifferentialDriveKinematics(0.381 * 2)

        if is_sim:
            self.physics = physics.PhysicsEngine()
            self.last_tm = time.time()

    if is_sim:
        # TODO: this needs to be builtin
        def robotPeriodic(self):
            now = time.time()
            tm_diff = now - self.last_tm
            self.last_tm = now
            self.physics.update_sim(now, tm_diff)

    def autonomousInit(self):
        """Called when autonomous mode is enabled"""

        self.timer = wpilib.Timer()
        self.timer.start()

    def autonomousPeriodic(self):
        #wheelSpeeds = wpilib.kinematics.DifferentialDriveWheelSpeeds(2.0, 3.0)
        #ChassisSpeeds = DifferentialDriveKinematics.toChassisSpeeds(wheelSpeeds)
        ChassisSpeed = ChassisSpeeds(1.0, 0.0, 0.0)
        wheelSpeeds = self.Kine.toWheelSpeeds(ChassisSpeed)
        print(wheelSpeeds)

        if self.timer.get() < 2.0:
            self.drive.arcadeDrive(-1.0, 0)
        else:
            self.drive.arcadeDrive(0, 0)

    def teleopPeriodic(self):
        """Called when operation control mode is enabled"""

        self.drive.arcadeDrive(self.lstick.getY(), self.lstick.getX())
        #self.drive.arcadeDrive(self.lstick.getRawAxis(1), self.lstick.getRawAxis(3))

        # Move a motor with a Joystick
        y = self.rstick.getY()

        # stop movement backwards when 1 is on
        if self.limit1.get():
            y = max(0, y)

        # stop movement forwards when 2 is on
        if self.limit2.get():
            y = min(0, y)

        self.motor.set(y)


if __name__ == "__main__":

    wpilib.run(MyRobot)

And that runs an error…

TypeError: __init__(): incompatible constructor arguments. The following argument types are supported:
    1. wpilib.kinematics._kinematics.ChassisSpeeds()

Invoked with: 1.0, 0.0, 0.0

I am trying to invoke this only with constants first, but even that is not working. The ultimate goal is to get Odometry working on our mecha drive, but I wanted to start simply.

Thank you for reading,
Mr.R^2

@hamac2003

This is a known issue with a workaround.

My team is using the differential drive kinematics and odometry classes here, if you want to look at other team’s code: https://github.com/thedropbears/pyinfiniterecharge/tree/master/components/chassis.py

2 Likes

Also, the RobotPy generated documentation might be helpful for figuring out which functions are available (or not): https://robotpy.readthedocs.io/projects/wpilib/en/latest/wpilib.kinematics.html

1 Like

Thanks to both of you, I got both kinematics and Inverse Kinematics working in sim (sending constants for drive variables (left and right wheel speeds, and vx and omega for chassis speeds). I will change to sim values soon. This also helps greatly with Odometry.
The big issue I was having was not teasing out each of the four variables on input or output.
I can probably move some of the work (if not all) to its own method, but I wanted to make this simple for now.
My current working code is below.
Thank you all again.

Edit: I just updated my code to use actual drive variables, and it works in Sim:)

#!/usr/bin/env python3
import math
import wpilib
from wpilib.kinematics import DifferentialDriveKinematics
from wpilib.kinematics import DifferentialDriveWheelSpeeds
from wpilib.kinematics import ChassisSpeeds
import wpilib.drive

if wpilib.RobotBase.isSimulation():
    is_sim = True
    import physics
    import time
else:
    is_sim = False

GEAR_RATIO = 10.71

# measurements in metres
TRACK_WIDTH = 0.581  # theoretical as measured
WHEEL_CIRCUMFERENCE = 0.0254 * 6 * math.pi


class MyRobot(wpilib.TimedRobot):
    """Main robot class"""

    def robotInit(self):
        """Robot-wide initialization code should go here"""

        self.lstick = wpilib.Joystick(0)
        self.rstick = wpilib.Joystick(1)

        self.l_motor = wpilib.Jaguar(1)
        self.r_motor = wpilib.Jaguar(2)

        # Position gets automatically updated as robot moves
        self.gyro = wpilib.AnalogGyro(1)

        self.drive = wpilib.drive.DifferentialDrive(self.l_motor, self.r_motor)

        self.motor = wpilib.Jaguar(4)

        self.limit1 = wpilib.DigitalInput(1)
        self.limit2 = wpilib.DigitalInput(2)

        self.position = wpilib.AnalogInput(2)
        self.left_encoder = wpilib.Encoder(1, 2)
        self.right_encoder = wpilib.Encoder(3, 4)

        self.kinematics = DifferentialDriveKinematics(TRACK_WIDTH)
        self.chassis_speeds = ChassisSpeeds()
        self.chassis_speeds.vx = 0.0
        self.chassis_speeds.omega = 0.0

        if is_sim:
            self.physics = physics.PhysicsEngine()
            self.last_tm = time.time()

    if is_sim:
        # TODO: this needs to be builtin
        def robotPeriodic(self):
            now = time.time()
            tm_diff = now - self.last_tm
            self.last_tm = now
            self.physics.update_sim(now, tm_diff)

    def autonomousInit(self):
        """Called when autonomous mode is enabled"""

        self.timer = wpilib.Timer()
        self.timer.start()

    def autonomousPeriodic(self):
        # Get WheelSPeeds out of Inverse Kinematics...
        # XXX: https://github.com/robotpy/robotpy-wpilib/issues/635
        
        #speeds = self.kinematics.toWheelSpeeds(self.chassis_speeds)
        # Uncomment to see wheel speed
        # print(speeds.left,  speeds.right)

        # Get Chassis Speeds using Kinematics
        #sampleWheelSpeeds = DifferentialDriveWheelSpeeds()
        #sampleWheelSpeeds.left = 2.0
        #sampleWheelSpeeds.right = 2.0

        #speeds2 = self.kinematics.toChassisSpeeds(sampleWheelSpeeds)

        #print(speeds2.vx, speeds2.omega)

        if self.timer.get() < 2.0:
            self.chassis_speeds.vx = -.5
            
        else:
            self.chassis_speeds.vx = 0.0
        
        speeds = self.kinematics.toWheelSpeeds(self.chassis_speeds)
        self.drive.tankDrive(speeds.left, speeds.right)

    def teleopPeriodic(self):
        """Called when operation control mode is enabled"""

        self.drive.arcadeDrive(self.lstick.getY(), self.lstick.getX())
        # self.drive.arcadeDrive(self.lstick.getRawAxis(1), self.lstick.getRawAxis(3))

        # Move a motor with a Joystick
        y = self.rstick.getY()

        # stop movement backwards when 1 is on
        if self.limit1.get():
            y = max(0, y)

        # stop movement forwards when 2 is on
        if self.limit2.get():
            y = min(0, y)

        self.motor.set(y)


if __name__ == "__main__":

    wpilib.run(MyRobot)

The speeds you get from DifferentialDriveKinematics are velocities in metres per second. They cannot be used directly as a [-1,1] motor output.

You will want to perform a feedforward calculation in order to get a sensible motor throttle output.

1 Like

Oops. Thank you. I knew that. I do not know how I missed it/ why I did that directly. I must have been so excited to get it working, that I was not thinking clearly. I am so glad you caught it before I put it on the real robot. It would have saturated the motors instantly.

Also, thank you for sharing your implementation of the Rev PID in a drive base as we were having difficulty envisioning how to implement that successfully. We were going to use the Wpilib version, but would prefer the Rev.