RoboidControl/RoboidControl-csharp
RoboidControl/RoboidControl-csharp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Migrating from ControlCore to RoboidControl

Browse Source
This commit is contained in:
Pascal Serrarens 2025-04-07 09:36:22 +02:00
parent 390f7807cd
commit d82388fc45
5 changed files with 155 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
Examples/BB2B/Program.cs
View File

@ -1,31 +0,0 @@
using System.Threading;
using RoboidControl;
class BB2B {
static void Main() {
// The robot's propulsion is a differential drive
DifferentialDrive bb2b = new();
// It has a touch sensor at the front left of the roboid
TouchSensor touchLeft = new(bb2b);
// and other one on the right
TouchSensor touchRight = new(bb2b);
// Do forever:
while (true) {
// The left wheel turns forward when nothing is touched on the right side
// and turn backward when the roboid hits something on the right
float leftWheelSpeed = touchRight.touchedSomething ? -600.0f : 600.0f;
// The right wheel does the same, but instead is controlled by
// touches on the left side
float rightWheelSpeed = touchLeft.touchedSomething ? -600.0f : 600.0f;
// When both sides are touching something, both wheels will turn backward
// and the roboid will move backwards
bb2b.SetWheelVelocity(leftWheelSpeed, rightWheelSpeed);
// Update the roboid state
bb2b.Update(true);
// and sleep for 100ms
Thread.Sleep(100);
}
}
}

185
LinearAlgebra/src/Matrix.cs
View File

