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

Updates to make HumanoidControl work

Browse Source
This commit is contained in:
Pascal Serrarens 2025-06-03 17:18:38 +02:00
parent 345c705bd0
commit 7bd7931196
4 changed files with 39 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
LinearAlgebra/src/Direction.cs
View File

@ -3,8 +3,7 @@ using System;
using Vector3Float = UnityEngine.Vector3; using Vector3Float = UnityEngine.Vector3;
#endif #endif
namespace LinearAlgebra namespace LinearAlgebra {
{
/// <summary> /// <summary>
/// A direction in 3D space /// A direction in 3D space
@ -25,10 +24,10 @@ namespace LinearAlgebra
horizontal = 0; horizontal = 0;
vertical = 0; vertical = 0;
} }
public Direction(Angle horizontal, Angle vertical) { // public Direction(Angle horizontal, Angle vertical) {
this.horizontal = horizontal.inDegrees; // this.horizontal = horizontal.inDegrees;
} // }
// public Direction(float horizontal, float vertical) { // public Direction(float horizontal, float vertical) {
// this.horizontal = horizontal; // this.horizontal = horizontal;
// this.vertical = vertical; // this.vertical = vertical;
@ -40,7 +39,7 @@ namespace LinearAlgebra
horizontal = horizontal, horizontal = horizontal,
vertical = vertical vertical = vertical
}; };
//Normalize(); d.Normalize();
return d; return d;
} }
public static Direction Radians(float horizontal, float vertical) { public static Direction Radians(float horizontal, float vertical) {
@ -48,7 +47,7 @@ namespace LinearAlgebra
horizontal = horizontal * Angle.Rad2Deg, horizontal = horizontal * Angle.Rad2Deg,
vertical = vertical * Angle.Rad2Deg vertical = vertical * Angle.Rad2Deg
}; };
//Normalize(); d.Normalize();
return d; return d;
} }
@ -60,14 +59,15 @@ namespace LinearAlgebra
public readonly static Direction right = Degrees(90, 0); public readonly static Direction right = Degrees(90, 0);
public void Normalize() { public void Normalize() {
this.vertical = Angles.Normalize(this.vertical);
if (this.vertical > 90 || this.vertical < -90) { if (this.vertical > 90 || this.vertical < -90) {
this.horizontal += 180; this.horizontal += 180;
this.vertical = 180 - this.vertical; this.vertical = 180 - this.vertical;
} }
this.horizontal = Angles.Normalize(this.horizontal);
} }
public Vector3Float ToVector3() public Vector3Float ToVector3() {
{
float verticalRad = ((float)Math.PI / 2) - this.vertical * Angle.Deg2Rad; float verticalRad = ((float)Math.PI / 2) - this.vertical * Angle.Deg2Rad;
float horizontalRad = this.horizontal * Angle.Deg2Rad; float horizontalRad = this.horizontal * Angle.Deg2Rad;
float cosVertical = (float)Math.Cos(verticalRad); float cosVertical = (float)Math.Cos(verticalRad);

9
LinearAlgebra/src/SwingTwist.cs
View File

@ -28,18 +28,23 @@ namespace LinearAlgebra {
return r; return r;
} }
public static SwingTwist Degrees(float horizontalSwing, float verticalSwing, float twist) {
SwingTwist r = new(horizontalSwing, verticalSwing, twist);
return r;
}
#if UNITY_5_3_OR_NEWER #if UNITY_5_3_OR_NEWER
public static SwingTwist FromQuaternion(Quaternion q) { public static SwingTwist FromQuaternion(Quaternion q) {
// q.ToAngles(out float right, out float up, out float forward); // q.ToAngles(out float right, out float up, out float forward);
UnityEngine.Vector3 angles = q.eulerAngles; UnityEngine.Vector3 angles = q.eulerAngles;
SwingTwist r = new SwingTwist(angles.y, angles.x, angles.z); SwingTwist r = Degrees(angles.y, -angles.x, -angles.z);
return r; return r;
} }
public Quaternion ToQuaternion() { public Quaternion ToQuaternion() {
Quaternion q = Quaternion.Euler(-this.swing.vertical, Quaternion q = Quaternion.Euler(-this.swing.vertical,
this.swing.horizontal, this.swing.horizontal,
this.twist); -this.twist);
return q; return q;
} }
#endif #endif

23
src/Messages/LowLevelMessages.cs
View File

@ -34,25 +34,36 @@ namespace RoboidControl {
buffer[ix++] = (byte)qw; buffer[ix++] = (byte)qw;
} }
public static Quat32 ReceiveQuat32(byte[] data, ref byte ix) { public static Quat32 ReceiveQuat32(byte[] data, ref byte ix) {
Quat32 q = new Quat32( Quat32 q32 = new Quat32(
(data[ix++] - 128.0F) / 127.0F, (data[ix++] - 128.0F) / 127.0F,
(data[ix++] - 128.0F) / 127.0F, (data[ix++] - 128.0F) / 127.0F,
(data[ix++] - 128.0F) / 127.0F, (data[ix++] - 128.0F) / 127.0F,
data[ix++] / 255.0F); data[ix++] / 255.0F);
return q; System.Console.Write($"receive q32: {q32.x} {q32.y} {q32.z} {q32.w}");
return q32;
} }
public static void SendQuat32(byte[] buffer, ref byte ix, SwingTwist s) { public static void SendQuat32(byte[] buffer, ref byte ix, SwingTwist s) {
Quat32 q32 = Quat32.FromSwingTwist(s); Quat32 q32 = Quat32.FromSwingTwist(s);
System.Console.Write($"send q32: {q32.x} {q32.y} {q32.z} {q32.w}");
SendQuat32(buffer, ref ix, q32); SendQuat32(buffer, ref ix, q32);
}
public static void SendSwingTwist(byte[] buffer, ref byte ix, SwingTwist r) {
System.Console.Write($"send st: {r.swing.horizontal} {r.swing.vertical} {r.twist}");
SendAngle8(buffer, ref ix, r.swing.horizontal);
SendAngle8(buffer, ref ix, r.swing.vertical);
SendAngle8(buffer, ref ix, r.twist);
} }
public static SwingTwist ReceiveSwingTwist(byte[] data, ref byte ix) { public static SwingTwist ReceiveSwingTwist(byte[] data, ref byte ix) {
Quat32 q32 = ReceiveQuat32(data, ref ix); float horizontal = ReceiveAngle8(data, ref ix);
// UnityEngine.Quaternion q = new(q32.x, q32.y, q32.z, q32.w); float vertical = ReceiveAngle8(data, ref ix);
// SwingTwist r = new(q.eulerAngles.y, q.eulerAngles.x, q.eulerAngles.z); float twist = ReceiveAngle8(data, ref ix);
System.Console.Write($"receive st: {horizontal} {vertical} {twist}");
SwingTwist r = SwingTwist.FromQuat32(q32); SwingTwist r = SwingTwist.Degrees(horizontal, vertical, twist);
return r; return r;
} }

