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

9
LinearAlgebra/src/SwingTwist.cs
View File

@ -28,18 +28,23 @@ namespace LinearAlgebra {
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
public static SwingTwist FromQuaternion(Quaternion q) {
// q.ToAngles(out float right, out float up, out float forward);
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;
}
public Quaternion ToQuaternion() {
Quaternion q = Quaternion.Euler(-this.swing.vertical,
this.swing.horizontal,
this.twist);
-this.twist);
return q;
}
#endif

23
src/Messages/LowLevelMessages.cs
View File

@ -34,25 +34,36 @@ namespace RoboidControl {
buffer[ix++] = (byte)qw;
}
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++] / 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) {
Quat32 q32 = Quat32.FromSwingTwist(s);
System.Console.Write($"send q32: {q32.x} {q32.y} {q32.z} {q32.w}");
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) {
Quat32 q32 = ReceiveQuat32(data, ref ix);
// UnityEngine.Quaternion q = new(q32.x, q32.y, q32.z, q32.w);
// SwingTwist r = new(q.eulerAngles.y, q.eulerAngles.x, q.eulerAngles.z);
float horizontal = ReceiveAngle8(data, ref ix);
float vertical = ReceiveAngle8(data, ref ix);
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;
}

10
src/Messages/PoseMsg.cs
View File

@ -108,9 +108,10 @@ namespace RoboidControl {
if ((this.poseType & Pose_Position) != 0)
this.position = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
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)
this.linearVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
this.linearVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
if ((this.poseType & Pose_AngularVelocity) != 0)
this.angularVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
}
@ -132,9 +133,10 @@ namespace RoboidControl {
if ((poseType & Pose_Position) != 0)
LowLevelMessages.SendSpherical(buffer, ref ix, this.position);
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)
LowLevelMessages.SendSpherical(buffer, ref ix, this.linearVelocity);
LowLevelMessages.SendSpherical(buffer, ref ix, this.linearVelocity);
if ((poseType & Pose_AngularVelocity) != 0)
LowLevelMessages.SendSpherical(buffer, ref ix, this.angularVelocity);
return ix;