diff --git a/pyproject.toml b/pyproject.toml index c60e14c..8528840 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -25,7 +25,7 @@ Source = "https://github.com/SpesRobotics/teleop" Tracker = "https://github.com/SpesRobotics/teleop/issues" [project.optional-dependencies] -utils = ["pin"] +utils = ["pin", "ruckig"] [tool.setuptools.packages.find] include = ["teleop*"] diff --git a/teleop/__init__.py b/teleop/__init__.py index eb36af4..180223a 100644 --- a/teleop/__init__.py +++ b/teleop/__init__.py @@ -2,9 +2,9 @@ import math import socket import logging -from typing import Callable, List +from typing import Callable, List, NamedTuple import uvicorn -from fastapi import FastAPI, WebSocket, WebSocketDisconnect +from fastapi import APIRouter, FastAPI, WebSocket, WebSocketDisconnect from fastapi.responses import FileResponse from fastapi.staticfiles import StaticFiles import transforms3d as t3d @@ -15,6 +15,11 @@ THIS_DIR = os.path.dirname(os.path.realpath(__file__)) +class FrontendMount(NamedTuple): + path: str + directory: str + + def get_local_ip(): try: # Connect to an external address (doesn't actually send data) @@ -147,7 +152,9 @@ class Teleop: Args: host (str, optional): The host IP address. Defaults to "0.0.0.0". port (int, optional): The port number. Defaults to 4443. - frontend_dir (str, optional): The directory containing the frontend assets. Defaults to THIS_DIR. + frontend_dir (str | list[tuple[str, str]], optional): The directory + containing the frontend assets, or a list of route prefix and + frontend directory pairs. Defaults to THIS_DIR. """ def __init__( @@ -157,6 +164,8 @@ def __init__( natural_phone_orientation_euler=None, natural_phone_position=None, frontend_dir=None, + offset_user_orientation=0, + current_pose_provider=None, ): self.__logger = logging.getLogger("teleop") self.__logger.setLevel(logging.INFO) @@ -170,6 +179,8 @@ def __init__( self.__previous_received_pose = None self.__callbacks = [] self.__pose = np.eye(4) + self.__previous_move = False + self.__current_pose_provider = current_pose_provider if natural_phone_orientation_euler is None: natural_phone_orientation_euler = [0, math.radians(-45), 0] @@ -180,10 +191,9 @@ def __init__( t3d.euler.euler2mat(*natural_phone_orientation_euler), [1, 1, 1], ) + self.__offset_user_orientation = t3d.affines.compose([0, 0, 0], t3d.euler.euler2mat(0, 0, offset_user_orientation), [1, 1, 1]) - if frontend_dir is None: - frontend_dir = THIS_DIR - self.__frontend_dir = frontend_dir + self.__frontend_mounts = self.__normalize_frontend_mounts(frontend_dir) self.__app = FastAPI() self.__manager = ConnectionManager() @@ -192,6 +202,52 @@ def __init__( logging.getLogger("uvicorn.access").setLevel(logging.WARNING) self.__setup_routes() + def include_router(self, router: APIRouter, **kwargs) -> None: + self.__app.include_router(router, **kwargs) + + def __normalize_frontend_mounts(self, frontend_dir): + if frontend_dir is None: + frontend_dir = THIS_DIR + + if isinstance(frontend_dir, (str, os.PathLike)): + return [FrontendMount("/", os.fspath(frontend_dir))] + + mounts = [] + seen_paths = set() + for mount in frontend_dir: + try: + path, directory = mount + except (TypeError, ValueError): + raise ValueError( + "frontend_dir entries must be (path, directory) pairs" + ) from None + + path = self.__normalize_frontend_path(path) + if path in seen_paths: + raise ValueError(f"Duplicate frontend mount path: {path}") + + seen_paths.add(path) + mounts.append(FrontendMount(path, os.fspath(directory))) + + if not mounts: + raise ValueError("frontend_dir mount list cannot be empty") + + return mounts + + @staticmethod + def __normalize_frontend_path(path): + path = str(path).strip() + if not path: + raise ValueError("Frontend mount path cannot be empty") + + if not path.startswith("/"): + path = f"/{path}" + + if path != "/": + path = path.rstrip("/") + + return path + def set_pose(self, pose: np.ndarray) -> None: """ Set the current pose of the end-effector. @@ -222,6 +278,9 @@ def __update(self, message): position = message["position"] orientation = message["orientation"] scale = message.get("scale", 1.0) + #Detect rising edge of move signal + move_started = move and not self.__previous_move + self.__previous_move = move position = np.array([position["x"], position["y"], position["z"]]) quat = np.array( @@ -233,7 +292,13 @@ def __update(self, message): self.__absolute_pose_init = None self.__notify_subscribers(self.__pose, message) return - + if move_started and self.__current_pose_provider is not None: # Rising edge + try: + current_pose = self.__current_pose_provider() + self.__pose = np.array(current_pose, dtype=float, copy=True) + except Exception: + self.__logger.warning("Failed to get current pose, using last known pose") + received_pose_rub = t3d.affines.compose( position, t3d.quaternions.quat2mat(quat), [1, 1, 1] ) @@ -241,7 +306,7 @@ def __update(self, message): received_pose[:3, :3] = received_pose[:3, :3] @ np.linalg.inv( TF_RUB2FLU[:3, :3] ) - received_pose = received_pose @ self.__natural_phone_pose + received_pose = self.__offset_user_orientation @ received_pose @ self.__natural_phone_pose # Pose jump protection if self.__previous_received_pose is not None: @@ -285,14 +350,8 @@ def __update(self, message): self.__notify_subscribers(self.__pose, message) def __setup_routes(self): - # Mount static files directory - assets_dir = os.path.join(self.__frontend_dir, "assets") - self.__app.mount("/assets", StaticFiles(directory=assets_dir), name="assets") - - @self.__app.get("/") - async def index(): - self.__logger.debug("Serving the index.html file") - return FileResponse(os.path.join(self.__frontend_dir, "index.html")) + for mount_index, mount in enumerate(self.__frontend_mounts): + self.__mount_frontend(mount, mount_index) @self.__app.websocket("/ws") async def websocket_endpoint(websocket: WebSocket): @@ -314,6 +373,43 @@ async def websocket_endpoint(websocket: WebSocket): self.__manager.disconnect(websocket) self.__logger.info("Client disconnected") + def __mount_frontend(self, mount: FrontendMount, mount_index: int): + assets_dir = os.path.join(mount.directory, "assets") + assets_path = "/assets" if mount.path == "/" else f"{mount.path}/assets" + self.__app.mount( + assets_path, + StaticFiles(directory=assets_dir), + name=f"frontend_assets_{mount_index}", + ) + + async def index(): + self.__logger.debug( + f"Serving {mount.path} frontend index.html from {mount.directory}" + ) + return FileResponse(os.path.join(mount.directory, "index.html")) + + if mount.path == "/": + self.__app.add_api_route("/", index, methods=["GET"]) + return + + self.__app.add_api_route(mount.path, index, methods=["GET"]) + self.__app.add_api_route(f"{mount.path}/", index, methods=["GET"]) + + @self.__app.get(f"{mount.path}/{{path:path}}") + async def prefixed_spa_index(path: str): + requested_path = os.path.abspath(os.path.join(mount.directory, path)) + frontend_root = os.path.abspath(mount.directory) + if ( + os.path.commonpath([frontend_root, requested_path]) == frontend_root + and os.path.isfile(requested_path) + ): + return FileResponse(requested_path) + + self.__logger.debug( + f"Serving {mount.path} frontend index.html from {mount.directory}" + ) + return FileResponse(os.path.join(mount.directory, "index.html")) + def run(self) -> None: """ Runs the teleop server. This method is blocking. diff --git a/teleop/utils/jacobi_robot.py b/teleop/utils/jacobi_robot.py index 22b316f..f2e1e76 100644 --- a/teleop/utils/jacobi_robot.py +++ b/teleop/utils/jacobi_robot.py @@ -1,6 +1,7 @@ import numpy as np import pinocchio as pin import matplotlib.pyplot as plt +import ruckig from typing import List, Tuple @@ -48,6 +49,8 @@ def __init__( min_angular_vel: float = 0.1, linear_gain: float = 20.0, angular_gain: float = 12.0, + max_joint_acc: float = 10.0, + max_joint_jerk: float = 100.0, ): """ Initialize the Pinocchio robot with servo control capabilities. @@ -67,6 +70,7 @@ def __init__( # Robot state self.q = pin.neutral(self.model) # Joint positions self.dq = np.zeros(self.model.nv) # Joint velocities + self.ddq = np.zeros(self.model.nv) # Joint accelerations # Joint limits self.q_min = self.model.lowerPositionLimit @@ -79,6 +83,8 @@ def __init__( self.max_linear_acc = max_linear_acc self.max_angular_acc = max_angular_acc self.max_joint_vel = max_joint_vel + self.max_joint_acc = max_joint_acc + self.max_joint_jerk = max_joint_jerk self.min_linear_vel = min_linear_vel self.min_angular_vel = min_angular_vel self.linear_gain = linear_gain @@ -324,10 +330,95 @@ def servo_to_pose( reached = position_error < linear_tol and orientation_error < angular_tol return reached + def servo_to_joint_positions( + self, + target_joint_positions, + dt: float = 0.01, + joint_tol: float = 1e-4, + max_joint_vel: float = None, + max_joint_acc: float = None, + max_joint_jerk: float = None, + ) -> bool: + """ + Move one jerk-limited Ruckig step toward target joint positions. + + Args: + target_joint_positions: Dict of joint names to positions, or a vector + ordered like ``self.q``. + dt: Time step for online trajectory generation. + joint_tol: Joint-space convergence tolerance. + max_joint_vel: Optional per-joint velocity cap. Defaults to the robot's + configured ``max_joint_vel``. + max_joint_acc: Optional per-joint acceleration cap. Defaults to the + robot's configured ``max_joint_acc``. + max_joint_jerk: Optional per-joint jerk cap. Defaults to the robot's + configured ``max_joint_jerk``. + + Returns: + bool: True if the target joint positions are reached after this step. + """ + if dt <= 0: + raise ValueError("dt must be greater than zero.") + if joint_tol < 0: + raise ValueError("joint_tol must be non-negative.") + + target_q = self.__target_joint_positions_to_vector(target_joint_positions) + + velocity_cap = self.max_joint_vel if max_joint_vel is None else max_joint_vel + if velocity_cap <= 0: + raise ValueError("max_joint_vel must be greater than zero.") + + acceleration_cap = ( + self.max_joint_acc if max_joint_acc is None else max_joint_acc + ) + if acceleration_cap <= 0: + raise ValueError("max_joint_acc must be greater than zero.") + + jerk_cap = self.max_joint_jerk if max_joint_jerk is None else max_joint_jerk + if jerk_cap <= 0: + raise ValueError("max_joint_jerk must be greater than zero.") + + self.__validate_joint_position_limits(target_q, joint_tol) + + max_velocity = self.__joint_velocity_limits(velocity_cap) + dofs = len(self.q) + otg = ruckig.Ruckig(dofs, dt) + ruckig_input = ruckig.InputParameter(dofs) + ruckig_output = ruckig.OutputParameter(dofs) + + ruckig_input.current_position = self.q.tolist() + ruckig_input.current_velocity = self.dq.tolist() + ruckig_input.current_acceleration = self.ddq.tolist() + ruckig_input.target_position = target_q.tolist() + ruckig_input.target_velocity = np.zeros(dofs).tolist() + ruckig_input.target_acceleration = np.zeros(dofs).tolist() + ruckig_input.max_velocity = max_velocity.tolist() + ruckig_input.max_acceleration = np.full(dofs, acceleration_cap).tolist() + ruckig_input.max_jerk = np.full(dofs, jerk_cap).tolist() + + try: + result = otg.update(ruckig_input, ruckig_output) + except Exception as error: + raise ValueError("Failed to calculate Ruckig joint trajectory.") from error + + if result not in (ruckig.Result.Working, ruckig.Result.Finished): + raise ValueError(f"Ruckig failed to calculate joint trajectory: {result}") + + self.q = np.asarray(ruckig_output.new_position, dtype=float) + self.dq = np.asarray(ruckig_output.new_velocity, dtype=float) + self.ddq = np.asarray(ruckig_output.new_acceleration, dtype=float) + + remaining_error = target_q - self.q + return bool(np.all(np.abs(remaining_error) <= joint_tol)) + def update_state(self, joint_velocities: np.ndarray, dt: float = 0.01): """Update robot state with given joint velocities.""" + previous_dq = self.dq.copy() + # Store velocities self.dq = joint_velocities.copy() + if dt > 0: + self.ddq = (self.dq - previous_dq) / dt # Integrate to get new joint positions self.q = self.q + self.dq * dt @@ -463,7 +554,6 @@ def set_joint_position(self, joint_name: str, position: float): joint_index = self.__get_joint_index(joint_name) if joint_index < 0 or joint_index >= self.model.njoints: raise ValueError(f"Joint '{joint_name}' not found in model.") - print(f"Setting joint '{joint_name}' to position {position:.3f}") self.q[joint_index] = position def get_joint_names(self) -> List[str]: @@ -482,6 +572,51 @@ def get_joint_velocity(self, joint_name: str) -> float: raise ValueError(f"Joint '{joint_name}' not found in model.") return self.dq[joint_index] + def __target_joint_positions_to_vector(self, target_joint_positions) -> np.ndarray: + if isinstance(target_joint_positions, dict): + target_q = self.q.copy() + for joint_name, position in target_joint_positions.items(): + joint_index = self.__get_joint_index(joint_name) + if joint_index < 0 or joint_index >= self.model.nq: + raise ValueError(f"Joint '{joint_name}' not found in model.") + target_q[joint_index] = position + else: + target_q = np.asarray(target_joint_positions, dtype=float) + if target_q.shape != self.q.shape: + raise ValueError( + "target_joint_positions must have shape " + f"{self.q.shape}, got {target_q.shape}" + ) + + if not np.all(np.isfinite(target_q)): + raise ValueError("target_joint_positions must be finite.") + + return target_q + + def __joint_velocity_limits(self, velocity_cap: float) -> np.ndarray: + max_velocity = np.full(len(self.q), velocity_cap, dtype=float) + for i in range(len(max_velocity)): + if i < len(self.dq_max) and self.dq_max[i] > 0: + max_velocity[i] = min(max_velocity[i], self.dq_max[i]) + return max_velocity + + def __validate_joint_position_limits( + self, target_q: np.ndarray, joint_tol: float + ) -> None: + lower_limit_mask = np.isfinite(self.q_min) + below_limits = ( + target_q[lower_limit_mask] < self.q_min[lower_limit_mask] - joint_tol + ) + if np.any(below_limits): + raise ValueError("target_joint_positions are below the robot joint limits.") + + upper_limit_mask = np.isfinite(self.q_max) + above_limits = ( + target_q[upper_limit_mask] > self.q_max[upper_limit_mask] + joint_tol + ) + if np.any(above_limits): + raise ValueError("target_joint_positions are above the robot joint limits.") + def __compute_regularized_jacobian_pinv(self, J: np.ndarray) -> np.ndarray: """ Compute regularized pseudo-inverse of Jacobian with multiple regularization terms. diff --git a/teleop/utils/jacobi_robot_ros.py b/teleop/utils/jacobi_robot_ros.py index 18ae8f7..d817681 100644 --- a/teleop/utils/jacobi_robot_ros.py +++ b/teleop/utils/jacobi_robot_ros.py @@ -25,10 +25,13 @@ def __init__( max_linear_acc: float = 3.0, max_angular_acc: float = 6.0, max_joint_vel: float = 5.0, + max_joint_acc: float = 10.0, + max_joint_jerk: float = 100.0, min_linear_vel: float = 0.03, min_angular_vel: float = 0.1, linear_gain: float = 5.0, angular_gain: float = 1.0, + ): """ Initialize ROS 2-enabled Jacobian robot. @@ -50,6 +53,8 @@ def __init__( min_angular_vel: Minimum angular velocity linear_gain: Linear gain for control angular_gain: Angular gain for control + max_joint_acc: Maximum joint acceleration + max_joint_jerk: Maximum joint jerk """ self.node = node @@ -68,6 +73,8 @@ def __init__( min_angular_vel, linear_gain, angular_gain, + max_joint_acc, + max_joint_jerk, ) self.joint_states_received = False @@ -205,6 +212,27 @@ def servo_to_pose(self, target_pose: np.ndarray, dt: float = 0.1) -> bool: self.__send_joint_trajectory_topic(duration=dt) return reached + def servo_to_joint_positions( + self, + target_joint_positions, + dt: float = 0.1, + joint_tol: float = 1e-4, + max_joint_vel: float = None, + max_joint_acc: float = None, + max_joint_jerk: float = None, + ) -> bool: + reached = super().servo_to_joint_positions( + target_joint_positions, + dt=dt, + joint_tol=joint_tol, + max_joint_vel=max_joint_vel, + max_joint_acc=max_joint_acc, + max_joint_jerk=max_joint_jerk, + ) + + self.__send_joint_trajectory_topic(duration=dt) + return reached + def reset_joint_states(self, blocked: bool = True): """Reset the robot state.""" self.joint_states_received = False diff --git a/tests/test_jacobi_robot_joint_servo.py b/tests/test_jacobi_robot_joint_servo.py new file mode 100644 index 0000000..881bb17 --- /dev/null +++ b/tests/test_jacobi_robot_joint_servo.py @@ -0,0 +1,96 @@ +import unittest +from pathlib import Path + +import numpy as np + +try: + from teleop.utils.jacobi_robot import JacobiRobot +except ImportError as error: + JacobiRobot = None + JACOBI_IMPORT_ERROR = error +else: + JACOBI_IMPORT_ERROR = None + + +class TestJacobiRobotJointServo(unittest.TestCase): + def setUp(self): + if JacobiRobot is None: + self.skipTest( + f"JacobiRobot dependencies are unavailable: {JACOBI_IMPORT_ERROR}" + ) + + urdf_path = ( + Path(__file__).resolve().parents[1] + / "teleop" + / "utils" + / "lite6.urdf" + ) + self.robot = JacobiRobot(str(urdf_path), ee_link="link6") + + def test_servo_to_joint_positions_steps_toward_named_targets(self): + target = { + "joint1": 0.2, + "joint2": 0.0, + "joint3": 0.0, + "joint4": 0.0, + "joint5": 0.0, + "joint6": 0.0, + } + + reached = self.robot.servo_to_joint_positions( + target, + dt=0.1, + max_joint_vel=1.0, + max_joint_acc=10.0, + max_joint_jerk=100.0, + ) + + self.assertFalse(reached) + self.assertGreater(self.robot.get_joint_position("joint1"), 0.0) + self.assertLess(self.robot.get_joint_position("joint1"), 0.1) + self.assertLessEqual(abs(self.robot.get_joint_velocity("joint1")), 1.0) + self.assertLessEqual(abs(self.robot.ddq[0]), 10.0) + + for _ in range(10): + reached = self.robot.servo_to_joint_positions( + target, + dt=0.1, + max_joint_vel=1.0, + max_joint_acc=10.0, + max_joint_jerk=100.0, + ) + if reached: + break + + self.assertTrue(reached) + self.assertAlmostEqual(self.robot.get_joint_position("joint1"), 0.2) + + def test_servo_to_joint_positions_accepts_vector_targets(self): + target = np.zeros_like(self.robot.q) + target[0] = 0.05 + + reached = self.robot.servo_to_joint_positions( + target, + dt=0.1, + max_joint_vel=1.0, + max_joint_acc=10.0, + max_joint_jerk=100.0, + ) + + for _ in range(10): + if reached: + break + reached = self.robot.servo_to_joint_positions( + target, + dt=0.1, + max_joint_vel=1.0, + max_joint_acc=10.0, + max_joint_jerk=100.0, + ) + + self.assertTrue(reached) + self.assertAlmostEqual(self.robot.get_joint_position("joint1"), 0.05) + + +if __name__ == "__main__": + unittest.main()