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__()
 

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()
+    (x, y), _ = self.to2d()
-    assert z == 0
+    return x, y
-    assert rx == 0
-    assert ry == 0
-    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