10
src/Messages/PoseMsg.cs
View File

@ -108,9 +108,10 @@ namespace RoboidControl {
if ((this.poseType & Pose_Position) != 0) if ((this.poseType & Pose_Position) != 0)
this.position = LowLevelMessages.ReceiveSpherical(buffer, ref ix); this.position = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
if ((this.poseType & Pose_Orientation) != 0) if ((this.poseType & Pose_Orientation) != 0)
this.orientation = SwingTwist.FromQuat32(LowLevelMessages.ReceiveQuat32(buffer, ref ix)); //this.orientation = SwingTwist.FromQuat32(LowLevelMessages.ReceiveQuat32(buffer, ref ix));
this.orientation = LowLevelMessages.ReceiveSwingTwist(buffer, ref ix);
if ((this.poseType & Pose_LinearVelocity) != 0) if ((this.poseType & Pose_LinearVelocity) != 0)
this.linearVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix); this.linearVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
if ((this.poseType & Pose_AngularVelocity) != 0) if ((this.poseType & Pose_AngularVelocity) != 0)
this.angularVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix); this.angularVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
} }
@ -132,9 +133,10 @@ namespace RoboidControl {
if ((poseType & Pose_Position) != 0) if ((poseType & Pose_Position) != 0)
LowLevelMessages.SendSpherical(buffer, ref ix, this.position); LowLevelMessages.SendSpherical(buffer, ref ix, this.position);
if ((poseType & Pose_Orientation) != 0) if ((poseType & Pose_Orientation) != 0)
LowLevelMessages.SendQuat32(buffer, ref ix, this.orientation); //LowLevelMessages.SendQuat32(buffer, ref ix, this.orientation);
LowLevelMessages.SendSwingTwist(buffer, ref ix, this.orientation);
if ((poseType & Pose_LinearVelocity) != 0) if ((poseType & Pose_LinearVelocity) != 0)
LowLevelMessages.SendSpherical(buffer, ref ix, this.linearVelocity); LowLevelMessages.SendSpherical(buffer, ref ix, this.linearVelocity);
if ((poseType & Pose_AngularVelocity) != 0) if ((poseType & Pose_AngularVelocity) != 0)
LowLevelMessages.SendSpherical(buffer, ref ix, this.angularVelocity); LowLevelMessages.SendSpherical(buffer, ref ix, this.angularVelocity);
return ix; return ix;