Source code for genericroboticarm.robo_APIs.precise_flex.pf_on_rail

"""
Implementation of RoboInterface for a PF400 on a rail.
This extends the PF400 implementation
"""
from typing import List, Tuple
from math import dist

from genericroboticarm.control.graph_manager import JointState
from genericroboticarm.robo_APIs.precise_flex import PFImplementation

MAX_Z_COORDINATE = 690.0



class PFonRail(PFImplementation):
    def __init__(self, start_simulating: bool = True,
                 host: str = "10.10.0.98",
                 port: int = 10100):
        super().__init__(start_simulating, host, port)
        self.set_metrik(self.my_dist)



    @classmethod
    def get_name(cls) -> str:
        return "PFonRail"




    def my_dist(self, u: JointState, v: JointState) -> float:
        carth_u = [u[coord] for coord in "xyz"]
        carth_v = [v[coord] for coord in "xyz"]
        #TODO: the rail coordinate gets added to the y-coordinate
        rail_diff = abs(u["rail"] - v["rail"])
        # to avoid the slow rail movements we add that distance instead of including it in eucledean distance
        distance = dist(carth_u, carth_v) + rail_diff
        return distance


    @property
    def joint_names(self) -> list[str]:
        return ["x", "y", "z", "dir", "rail", "joint2", "joint3"]



    def move_relative(self, movements: List[Tuple[str, float]], **kwargs):
        """
        we simply forbid simultaneous movements of rail and head
        """
        move_dict = dict(movements)
        carth_coords = ["x", "y", "z", "dir"]
        rail_diff = move_dict.get("rail", 0)
        if any(coord in move_dict for coord in carth_coords):
            # use carthesian coordinates together with destC for manual movements without rail
            destination_str = self.socket.send_command("DestC")
            dest_coordinates = destination_str.split(" ")[1:7]
            # manipulate the destination coordinates
            for idx, coord in enumerate(carth_coords):
                diff = move_dict.get(coord, 0)
                curr_dest = float(dest_coordinates[idx])
                new_dest = curr_dest + diff
                # there is a maximum height
                if coord == "z":
                    new_dest = max(0., min(MAX_Z_COORDINATE, new_dest))
                dest_coordinates[idx] = str(new_dest)
            position_str = " ".join(dest_coordinates)
            cmd = f"MoveC {self.move_profile} {position_str}"
            self.socket.send_command(cmd)
        elif rail_diff:
            # use move_joint together with DestJ for rail movements
            destj_str = self.socket.send_command("DestJ")
            curr_rail_dest_str = destj_str.split(" ")[6]
            new_rail_dest = float(curr_rail_dest_str) + rail_diff
            self.move_joint(5, new_rail_dest)


    @property
    def current_coordinates(self) -> JointState:
        # internally work in joint space when on normal operation
        joint_position_str = self.socket.send_command("wherej")
        joint_positions = joint_position_str.split(' ')[1:7]
        carth_position_str = self.socket.send_command("wherec")
        carth_positions = carth_position_str.split(' ')[1:7]
        # the gripper is the 5th joint but should not be included in movements
        joint_state = dict(
            x=float(carth_positions[0]),
            y=float(carth_positions[1]),
            z=float(joint_positions[0]),
            dir=float(joint_positions[3]),
            rail=float(joint_positions[5]),
            joint2=float(joint_positions[1]),
            joint3=float(joint_positions[2]),
        )
        return joint_state



    def move_to_coordinates(self, coords: JointState, **kwargs) -> None:
        destj_str = self.socket.send_command("DestJ")
        gripper_pos = destj_str.split()[5]  # 5th joint and one buffer number
        # we move in the joint space
        target_pos = [coords["z"], coords["joint2"], coords["joint3"], coords["dir"], gripper_pos, coords["rail"]]
        position_str = " ".join(str(s) for s in target_pos)
        cmd = f'MoveJ {self.move_profile} {position_str}'
        self.socket.send_command(cmd)