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

HumanoidControl support

Browse Source
This commit is contained in:
Pascal Serrarens 2025-06-03 17:21:25 +02:00
parent 345c705bd0
commit 0353e6f2cf
5 changed files with 78 additions and 40 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);

11
LinearAlgebra/src/SwingTwist.cs
View File

@ -28,18 +28,25 @@ 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 = new(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);
System.Console.Write($"{q.eulerAngles}");
return q;
}
#endif

58
Unity/Thing.cs
View File

@ -238,24 +238,41 @@ namespace RoboidControl.Unity {
}
private void ScanForThings(Transform rootTransform) {
// Thing[] thingArray = allThings.ToArray();
RoboidControl.Thing[] thingArray = this.core.owner.things.ToArray();
// for (int thingIx = 0; thingIx < thingArray.Length; thingIx++) {
// Thing thing = thingArray[thingIx];
// GameObject foundObj = FindThingByName(thing, rootTransform);
// if (foundObj != null && foundObj != thing.gameObject) {
// Thing foundThing = foundObj.GetComponent<Thing>();
// if (foundThing == null) {
// allThings.Remove(thing);
for (int thingIx = 0; thingIx < thingArray.Length; thingIx++) {
RoboidControl.Thing thing = thingArray[thingIx];
GameObject foundObj = FindThingByName(thing, rootTransform);
if (foundObj != null && foundObj != thing.component.gameObject) {
Thing foundThing = foundObj.GetComponent<Thing>();
if (foundThing == null) {
Debug.Log($"move thing [{thing.owner.networkId}/{thing.id}] to {foundObj.name}");
foundThing = foundObj.AddComponent<Thing>();
foundThing.core = thing;
foundThing.core.position = LinearAlgebra.Spherical.FromVector3(foundObj.transform.localPosition);
foundThing.core.orientation = LinearAlgebra.SwingTwist.FromQuaternion(foundObj.transform.localRotation);
// foundThing = foundObj.AddComponent<Thing>();
// foundThing.networkId = thing.networkId;
// foundThing.objectId = thing.objectId;
// allThings.Add(foundThing);
// Destroy(thing.gameObject);
// }
// }
// }
Destroy(thing.component.gameObject);
thing.component = foundThing;
}
}
}
}
private GameObject FindThingByName(RoboidControl.Thing thing, Transform rootTransform) {
if (rootTransform == null || thing == null)
return null;
if (rootTransform.name == thing.name)
return rootTransform.gameObject;
for (int childIx = 0; childIx < rootTransform.childCount; childIx++) {
Transform child = rootTransform.GetChild(childIx);
GameObject foundObj = FindThingByName(thing, child);
if (foundObj != null)
return foundObj;
}
return null;
}
@ -266,10 +283,11 @@ namespace RoboidControl.Unity {
/// If a velocity is not zero, the position and/or orientation update will be ignored
protected virtual void HandlePose() {
this.transform.localRotation = core.orientation.ToQuaternion();
if (core.linearVelocity.distance == 0)
this.transform.localPosition = core.position.ToVector3();
if (core.angularVelocity.distance == 0)
this.transform.localRotation = core.orientation.ToQuaternion();
this.transform.localPosition = core.position.ToVector3();
// if (core.linearVelocity.distance == 0)
// this.transform.localPosition = core.position.ToVector3();
// if (core.angularVelocity.distance == 0)
// this.transform.localRotation = core.orientation.ToQuaternion();
}

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;
}

8
src/Messages/PoseMsg.cs
View File

@ -108,7 +108,8 @@ 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);
if ((this.poseType & Pose_AngularVelocity) != 0)
@ -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;