Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

chassis: Use pinv instead of solving least squares #76

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

chassis: Use pinv instead of solving least squares #76

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
5542a50
Pin wpilib-controller version
auscompgeek Jul 3, 2019
32b6a36
chassis: Use pinv instead of solving least squares
auscompgeek Jul 22, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 8 additions & 15 deletions pyswervedrive/chassis.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,8 +58,9 @@ def setup(self):
self.odometry_x_vel = 0
self.odometry_y_vel = 0
self.odometry_z_vel = 0
self.last_odometry_time = 0

self.A = np.array(
A = np.array(
[
[1, 0, 1],
[0, 1, 1],
Expand All @@ -73,19 +74,12 @@ def setup(self):
dtype=float,
)

self.last_odometry_time = 0
# wpilib.SmartDashboard.putData("heading_pid", self.heading_pid)

# figure out the contribution of the robot's overall rotation about the
# z axis to each module's movement, and encode that information in a

for i, module in enumerate(self.modules):
module_dist = math.hypot(module.x_pos, module.y_pos)
module_angle = math.atan2(module.y_pos, module.x_pos)
# self.z_axis_adjustment[i*2, 0] = -module_dist*math.sin(module_angle)
# self.z_axis_adjustment[i*2+1, 0] = module_dist*math.cos(module_angle)
self.A[i * 2, 2] = -module_dist * math.sin(module_angle)
self.A[i * 2 + 1, 2] = module_dist * math.cos(module_angle)
A[2 * i, 2] = -module.y_pos
A[2 * i + 1, 2] = module.x_pos

# Take the pseudoinverse of A to solve for robot movement from wheel odometry
self.A_inv = np.linalg.pinv(A)

def set_heading_sp_current(self):
self.set_heading_sp(self.imu.getAngle())
Expand Down Expand Up @@ -207,8 +201,7 @@ def update_odometry(self, *args):
self.last_odometry_time = now

def robot_movement_from_odometry(self, odometry_outputs, angle, z_vel=0):
lstsq_ret = np.linalg.lstsq(self.A, odometry_outputs, rcond=None)
x, y, theta = lstsq_ret[0].reshape(3)
x, y, theta = self.A_inv @ odometry_outputs
# TODO: re-enable if we move back to running in the same thread
x_field, y_field = self.field_orient(x, y, angle + z_vel * (1 / 200))
# x_field, y_field = self.field_orient(x, y, angle)
Expand Down
2 changes: 1 addition & 1 deletion requirements.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@ pyfrc ~= 2019.0.7
robotpy-ctre ~= 2019.2
robotpy-navx ~= 2019.0
robotpy-rev ~= 2019.0
wpilib-controller
wpilib-controller == 0.4.1
numpy ~= 1.15
dataclasses; python_version < '3.7'

Expand Down