Skip to content

Commit 4e16ae6

Browse files
DrenayazDrenayaz
committed
added type on function
1 parent 11b3b47 commit 4e16ae6

File tree

11 files changed

+426
-281
lines changed

11 files changed

+426
-281
lines changed

‎scripts/git_hooks/pre-commit

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
#!/usr/bin/env bash
2+
#
3+
# Call black and flake before each commit.
4+
# The command "git commit" with no argument will trigger this script.
5+
#
6+
# To enable this hook, cp this file to .git/hooks/.
7+
8+
# If any command fails, exit immediately with that command's exit status
9+
set -eo pipefail
10+
11+
# Run black against all code in the `source_code` directory
12+
black . --check
13+
echo "-------> Black passed!"
14+
15+
# Run isort against all code in the `source_code` directory
16+
isort . -c
17+
echo "-------> Isort passed!"
18+
19+
# Run flake8 against all code in the `source_code` directory
20+
flake8 .
21+
echo "-------> Flake8 passed!"
22+
23+
# Run mypy against all code in the `source_code` directory
24+
mypy .
25+
echo "-------> Mypy passed!"

‎scripts/setup_git_hooks.sh

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
#!/bin/bash
2+
3+
# Define the source and destination directories
4+
SOURCE_DIR="$(git rev-parse --show-toplevel)/scripts/git_hooks"
5+
DEST_DIR="$(git rev-parse --show-toplevel)/.git/hooks"
6+
7+
# Check if the git repository root is found
8+
if [ -z "$DEST_DIR" ]; then
9+
echo "Error: This script must be run within a git repository."
10+
exit 1
11+
fi
12+
13+
# Create the destination directory if it doesn't exist
14+
mkdir -p "$DEST_DIR"
15+
16+
# Copy the git_hooks folder to the destination
17+
cp -r "$SOURCE_DIR/"* "$DEST_DIR"
18+
19+
# Make scripts executable
20+
chmod +x "$DEST_DIR"/*
21+
22+
echo "git hooks copied in $DEST_DIR"

‎scripts/setup_vscode_config.sh

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
#!/bin/bash
2+
3+
# Define the source and destination directories
4+
SOURCE_DIR="$(git rev-parse --show-toplevel)/scripts/vscode"
5+
DEST_DIR="$(git rev-parse --show-toplevel)/.vscode"
6+
7+
# Check if the git repository root is found
8+
if [ -z "$DEST_DIR" ]; then
9+
echo "Error: This script must be run within a git repository."
10+
exit 1
11+
fi
12+
13+
# Create the destination directory if it doesn't exist
14+
mkdir -p "$DEST_DIR"
15+
16+
# Copy the vscode folder to the destination
17+
cp -r "$SOURCE_DIR/"* "$DEST_DIR"
18+
19+
echo "vscode config has been copied to $DEST_DIR"

‎scripts/setup_vscode_extensions.sh

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
#!/bin/bash
2+
3+
# List of VSCode extensions to install
4+
EXTENSIONS=(
5+
"ms-python.python"
6+
"ms-python.mypy-type-checker"
7+
"ms-python.black-formatter"
8+
"ms-python.flake8"
9+
"ms-python.isort"
10+
)
11+
12+
# Installing each extension
13+
for EXT in "${EXTENSIONS[@]}"; do
14+
code --install-extension "$EXT"
15+
done
16+
17+
# Uninstall Pylance if it is installed
18+
if code --list-extensions | grep -q 'ms-python.vscode-pylance'; then
19+
echo "Uninstalling Pylance..."
20+
code --uninstall-extension ms-python.vscode-pylance
21+
fi
22+
23+
echo "Extensions installation complete."

‎scripts/vscode/settings.json

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,15 @@
1+
{
2+
"black-formatter.importStrategy": "fromEnvironment",
3+
"isort.check": true,
4+
"isort.importStrategy": "fromEnvironment",
5+
"editor.formatOnSave": true,
6+
"editor.codeActionsOnSave": {
7+
"source.organizeImports": "explicit"
8+
},
9+
"isort.args": [
10+
"--profile",
11+
"black"
12+
],
13+
"mypy-type-checker.importStrategy": "fromEnvironment",
14+
"flake8.importStrategy": "fromEnvironment"
15+
}

‎src/benchmark/ik_comparison.py

Lines changed: 61 additions & 60 deletions
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,14 @@
1-
from reachy2_symbolic_ik.symbolic_ik import SymbolicIK
2-
import numpy as np
3-
from reachy_placo.ik_reachy_placo import IKReachyQP
41
import math
52
import time
6-
from scipy.spatial.transform import Rotation as R
3+
from pathlib import Path
4+
from typing import List
5+
6+
import numpy as np
77
from grasping_utils.utils import get_homogeneous_matrix_msg_from_euler
8-
from importlib.resources import files
8+
from reachy_placo.ik_reachy_placo import IKReachyQP
9+
from scipy.spatial.transform import Rotation as R
10+
11+
from reachy2_symbolic_ik.symbolic_ik import SymbolicIK
912
from reachy2_symbolic_ik.utils import go_to_position
1013

1114

@@ -60,7 +63,7 @@ def time_test(symbolic_ik: SymbolicIK, placo_ik: IKReachyQP) -> None:
6063
print("placo ik : ", end - start)
6164

6265

63-
def joints_space_test(symbolic_ik: SymbolicIK, placo_ik: IKReachyQP, verbose=False) -> None:
66+
def joints_space_test(symbolic_ik: SymbolicIK, placo_ik: IKReachyQP, verbose: bool = False) -> None:
6467
green = "\033[92m" # GREEN
6568
blue = "\033[94m" # BLUE
6669
yellow = "\033[93m" # YELLOW
@@ -72,7 +75,7 @@ def joints_space_test(symbolic_ik: SymbolicIK, placo_ik: IKReachyQP, verbose=Fal
7275
symbolic_success = 0
7376
symbolic_fail = 0
7477
# pose_fail = []
75-
goal_pose = [[], []]
78+
goal_pose: List[List[float]] = []
7679
for k in range(100):
7780
shoulder_pitch = np.random.uniform(-math.pi, math.pi)
7881
shoulder_roll = np.random.uniform(-math.pi, math.pi)
@@ -96,7 +99,6 @@ def joints_space_test(symbolic_ik: SymbolicIK, placo_ik: IKReachyQP, verbose=Fal
9699
goal_pose_matrix = placo_ik.robot.get_T_a_b("torso", "r_tip_joint")
97100
goal_pose[0] = goal_pose_matrix[:3, 3]
98101
goal_pose[1] = R.from_matrix(goal_pose_matrix[:3, :3]).as_euler("xyz")
99-
# print("goal_pose : ", goal_pose)
100102
is_reachable, joints_placo, errors = placo_ik.is_pose_reachable(
101103
goal_pose_matrix,
102104
arm_name="r_arm",
@@ -124,49 +126,49 @@ def joints_space_test(symbolic_ik: SymbolicIK, placo_ik: IKReachyQP, verbose=Fal
124126
print(red + "Symbolic not reachable" + reset_color)
125127
# print("goal_pose : ", goal_pose)
126128
# pose_fail.append(np.copy(goal_pose))
127-
if verbose:
128-
time.sleep(0.2)
129-
130-
for k in range(100):
131-
shoulder_pitch = np.random.uniform(-math.pi, math.pi)
132-
shoulder_roll = np.random.uniform(-math.pi, math.pi)
133-
elbow_yaw = np.random.uniform(-math.pi, math.pi)
134-
elbow_pitch = np.random.uniform(-math.pi, math.pi)
135-
wrist_pitch = np.random.uniform(math.pi / 4, 2 * math.pi - math.pi / 4)
136-
wrist_roll = np.random.uniform(math.pi / 4, 2 * math.pi - math.pi / 4)
137-
wrist_yaw = np.random.uniform(-math.pi, math.pi)
138-
joints = [shoulder_pitch, shoulder_roll, elbow_yaw, elbow_pitch, wrist_pitch, wrist_roll, wrist_yaw]
139-
if verbose:
140-
print(blue + str(np.degrees(joints)) + reset_color)
141-
go_to_position(placo_ik, joints, wait=0.0)
142-
goal_pose_matrix = placo_ik.robot.get_T_a_b("torso", "r_tip_joint")
143-
goal_pose[0] = goal_pose_matrix[:3, 3]
144-
goal_pose[1] = R.from_matrix(goal_pose_matrix[:3, :3]).as_euler("xyz")
145-
is_reachable, joints_placo, errors = placo_ik.is_pose_reachable(
146-
goal_pose_matrix,
147-
arm_name="r_arm",
148-
q0=[0.0, 0.0, 0.0, -math.pi / 2, 0, 0.0, 0.0],
149-
tolerances=[0.001, 0.001, 0.001, 0.02, 0.02, 0.02],
150-
max_iter=45,
151-
nb_stepper_solve=25,
152-
)
153-
if is_reachable:
154-
if verbose:
155-
print(green + "Placo reachable" + reset_color)
156-
placo_fail += 1
157-
else:
158-
if verbose:
159-
print(red + "Placo not reachable" + reset_color)
160-
result = symbolic_ik.is_reachable(goal_pose)
161-
if result[0]:
162-
if verbose:
163-
print(green + "Symbolic reachable" + reset_color)
164-
symbolic_fail += 1
165-
else:
166-
if verbose:
167-
print(red + "Symbolic not reachable" + reset_color)
168-
if verbose:
169-
time.sleep(0.2)
129+
# if verbose:
130+
# time.sleep(0.2)
131+
132+
# for k in range(100):
133+
# shoulder_pitch = np.random.uniform(-math.pi, math.pi)
134+
# shoulder_roll = np.random.uniform(-math.pi, math.pi)
135+
# elbow_yaw = np.random.uniform(-math.pi, math.pi)
136+
# elbow_pitch = np.random.uniform(-math.pi, math.pi)
137+
# wrist_pitch = np.random.uniform(math.pi / 4, 2 * math.pi - math.pi / 4)
138+
# wrist_roll = np.random.uniform(math.pi / 4, 2 * math.pi - math.pi / 4)
139+
# wrist_yaw = np.random.uniform(-math.pi, math.pi)
140+
# joints = [shoulder_pitch, shoulder_roll, elbow_yaw, elbow_pitch, wrist_pitch, wrist_roll, wrist_yaw]
141+
# if verbose:
142+
# print(blue + str(np.degrees(joints)) + reset_color)
143+
# go_to_position(placo_ik, joints, wait=0.0)
144+
# goal_pose_matrix = placo_ik.robot.get_T_a_b("torso", "r_tip_joint")
145+
# goal_pose[0] = goal_pose_matrix[:3, 3]
146+
# goal_pose[1] = R.from_matrix(goal_pose_matrix[:3, :3]).as_euler("xyz")
147+
# is_reachable, joints_placo, errors = placo_ik.is_pose_reachable(
148+
# goal_pose_matrix,
149+
# arm_name="r_arm",
150+
# q0=[0.0, 0.0, 0.0, -math.pi / 2, 0, 0.0, 0.0],
151+
# tolerances=[0.001, 0.001, 0.001, 0.02, 0.02, 0.02],
152+
# max_iter=45,
153+
# nb_stepper_solve=25,
154+
# )
155+
# if is_reachable:
156+
# if verbose:
157+
# print(green + "Placo reachable" + reset_color)
158+
# placo_fail += 1
159+
# else:
160+
# if verbose:
161+
# print(red + "Placo not reachable" + reset_color)
162+
# result = symbolic_ik.is_reachable(goal_pose)
163+
# if result[0]:
164+
# if verbose:
165+
# print(green + "Symbolic reachable" + reset_color)
166+
# symbolic_fail += 1
167+
# else:
168+
# if verbose:
169+
# print(red + "Symbolic not reachable" + reset_color)
170+
# if verbose:
171+
# time.sleep(0.2)
170172
print("JOINTS SPACE TEST")
171173
# print("Pose fail : ", pose_fail)
172174
print("Placo success : ", placo_success)
@@ -178,12 +180,12 @@ def joints_space_test(symbolic_ik: SymbolicIK, placo_ik: IKReachyQP, verbose=Fal
178180
def task_space_test(
179181
symbolic_ik: SymbolicIK,
180182
placo_ik: IKReachyQP,
181-
x_step=0.15,
182-
y_step=0.15,
183-
z_step=0.15,
184-
roll_step=45,
185-
pitch_step=45,
186-
yaw_step=45,
183+
x_step: float = 0.15,
184+
y_step: float = 0.15,
185+
z_step: float = 0.15,
186+
roll_step: int = 45,
187+
pitch_step: int = 45,
188+
yaw_step: int = 45,
187189
) -> None:
188190
print("TASK SPACE TEST")
189191
shoulder_position = symbolic_ik.shoulder_position
@@ -204,8 +206,7 @@ def task_space_test(
204206
for roll in np.arange(0, 360, roll_step):
205207
for pitch in np.arange(0, 360, pitch_step):
206208
for yaw in np.arange(0, 360, yaw_step):
207-
goal_orientation = (roll, pitch, yaw)
208-
goal_orientation = np.deg2rad(goal_orientation)
209+
goal_orientation = [np.radians(roll), np.radians(pitch), np.radians(yaw)]
209210
goal_pose = [goal_position, goal_orientation]
210211
goal_poses.append(goal_pose)
211212
end_time = time.time()
@@ -225,7 +226,7 @@ def task_space_test(
225226

226227
def main_test() -> None:
227228
symbolib_ik = SymbolicIK()
228-
urdf_path = files("config_files")
229+
urdf_path = Path("src/config_files")
229230
for file in urdf_path.glob("**/*.urdf"):
230231
if file.stem == "reachy2":
231232
urdf_path = file.resolve()

