Merge pull request #14 from alberto-abarzua/feat/joint_ctrl

Feat/joint ctrl

backend/Dockerfile

@@ -10,7 +10,7 @@ ENV PYTHONUNBUFFERED 1
 ENV PYTHONPATH=/app/src
 
 
-RUN pip install pdm==2.8.2
+RUN pip install pdm==2.9.3
 
 COPY ./pyproject.toml ./pyproject.toml
 COPY ./pdm.lock ./pdm.lock

backend/src/config/main_arm.toml

@@ -0,0 +1,121 @@
+title = "Robot arm configuration"
+[joint_configuration]
+    speed_rad_per_s = 0.35
+    steps_per_rev_motor_axis = 800 # Microstepping
+    homing_direction = 1 # 1 or -1
+    max_angle_rad = 7.0
+    min_angle_rad = -7.0
+    conversion_rate_axis_joint = 1.0 
+    homing_offset_rad = 0.0 # Offset from homing position
+    # Settings can be overwritten for each joint
+
+[arm_parameters]
+# J1 
+    a1x= 0
+    a1y= 0
+    a1z= 0
+# J2
+    a2x= 0
+    a2y= 0
+    a2z= 172.48
+# J3
+    a3x= 0
+    a3y= 0
+    a3z= 173.5
+# J4
+    a4x= 126.2
+    a4y= 0
+    a4z= 0
+# J5
+    a5x= 64.1
+    a5y= 0
+    a5z= 0
+# J6
+    a6x= 169.0
+    a6z= 0
+    a6y= 0
+
+[[joints]]
+    name = "joint_1"
+    homing_direction = -1 # 1 or -1
+    conversion_rate_axis_joint = 9.3 # Gear ratio
+    homing_offset_rad = 0 # Offset from home position
+    speed_rad_per_s = 0.5
+    [joints.driver]
+        type = "stepper"
+        dir_pin = 5
+        step_pin = 4
+    # [joints.driver]
+    #     type = "servo"
+    #     pin = 20
+    [joints.endstop]
+        type = "hall"
+        pin = 26
+    # [joints.endstop]
+    #     type = "dummy"
+
+[[joints]]
+    name = "joint_2"
+    homing_direction = 1 
+    conversion_rate_axis_joint = 1.0 
+    homing_offset_rad = 0.0 
+    [joints.driver]
+        type = "stepper"
+        dir_pin = 14
+        step_pin = 13
+    [joints.endstop]
+        type = "dummy"
+        pin = 25
+
+[[joints]]
+    name = "joint_3"
+    homing_direction = 1 
+    conversion_rate_axis_joint = 1.0 
+    homing_offset_rad = 0.0
+    [joints.driver]
+        type = "stepper"
+        dir_pin = 16
+        step_pin = 15
+    [joints.endstop]
+        type = "hall"
+        pin = 33
+
+[[joints]]
+    name = "joint_4"
+    homing_direction = 1
+    conversion_rate_axis_joint = 1.0
+    homing_offset_rad = 0.0 
+    [joints.driver]
+        type = "stepper"
+        dir_pin = 18
+        step_pin = 17
+    [joints.endstop]
+        type = "hall"
+        pin = 32
+
+[[joints]]
+    name = "joint_5"
+    homing_direction = 1 
+    conversion_rate_axis_joint = 1.0
+    homing_offset_rad = 0.0
+    [joints.driver]
+        type = "stepper"
+        dir_pin = 21
+        step_pin = 19
+    [joints.endstop]
+        type = "hall"
+        pin = 35
+
+[[joints]]
+    name = "joint_6"
+    homing_direction = 1
+    conversion_rate_axis_joint = 1.0
+    homing_offset_rad = 0.0
+    [joints.driver]
+        type = "stepper"
+        dir_pin = 23
+        step_pin = 22 
+    [joints.endstop]
+        type = "hall"
+        pin = 34
+

backend/src/main.py

@@ -1,15 +1,12 @@
-import numpy as np
 from fastapi import FastAPI
 from fastapi.middleware.cors import CORSMiddleware
-from robot_arm_controller.controller import Settings
 
 from routers.move import router as move_router
 from routers.settings import router as settings_router
-from utils.general import get_controller
+from utils.general import start_controller, stop_controller
 
 app = FastAPI()
 
