replaced list by np.array

src/example/movement_test.py

@@ -1,14 +1,14 @@
 from pathlib import Path
-from typing import List
 
 import numpy as np
+import numpy.typing as npt
 from reachy_placo.ik_reachy_placo import IKReachyQP
 
 from reachy2_symbolic_ik.symbolic_ik import SymbolicIK
 from reachy2_symbolic_ik.utils import go_to_position
 
 
-def make_movement_test(symbolic_ik: SymbolicIK, placo_ik: IKReachyQP, goal_pose: List[List[float]]) -> None:
+def make_movement_test(symbolic_ik: SymbolicIK, placo_ik: IKReachyQP, goal_pose: npt.NDArray[np.float64]) -> None:
     result = symbolic_ik.is_reachable(goal_pose)
     if result[0]:
         print(int((result[1][1] - result[1][0]) * 50))
@@ -24,10 +24,10 @@ def make_movement_test(symbolic_ik: SymbolicIK, placo_ik: IKReachyQP, goal_pose:
 def make_line(
     symbolic_ik: SymbolicIK,
     placo_ik: IKReachyQP,
-    start_position: List[float],
-    end_position: List[float],
-    start_orientation: List[float],
-    end_orientation: List[float],
+    start_position: npt.NDArray[np.float64],
+    end_position: npt.NDArray[np.float64],
+    start_orientation: npt.NDArray[np.float64],
+    end_orientation: npt.NDArray[np.float64],
     nb_points: int = 100,
 ) -> None:
     x = np.linspace(start_position[0], end_position[0], nb_points)
@@ -72,10 +72,10 @@ def main_test() -> None:
     # goal_pose = [goal_position, goal_orientation]
     # make_movement_test(symbolib_ik, placo_ik, goal_pose)
 
-    start_position = [0.4, 0.1, -0.4]
-    end_position = [0.3, -0.2, -0.1]
-    start_orientation = [0.35, -1.40, 0.17]
-    end_orientation = [0.0, -0.0, 0.0]
+    start_position = np.array([0.4, 0.1, -0.4])
+    end_position = np.array([0.3, -0.2, -0.1])
+    start_orientation = np.array([0.35, -1.40, 0.17])
+    end_orientation = np.array([0.0, -0.0, 0.0])
     make_line(symbolic_ik, placo_ik, start_position, end_position, start_orientation, end_orientation, nb_points=300)
 
 

src/example/symbolic_ik_test.py

@@ -1,15 +1,15 @@
 from pathlib import Path
-from typing import List
 
 import numpy as np
+import numpy.typing as npt
 from reachy_placo.ik_reachy_placo import IKReachyQP
 from scipy.spatial.transform import Rotation as R
 
 from reachy2_symbolic_ik.symbolic_ik import SymbolicIK
 from reachy2_symbolic_ik.utils import go_to_position
 
 
-def are_joints_correct(placo_ik: IKReachyQP, joints: List[float], goal_pose: List[List[float]]) -> bool:
+def are_joints_correct(placo_ik: IKReachyQP, joints: npt.NDArray[float], goal_pose: npt.NDArray[float]) -> bool:
     go_to_position(placo_ik, joints, wait=0)
     T_torso_tip = placo_ik.robot.get_T_a_b("torso", "r_tip_joint")
     position = T_torso_tip[:3, 3]
@@ -51,7 +51,7 @@ def main_test() -> None:
 
     goal_position = [0.4, 0.1, -0.4]
     goal_orientation = [np.radians(20), np.radians(-80), np.radians(10)]
-    goal_pose = [goal_position, goal_orientation]
+    goal_pose = np.array([goal_position, goal_orientation])
     result = symbolib_ik.is_reachable(goal_pose)
     if result[0]:
         joints = result[2](result[1][0])

src/reachy2_symbolic_ik/symbolic_ik.py

@@ -1,6 +1,6 @@
 import math
 import time
-from typing import Any, List, Optional, Tuple
+from typing import Any, Optional, Tuple
 
 import matplotlib.pyplot as plt
 import numpy as np
@@ -359,15 +359,15 @@ def intersection_point(
         p01: npt.NDArray[np.float64],
         v2: npt.NDArray[np.float64],
         p02: npt.NDArray[np.float64],
-    ) -> List[np.float64]:
+    ) -> npt.NDArray[np.float64]:
         A = np.vstack((v1, -v2)).T
         b = np.subtract(p02, p01)
         params, _, _, _ = np.linalg.lstsq(A, b, rcond=None)
 
         if np.all(np.isclose(params, params[0])):
-            return []
+            return np.array([])
 
-        intersection = list(v1 * params[0] + p01)
+        intersection = v1 * np.float64(params[0]) + p01
         return intersection
 
     def points_of_nearest_approach(
@@ -461,7 +461,7 @@ def make_homogenous_matrix(
             ]
         )
 
-    def get_joints(self, theta: float) -> List[np.float64]:
+    def get_joints(self, theta: float) -> npt.NDArray[np.float64]:
         elbow_position = self.get_coordinate_cercle(self.intersection_circle, theta)
         wrist_position = self.wrist_position
         tip_position = self.goal_pose[0]
@@ -552,7 +552,7 @@ def get_joints(self, theta: float) -> List[np.float64]:
 
         gamma_wrist = -math.atan2(x_in_wrist[1], x_in_wrist[2])
 
-        joints = [alpha_shoulder, beta_shoulder, alpha_elbow, beta_elbow, beta_wrist, alpha_wrist, gamma_wrist]
+        joints = np.array([alpha_shoulder, beta_shoulder, alpha_elbow, beta_elbow, beta_wrist, alpha_wrist, gamma_wrist])
 
         # norm = np.sqrt(
         #     (self.shoulder_position[0] - self.wrist_position[0]) ** 2

src/reachy2_symbolic_ik/utils.py

@@ -2,12 +2,17 @@
 import time
 from typing import List
 
+import numpy as np
+import numpy.typing as npt
+
 # import numpy as np
 from reachy_placo.ik_reachy_placo import IKReachyQP
 
 
 def go_to_position(
-    reachy_placo: IKReachyQP, joint_pose: List[float] = [0.0, 0.0, 0.0, -math.pi / 2, 0.0, 0.0, 0.0], wait: int = 10
+    reachy_placo: IKReachyQP,
+    joint_pose: npt.NDArray[np.float64] = np.array([0.0, 0.0, 0.0, -math.pi / 2, 0.0, 0.0, 0.0]),
+    wait: int = 10,
 ) -> None:
     """
     Show pose with the r_arm in meshcat