diff --git a/src/example/example_control.py b/src/example/example_control.py index 1dc20a4..9adfb77 100644 --- a/src/example/example_control.py +++ b/src/example/example_control.py @@ -1,10 +1,8 @@ -import time - import numpy as np +from scipy.spatial.transform import Rotation as R from reachy2_symbolic_ik.control_ik import ControlIK from reachy2_symbolic_ik.utils import make_homogenous_matrix_from_rotation_matrix -from scipy.spatial.transform import Rotation as R def control_basics() -> None: @@ -24,7 +22,7 @@ def control_basics() -> None: control_type = "discrete" joints, is_reachable, state = control.symbolic_inverse_kinematics("r_arm", goal_pose, control_type) if is_reachable: - print(f"Joints: {joints}") + print(f"Pose is reachable \nJoints: {joints}") else: print("Pose not reachable") diff --git a/src/example/example_ik.py b/src/example/example_ik.py index 42f8451..e7b01ce 100644 --- a/src/example/example_ik.py +++ b/src/example/example_ik.py @@ -2,8 +2,9 @@ from reachy2_symbolic_ik.symbolic_ik import SymbolicIK + def ik_basics() -> None: - #Create the symbolic IK for the right arm + # Create the symbolic IK for the right arm symbolic_ik = SymbolicIK(arm="r_arm") # Define the goal position and orientation @@ -21,7 +22,7 @@ def ik_basics() -> None: # else theta can be in the intervals [-np.pi, theta_interval[1]] or [theta_interval[0], np.pi] theta = theta_interval[0] - # Get the joints + # Get the joints joints, elbow_position = theta_to_joints_func(theta) print(f"Pose is reachable \nJoints: {joints}") else: diff --git a/src/example/example_sdk.py b/src/example/example_sdk.py index ef9a65d..58d6ef6 100644 --- a/src/example/example_sdk.py +++ b/src/example/example_sdk.py @@ -3,8 +3,10 @@ import numpy as np from reachy2_sdk import ReachySDK from scipy.spatial.transform import Rotation as R + from reachy2_symbolic_ik.utils import make_homogenous_matrix_from_rotation_matrix + def sdk_basics() -> None: # Create the ReachySDK object print("Trying to connect on localhost Reachy...") @@ -32,5 +34,3 @@ def sdk_basics() -> None: if __name__ == "__main__": sdk_basics() - -