From 052575017a6cdc7b0a65df993af5e17398d5fbfd Mon Sep 17 00:00:00 2001
From: Leni Aniva <v@leni.sh>
Date: Thu, 18 Jul 2024 14:09:53 -0700
Subject: [PATCH] feat: Rotated wrist joint on left side

---
 nhf/touhou/houjuu_nue/wing.py |  6 +++++-
 nhf/utils.py                  | 16 +++++++++++-----
 2 files changed, 16 insertions(+), 6 deletions(-)

diff --git a/nhf/touhou/houjuu_nue/wing.py b/nhf/touhou/houjuu_nue/wing.py
index 9df38e6..a1d5d37 100644
--- a/nhf/touhou/houjuu_nue/wing.py
+++ b/nhf/touhou/houjuu_nue/wing.py
@@ -88,7 +88,7 @@ class WingProfile(Model):
     elbow_height: float
     wrist_bot_loc: Cq.Location
     wrist_height: float
-    elbow_rotate: float = -5
+    elbow_rotate: float = -5.0
     wrist_rotate: float = 30.0
 
     flip: bool = False
@@ -842,6 +842,10 @@ class WingL(WingProfile):
     def __post_init__(self):
         assert self.wrist_height <= self.shoulder_joint.height
         self.wrist_bot_loc = self.wrist_bot_loc.with_angle_2d(self.wrist_angle)
+        self.elbow_joint.angle_neutral = -15.0
+        self.elbow_rotate = 5.0
+        self.wrist_joint.angle_neutral = -self.wrist_bot_loc.to2d_rot() - 30.0
+        self.wrist_rotate = self.wrist_joint.angle_neutral
 
         super().__post_init__()
 
diff --git a/nhf/utils.py b/nhf/utils.py
index 7cd0d42..f542632 100644
--- a/nhf/utils.py
+++ b/nhf/utils.py
@@ -67,13 +67,19 @@ def to2d_pos(self: Cq.Location) -> Tuple[float, float]:
     """
     Returns position and angle
     """
-    (x, y, z), (rx, ry, _) = self.toTuple()
-    assert z == 0
-    assert rx == 0
-    assert ry == 0
-    return (x, y)
+    (x, y), _ = self.to2d()
+    return x, y
 Cq.Location.to2d_pos = to2d_pos
 
+def to2d_rot(self: Cq.Location) -> Tuple[float, float]:
+    """
+    Returns position and angle
+    """
+    _, r = self.to2d()
+    return r
+Cq.Location.to2d_rot = to2d_rot
+
+
 def with_angle_2d(self: Cq.Location, angle: float) -> Tuple[float, float]:
     """
     Returns position and angle