66 lines
2.8 KiB
C#
66 lines
2.8 KiB
C#
using UnityEngine;
|
|
|
|
namespace URDF
|
|
{
|
|
/// <summary>
|
|
/// Utility class for converting between URDF coordinate system (right-handed, Z-up)
|
|
/// and Unity coordinate system (left-handed, Y-up)
|
|
/// </summary>
|
|
public static class URDFCoordinateConverter
|
|
{
|
|
/// <summary>
|
|
/// Convert URDF position (right-handed, Z-up) to Unity position (left-handed, Y-up)
|
|
/// Conversion: (x, y, z) -> (x, -z, y)
|
|
/// </summary>
|
|
public static Vector3 ConvertPosition(Vector3 urdfPosition)
|
|
{
|
|
return new Vector3(urdfPosition.x, -urdfPosition.z, urdfPosition.y);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Convert URDF rotation (RPY in right-handed, Z-up) to Unity rotation (left-handed, Y-up)
|
|
/// </summary>
|
|
public static Quaternion ConvertRotation(Vector3 rpy)
|
|
{
|
|
// Convert RPY to Euler angles in Unity coordinate system
|
|
// RPY: Roll (around X), Pitch (around Y), Yaw (around Z)
|
|
// Unity: X, Y, Z rotations in left-handed system
|
|
|
|
// First convert RPY to quaternion in URDF system, then convert to Unity
|
|
Quaternion roll = Quaternion.AngleAxis(rpy.x * Mathf.Rad2Deg, Vector3.right);
|
|
Quaternion pitch = Quaternion.AngleAxis(rpy.y * Mathf.Rad2Deg, Vector3.up);
|
|
Quaternion yaw = Quaternion.AngleAxis(rpy.z * Mathf.Rad2Deg, Vector3.forward);
|
|
|
|
// In URDF: rotation order is typically Roll-Pitch-Yaw
|
|
Quaternion urdfRotation = yaw * pitch * roll;
|
|
|
|
// Convert from URDF (right-handed, Z-up) to Unity (left-handed, Y-up)
|
|
// This is a coordinate system transformation
|
|
Matrix4x4 urdfToUnity = Matrix4x4.TRS(Vector3.zero, Quaternion.identity, Vector3.one);
|
|
urdfToUnity.m00 = 1; urdfToUnity.m01 = 0; urdfToUnity.m02 = 0;
|
|
urdfToUnity.m10 = 0; urdfToUnity.m11 = 0; urdfToUnity.m12 = -1;
|
|
urdfToUnity.m20 = 0; urdfToUnity.m21 = 1; urdfToUnity.m22 = 0;
|
|
|
|
// Alternative approach: directly convert Euler angles
|
|
// URDF: X=right, Y=forward, Z=up
|
|
// Unity: X=right, Y=up, Z=forward
|
|
// So: Unity X = URDF X, Unity Y = URDF Z, Unity Z = -URDF Y
|
|
|
|
float unityX = rpy.x * Mathf.Rad2Deg; // Roll stays the same
|
|
float unityY = rpy.z * Mathf.Rad2Deg; // Yaw becomes Y rotation
|
|
float unityZ = -rpy.y * Mathf.Rad2Deg; // Pitch becomes -Z rotation
|
|
|
|
return Quaternion.Euler(unityX, unityY, unityZ);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Convert URDF axis (right-handed, Z-up) to Unity axis (left-handed, Y-up)
|
|
/// </summary>
|
|
public static Vector3 ConvertAxis(Vector3 urdfAxis)
|
|
{
|
|
return new Vector3(urdfAxis.x, -urdfAxis.z, urdfAxis.y);
|
|
}
|
|
}
|
|
}
|
|
|