‎src/example/movement_test.py

Lines changed: 18 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,14 @@
1-
from reachy2_symbolic_ik.symbolic_ik import SymbolicIK
1+
from pathlib import Path
2+
from typing import List
3+
24
import numpy as np
3-
from reachy2_symbolic_ik.utils import go_to_position
45
from reachy_placo.ik_reachy_placo import IKReachyQP
5-
from importlib.resources import files
6+
7+
from reachy2_symbolic_ik.symbolic_ik import SymbolicIK
8+
from reachy2_symbolic_ik.utils import go_to_position
69

710

8-
def make_movement_test(symbolic_ik: SymbolicIK, placo_ik: IKReachyQP, goal_pose) -> None:
11+
def make_movement_test(symbolic_ik: SymbolicIK, placo_ik: IKReachyQP, goal_pose: List[List[float]]) -> None:
912
result = symbolic_ik.is_reachable(goal_pose)
1013
if result[0]:
1114
print(int((result[1][1] - result[1][0]) * 50))
@@ -21,12 +24,12 @@ def make_movement_test(symbolic_ik: SymbolicIK, placo_ik: IKReachyQP, goal_pose)
2124
def make_line(
2225
symbolic_ik: SymbolicIK,
2326
placo_ik: IKReachyQP,
24-
start_position,
25-
end_position,
26-
start_orientation,
27-
end_orientation,
28-
nb_points=100,
29-
):
27+
start_position: List[float],
28+
end_position: List[float],
29+
start_orientation: List[float],
30+
end_orientation: List[float],
31+
nb_points: int = 100,
32+
) -> None:
3033
x = np.linspace(start_position[0], end_position[0], nb_points)
3134
y = np.linspace(start_position[1], end_position[1], nb_points)
3235
z = np.linspace(start_position[2], end_position[2], nb_points)
@@ -45,8 +48,8 @@ def make_line(
4548

4649

4750
def main_test() -> None:
48-
symbolib_ik = SymbolicIK()
49-
urdf_path = files("config_files")
51+
symbolic_ik = SymbolicIK()
52+
urdf_path = Path("src/config_files")
5053
for file in urdf_path.glob("**/*.urdf"):
5154
if file.stem == "reachy2":
5255
urdf_path = file.resolve()
@@ -71,9 +74,9 @@ def main_test() -> None:
7174

7275
start_position = [0.4, 0.1, -0.4]
7376
end_position = [0.3, -0.2, -0.1]
74-
start_orientation = np.deg2rad([20, -80, 10])
75-
end_orientation = np.deg2rad([0, -0, 0])
76-
make_line(symbolib_ik, placo_ik, start_position, end_position, start_orientation, end_orientation, nb_points=300)
77+
start_orientation = [0.35, -1.40, 0.17]
78+
end_orientation = [0.0, -0.0, 0.0]
79+
make_line(symbolic_ik, placo_ik, start_position, end_position, start_orientation, end_orientation, nb_points=300)
7780

7881

7982
if __name__ == "__main__":

‎src/example/symbolic_ik_test.py

Lines changed: 8 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -1,15 +1,15 @@
1-
from reachy2_symbolic_ik.symbolic_ik import SymbolicIK
1+
from pathlib import Path
2+
from typing import List
3+
24
import numpy as np
35
from reachy_placo.ik_reachy_placo import IKReachyQP
4-
import math
5-
import time
66
from scipy.spatial.transform import Rotation as R
7-
from grasping_utils.utils import get_homogeneous_matrix_msg_from_euler
8-
from importlib.resources import files
7+
8+
from reachy2_symbolic_ik.symbolic_ik import SymbolicIK
99
from reachy2_symbolic_ik.utils import go_to_position
1010

1111

12-
def are_joints_correct(placo_ik: IKReachyQP, joints: list, goal_pose) -> bool:
12+
def are_joints_correct(placo_ik: IKReachyQP, joints: List[float], goal_pose: List[List[float]]) -> bool:
1313
go_to_position(placo_ik, joints, wait=0)
1414
T_torso_tip = placo_ik.robot.get_T_a_b("torso", "r_tip_joint")
1515
position = T_torso_tip[:3, 3]
@@ -32,7 +32,7 @@ def are_joints_correct(placo_ik: IKReachyQP, joints: list, goal_pose) -> bool:
3232

3333
def main_test() -> None:
3434
symbolib_ik = SymbolicIK()
35-
urdf_path = files("config_files")
35+
urdf_path = Path("src/config_files")
3636
for file in urdf_path.glob("**/*.urdf"):
3737
if file.stem == "reachy2":
3838
urdf_path = file.resolve()
@@ -50,8 +50,7 @@ def main_test() -> None:
5050
placo_ik.create_tasks()
5151

5252
goal_position = [0.4, 0.1, -0.4]
53-
goal_orientation = [20, -80, 10]
54-
goal_orientation = np.deg2rad(goal_orientation)
53+
goal_orientation = [np.radians(20), np.radians(-80), np.radians(10)]
5554
goal_pose = [goal_position, goal_orientation]
5655
result = symbolib_ik.is_reachable(goal_pose)
5756
if result[0]:

0 commit comments

Comments
 (0)