-
 app.add_middleware(
     CORSMiddleware,
     allow_origins=["*"],
@@ -25,18 +22,9 @@
 
 @app.on_event("startup")
 async def startup_event() -> None:
-    controller = get_controller()
-    controller.start(wait=True)
-    # controller.print_status = True
-    controller.set_setting_joints(Settings.HOMING_OFFSET_RADS, np.pi / 4)
-    controller.set_setting_joints(Settings.STEPS_PER_REV_MOTOR_AXIS, 800)
-    controller.set_setting_joints(Settings.CONVERSION_RATE_AXIS_JOINTS, 1.5)
-    controller.set_setting_joints(Settings.SPEED_RAD_PER_S, 0.4)
-    offsets = controller.get_setting_joints(Settings.HOMING_OFFSET_RADS)
-    print(offsets)
+    start_controller()
 
 
 @app.on_event("shutdown")
 async def shutdown_event() -> None:
-    controller = get_controller()
-    controller.stop()
+    stop_controller()

backend/src/routers/move.py

@@ -1,3 +1,4 @@
+from copy import deepcopy
 from typing import Any, Dict, Optional
 
 from fastapi import APIRouter
@@ -10,6 +11,10 @@
 
 router = APIRouter()
 
+# --------
+# Post Models
+# --------
+
 
 class Move(BaseModel):
     x: float
@@ -26,6 +31,22 @@ class Tool(BaseModel):
     wait: Optional[bool] = False
 
 
+class MoveJoint(BaseModel):
+    joint_idx: int
+    joint_value: float
+    wait: Optional[bool] = False
+
+
+class HomeJoint(BaseModel):
+    joint_idx: int
+    wait: Optional[bool] = False
+
+
+class MoveJoints(BaseModel):
+    joint_values: list
+    wait: Optional[bool] = False
+
+
 # --------
 # General
 # --------
@@ -38,6 +59,15 @@ def home(controller: ArmController = controller_dependency) -> Dict[Any, Any]:
     return {"message": "Homed"}
 
 
+@router.post("/home_joint/")
+def home_joint(
+    home_joint: HomeJoint, controller: ArmController = controller_dependency
+) -> Dict[Any, Any]:
+    joint_idx = home_joint.dict().pop("joint_idx")
+    controller.home_joint(joint_idx)
+    return {"message": "Homed"}
+
+
 # --------
 # Pose
 # --------
@@ -81,6 +111,50 @@ def valid_pose(
         return JSONResponse(content={"message": "Pose is not valid"}, status_code=400)
 
 
+@router.post("/joint/")
+def move_joint(
+    move: MoveJoint, controller: ArmController = controller_dependency
+) -> Dict[Any, Any]:
+    move_dict = move.dict()
+    wait = move_dict.pop("wait")
+
+    joint_idx = move_dict.pop("joint_idx")
+    joint_value = move_dict.pop("joint_value")
+    controller.move_joint_to(joint_idx, joint_value)
+    if wait:
+        controller.wait_done_moving()
+    return {"message": "Moved"}
+
+
+@router.post("/joint/relative/")
+def move_joint_to_relative(
+    move: MoveJoint, controller: ArmController = controller_dependency
+) -> Dict[Any, Any]:
+    move_dict = move.dict()
+    wait = move_dict.pop("wait")
+
+    joint_idx = move_dict.pop("joint_idx")
+    joint_value = move_dict.pop("joint_value")
+    controller.move_joint_to_relative(joint_idx, joint_value)
+    if wait:
+        controller.wait_done_moving()
+    return {"message": "Moved"}
+
+
+@router.post("/joints/relative/")
+def move_joints_to_relative(
+    move: MoveJoints, controller: ArmController = controller_dependency
+) -> Dict[Any, Any]:
+    move_dict = move.dict()
+    wait = move_dict.pop("wait")
+
+    joint_values = move_dict.pop("joint_values")
+    controller.move_joints_to_relative(joint_values)
+    if wait:
+        controller.wait_done_moving()
+    return {"message": "Moved"}
+
+
 # --------
 # Pose
 # --------
@@ -114,14 +188,20 @@ def tool_get(controller: ArmController = controller_dependency) -> Dict[Any, Any
 @router.get("/status/")
 def status(controller: ArmController = controller_dependency) -> Dict[Any, Any]:
     pose = controller.current_pose
-    status_dict = pose.as_dict
+    status_dict: Dict[str, Any] = deepcopy(pose.as_dict)
     status_dict["toolValue"] = controller.tool_value
     status_dict["isHomed"] = controller.is_homed
-    print(controller)
     status_dict["moveQueueSize"] = controller.move_queue_size
+    status_dict["currentAngles"] = controller.current_angles
     return status_dict
 
 
+@router.post("/stop/")
+def stop_movement(controller: ArmController = controller_dependency) -> Dict[Any, Any]:
+    controller.stop_movement()
+    return {"message": "Movement stopped"}
+
+
 # --------
 # Angle
 # --------

backend/src/routers/settings.py

@@ -1,19 +1,49 @@
-from typing import Any, Dict
+from typing import Any, Dict, Optional
 
 from fastapi import APIRouter
-from robot_arm_controller.controller import ArmController
+from pydantic import BaseModel
+from robot_arm_controller.controller import ArmController, Settings
 
 from utils.general import controller_dependency
 
 router = APIRouter()
 
 
-@router.get("/")
-def get_items(controller: ArmController = controller_dependency) -> Dict[Any, Any]:
-    return {"items": ["item1", "item2"]}
+class SetSetting(BaseModel):
+    setting: int
+    value: float
+    joint_idx: Optional[int] = None
 
 
-@router.post("/stop_movement/")
-def stop_movement(controller: ArmController = controller_dependency) -> Dict[Any, Any]:
-    controller.stop_movement()
-    return {"message": "Movement stopped"}
+def get_items(
+    setting: int,
+    joint_idx: Optional[int],
+    controller: ArmController = controller_dependency,
+) -> Dict[Any, Any]:
+    if joint_idx is not None:
+        return {
+            "setting": setting,
+            "joint_idx": joint_idx,
+            "value": controller.get_setting_joint(Settings(setting), joint_idx),
+        }
+    else:
+        return {
+            "setting": setting,
+            "value": controller.get_setting_joints(Settings(setting)),
+        }
+
+
+@router.post("/set/")
+def set_item(
+    item: SetSetting, controller: ArmController = controller_dependency
+) -> Dict[Any, Any]:
+    item_dict = item.dict()
+    setting = Settings(item_dict["setting"])
+    value = item_dict["value"]
+    joint_idx = item_dict["joint_idx"]
+    if joint_idx is not None:
+        controller.set_setting_joint(setting, value, joint_idx)
+
+    controller.set_setting_joints(setting, value)
+
+    return {"setting": setting, "value": value}