@ -10,9 +10,9 @@ namespace LinearAlgebra
public readonly struct Slice
{
public uint start { get; }
public uint stop { get; }
public Slice(uint start, uint stop)
public int start { get; }
public int stop { get; }
public Slice(int start, int stop)
{
this.start = start;
this.stop = stop;
@ -23,10 +23,10 @@ namespace LinearAlgebra
{
public float[,] data { get; }
public uint nRows => (uint)data.GetLength(0);
public uint nCols => (uint)data.GetLength(1);
public int nRows => data.GetLength(0);
public int nCols => data.GetLength(1);
public Matrix2(uint nRows, uint nCols)
public Matrix2(int nRows, int nCols)
{
this.data = new float[nRows, nCols];
}
@ -46,7 +46,7 @@ namespace LinearAlgebra
return new Matrix2(data);
}
public static Matrix2 Zero(uint nRows, uint nCols)
public static Matrix2 Zero(int nRows, int nCols)
{
return new Matrix2(nRows, nCols);
}
@ -60,11 +60,11 @@ namespace LinearAlgebra
return new Matrix2(result);
}
public static Matrix2 Identity(uint size)
public static Matrix2 Identity(int size)
{
return Diagonal(1, size);
}
public static Matrix2 Identity(uint nRows, uint nCols)
public static Matrix2 Identity(int nRows, int nCols)
{
Matrix2 m = Zero(nRows, nCols);
m.FillDiagonal(1);
@ -78,7 +78,7 @@ namespace LinearAlgebra
resultData[ix, ix] = v.data[ix];
return new Matrix2(resultData);
}
public static Matrix2 Diagonal(float f, uint size)
public static Matrix2 Diagonal(float f, int size)
{
float[,] resultData = new float[size, size];
for (int ix = 0; ix < size; ix++)
@ -87,13 +87,13 @@ namespace LinearAlgebra
}
public void FillDiagonal(Matrix1 v)
{
uint n = Math.Min(Math.Min(this.nRows, this.nCols), v.size);
int n = (int)Math.Min(Math.Min(this.nRows, this.nCols), v.size);
for (int ix = 0; ix < n; ix++)
this.data[ix, ix] = v.data[ix];
}
public void FillDiagonal(float f)
{
uint n = Math.Min(this.nRows, this.nCols);
int n = Math.Min(this.nRows, this.nCols);
for (int ix = 0; ix < n; ix++)
this.data[ix, ix] = f;
}
@ -108,9 +108,17 @@ namespace LinearAlgebra
return new Matrix2(result);
}
public Matrix1 GetRow(int rowIx) {
float[] row = new float[this.nCols];
for (int colIx = 0; colIx < this.nCols; colIx++) {
row[colIx] = this.data[rowIx, colIx];
}
return new Matrix1(row);
}
#if UNITY_5_3_OR_NEWER
public Vector3Float GetRow3(int rowIx) {
uint cols = this.nCols;
int cols = this.nCols;
Vector3Float row = new() {
x = this.data[rowIx, 0],
y = this.data[rowIx, 1],
@ -131,6 +139,14 @@ namespace LinearAlgebra
this.data[rowIx, 2] = v.z;
}
public void SwapRows(int row1, int row2) {
for (uint ix = 0; ix < this.nCols; ix++) {
float temp = this.data[row1, ix];
this.data[row1, ix] = this.data[row2, ix];
this.data[row2, ix] = temp;
}
}
public Matrix1 GetColumn(int colIx)
{
float[] column = new float[this.nRows];
@ -140,6 +156,15 @@ namespace LinearAlgebra
}
return new Matrix1(column);
}
public void SetColumn(int colIx, Matrix1 v) {
for (uint ix = 0; ix < v.size; ix++)
this.data[ix, colIx] = v.data[ix];
}
public void SetColumn(int colIx, Vector3Float v) {
this.data[0, colIx] = v.x;
this.data[1, colIx] = v.y;
this.data[2, colIx] = v.z;
}
public static bool AllClose(Matrix2 A, Matrix2 B, float atol = 1e-08f)
{
@ -302,18 +327,37 @@ namespace LinearAlgebra
return new Matrix2(result);
}
public Matrix2 GetRows(Slice slice) {
return GetRows(slice.start, slice.stop);
}
public Matrix2 GetRows(int from, int to) {
if (from < 0 || to >= this.nRows)
throw new System.ArgumentException("Slice index out of range.");
float[,] result = new float[to - from, this.nCols];
int resultRowIx = 0;
for (int rowIx = from; rowIx < to; rowIx++) {
for (int colIx = 0; colIx < this.nCols; colIx++)
result[resultRowIx, colIx] = this.data[rowIx, colIx];
resultRowIx++;
}
return new Matrix2(result);
}
public Matrix2 Slice(Slice slice)
{
return Slice(slice.start, slice.stop);
}
public Matrix2 Slice(uint from, uint to)
public Matrix2 Slice(int from, int to)
{
if (from < 0 || to >= this.nRows)
throw new System.ArgumentException("Slice index out of range.");
float[,] result = new float[to - from, this.nCols];
int resultRowIx = 0;
for (uint rowIx = from; rowIx < to; rowIx++)
for (int rowIx = from; rowIx < to; rowIx++)
{
for (int colIx = 0; colIx < this.nCols; colIx++)
{
@ -328,39 +372,42 @@ namespace LinearAlgebra
{
return Slice((rowRange.start, rowRange.stop), (colRange.start, colRange.stop));
}
public Matrix2 Slice((uint start, uint stop) rowRange, (uint start, uint stop) colRange)
public Matrix2 Slice((int start, int stop) rowRange, (int start, int stop) colRange)
{
float[,] result = new float[rowRange.stop - rowRange.start, colRange.stop - colRange.start];
uint resultRowIx = 0;
uint resultColIx = 0;
for (uint i = rowRange.start; i < rowRange.stop; i++)
int resultRowIx = 0;
int resultColIx = 0;
for (int i = rowRange.start; i < rowRange.stop; i++)
{
for (uint j = colRange.start; j < colRange.stop; j++)
for (int j = colRange.start; j < colRange.stop; j++)
result[resultRowIx, resultColIx] = this.data[i, j];
}
return new Matrix2(result);
}
public void UpdateSlice(Slice slice, Matrix2 m)
{
public void UpdateSlice(Slice slice, Matrix2 m) {
UpdateSlice((slice.start, slice.stop), m);
}
public void UpdateSlice((int start, int stop) slice, Matrix2 m) {
// if (slice.start == slice.stop)
// Console.WriteLine("WARNING: no data is updates when start equals stop in a slice!");
int mRowIx = 0;
for (uint rowIx = slice.start; rowIx < slice.stop; rowIx++, mRowIx++)
{
for (int rowIx = slice.start; rowIx < slice.stop; rowIx++, mRowIx++) {
for (int colIx = 0; colIx < this.nCols; colIx++)
this.data[rowIx, colIx] = m.data[mRowIx, colIx];
}
}
public void UpdateSlice(Slice rowRange, Slice colRange, Matrix2 m)
{
UpdateSlice((rowRange.start, rowRange.stop), (colRange.start, colRange.stop), m);
}
public void UpdateSlice((uint start, uint stop) rowRange, (uint start, uint stop) colRange, Matrix2 m)
public void UpdateSlice((int start, int stop) rowRange, (int start, int stop) colRange, Matrix2 m)
{
for (uint i = rowRange.start; i < rowRange.stop; i++)
for (int i = rowRange.start; i < rowRange.stop; i++)
{
for (uint j = colRange.start; j < colRange.stop; j++)
for (int j = colRange.start; j < colRange.stop; j++)
this.data[i, j] = m.data[i - rowRange.start, j - colRange.start];
}
}
@ -369,7 +416,7 @@ namespace LinearAlgebra
{
Matrix2 A = this;
// unchecked
uint n = A.nRows;
int n = A.nRows;
// Create an identity matrix of the same size as the original matrix
float[,] augmentedMatrix = new float[n, 2 * n];
@ -404,10 +451,10 @@ namespace LinearAlgebra
}
// Back substitution
for (uint i = n - 1; i >= 0; i--)
for (int i = n - 1; i >= 0; i--)
{
// Eliminate the column above the pivot
for (uint j = i - 1; j >= 0; j--)
for (int j = i - 1; j >= 0; j--)
{
float factor = augmentedMatrix[j, i];
for (int k = 0; k < 2 * n; k++)
@ -428,7 +475,7 @@ namespace LinearAlgebra
public float Determinant()
{
uint n = this.nRows;
int n = this.nRows;
if (n != this.nCols)
throw new System.ArgumentException("Matrix must be square.");
@ -448,7 +495,7 @@ namespace LinearAlgebra
// Helper function to compute the minor of a matrix
private Matrix2 Minor(int rowToRemove, int colToRemove)
{
uint n = this.nRows;
int n = this.nRows;
float[,] minor = new float[n - 1, n - 1];
int r = 0, c = 0;
@ -469,15 +516,46 @@ namespace LinearAlgebra
return new Matrix2(minor);
}
public static Matrix2 DeleteRows(Matrix2 A, Slice rowRange) {
float[,] result = new float[A.nRows - (rowRange.stop - rowRange.start), A.nCols];
int resultRowIx = 0;
for (int i = 0; i < A.nRows; i++) {
if (i >= rowRange.start && i < rowRange.stop)
continue;
for (int j = 0; j < A.nCols; j++)
result[resultRowIx, j] = A.data[i, j];
resultRowIx++;
}
return new Matrix2(result);
}
internal static Matrix2 DeleteColumns(Matrix2 A, Slice colRange) {
float[,] result = new float[A.nRows, A.nCols - (colRange.stop - colRange.start)];
for (int i = 0; i < A.nRows; i++) {
int resultColIx = 0;
for (int j = 0; j < A.nCols; j++) {
if (j >= colRange.start && j < colRange.stop)
continue;
result[i, resultColIx++] = A.data[i, j];
}
}
return new Matrix2(result);
}
}
public class Matrix1
{
public float[] data { get; }
public uint size => (uint)data.GetLength(0);
public int size => data.GetLength(0);
public Matrix1(uint size)
public Matrix1(int size)
{
this.data = new float[size];
}
@ -487,7 +565,7 @@ namespace LinearAlgebra
this.data = data;
}
public static Matrix1 Zero(uint size)
public static Matrix1 Zero(int size)
{
return new Matrix1(size);
}
@ -604,6 +682,18 @@ namespace LinearAlgebra
return result;
}
public static Matrix1 operator -(Matrix1 A, Matrix1 B) {
if (A.size != B.size)
throw new System.ArgumentException("Size of A must match size of B.");
float[] result = new float[A.size];
for (int i = 0; i < A.size; i++) {
result[i] = A.data[i] - B.data[i];
}
return new Matrix1(result);
}
public static Matrix1 operator *(Matrix1 A, float f)
{
float[] result = new float[A.size];
@ -618,18 +708,35 @@ namespace LinearAlgebra
return A * f;
}
public static Matrix1 operator /(Matrix1 A, float f) {
float[] result = new float[A.size];
for (int i = 0; i < A.size; i++)
result[i] = A.data[i] / f;
return new Matrix1(result);
}
public static Matrix1 operator /(float f, Matrix1 A) {
float[] result = new float[A.size];
for (int i = 0; i < A.size; i++)
result[i] = f / A.data[i];
return new Matrix1(result);
}
public Matrix1 Slice(Slice range)
{
return Slice(range.start, range.stop);
}
public Matrix1 Slice(uint from, uint to)
public Matrix1 Slice(int from, int to)
{
if (from < 0 || to >= this.size)
throw new System.ArgumentException("Slice index out of range.");
float[] result = new float[to - from];
int resultIx = 0;
for (uint ix = from; ix < to; ix++)
for (int ix = from; ix < to; ix++)
result[resultIx++] = this.data[ix];
return new Matrix1(result);
@ -637,7 +744,7 @@ namespace LinearAlgebra
public void UpdateSlice(Slice slice, Matrix1 v)
{
int vIx = 0;
for (uint ix = slice.start; ix < slice.stop; ix++, vIx++)
for (int ix = slice.start; ix < slice.stop; ix++, vIx++)
this.data[ix] = v.data[vIx];
}
}

14
LinearAlgebra/src/Quaternion.cs
View File

@ -3,24 +3,26 @@ using System;
using Quaternion = UnityEngine.Quaternion;
#endif
namespace LinearAlgebra
{
namespace LinearAlgebra {
public class QuaternionOf<T>
{
public class QuaternionOf<T> {
public T x;
public T y;
public T z;
public T w;
public QuaternionOf(T x, T y, T z, T w)
{
public QuaternionOf(T x, T y, T z, T w) {
this.x = x;
this.y = y;
this.z = z;
this.w = w;
}
public static Quaternion Reflect(Quaternion q) {
return new(-q.x, -q.y, -q.z, q.w);
}
#if UNITY_5_3_OR_NEWER
public static Matrix2 ToRotationMatrix(Quaternion q) {
float w = q.x, x = q.y, y = q.z, z = q.w;

1
Unity/TouchSensor.cs
View File

@ -1,5 +1,4 @@
#if UNITY_5_3_OR_NEWER
using Unity.VisualScripting;
using UnityEngine;
namespace RoboidControl.Unity {

2
src/Things/TemperatureSensor.cs
View File

@ -17,7 +17,7 @@ namespace RoboidControl {
/// <param name="participant">The participant for with the sensor is needed</param>
/// <param name="networkId">The network ID of the sensor</param>
/// <param name="thingId">The ID of the thing</param>
public TemperatureSensor(LocalParticipant participant, byte networkId, byte thingId) : base(participant, networkId, thingId, (byte)Type.TemperatureSensor) { }
public TemperatureSensor(Participant participant, byte networkId, byte thingId) : base(participant, networkId, thingId, (byte)Type.TemperatureSensor) { }
/// <summary>
/// Function to extract the temperature received in the binary message