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: pyinfiniterecharge/components/chassis.py at master · thedropbears/pyinfiniterecharge · GitHub

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.

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.