Swerve Drive Kinematics

LiamC · February 23, 2020, 3:41am

We went to implement swerve kinematics/odometry and found that they aren’t included in the robotpy version of the wpilib.kinematics package. As they are in the C++ library, is this something that is currently planned on being added to robotpy, or should we start developing alternative methods of auto control/manipulating the mecanum kinematics?

virtuald · February 23, 2020, 3:50am

Glad you asked! When originally implemented, I didn’t have template support, so I didn’t include them. However, I’ve since added template support, so we can add them – I just neglected to do so.

How many wheels do you need?

virtuald · February 23, 2020, 3:54am

Also, I’ve opened a github issue to track this @ https://github.com/robotpy/robotpy-wpilib/issues/667

LiamC · February 23, 2020, 3:58am

Thanks! We use 4 modules

auscompgeek · February 23, 2020, 4:51am

If you don’t want to wait, I did port the swerve kinematics to pure-Python during the off-season, which you could copy-paste into your project:

github.com

auscompgeek/wpilib.kinematics/blob/master/wpilib/kinematics/swerve.py

import math
from dataclasses import dataclass
from typing import List, Optional

import numpy as np

from ..geometry import (
    Pose2d,
    Rotation2d,
    Translation2d,
    Twist2d,
    _identity_translation,
    _zero_rotation,
)
from .chassisspeeds import ChassisSpeeds


__all__ = ("SwerveModuleState", "SwerveDriveKinematics", "SwerveDriveOdometry")

This file has been truncated. show original

You’ll need to change the imports and potentially some method names. It also depends on numpy if you don’t already have it installed. It’ll will work for an arbitrary number of modules.

LiamC · February 23, 2020, 4:54am

That will let us start testing tomorrow. Thanks for sharing

virtuald · February 23, 2020, 6:40am

wpilib 2020.3.2.1 will be uploaded in a few minutes, and contains swerve module kinematics support. Let me know how it goes!

system · February 22, 2021, 6:42am

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