Source code for genericroboticarm.robo_APIs.precise_flex.pf_simulation

"""
Simulation for PF400
"""
import time
from math import dist
from datetime import datetime




class PFSimulation:
    # mirror the gripper positions of the real implementation so simulated joint reads
    # (wherej/DestJ, axis 5) behave like the hardware
    GRIPPER_CLOSED = 74
    GRIPPER_OPEN = 85

    def __init__(self, simulated_plate: bool = False):
        # cartesian position, returned by wherec/DestC
        self.pos = [0, 0, 0, 0, 0, 0]
        # joint position, returned by wherej/DestJ. Axis 5 (index 4) is the gripper.
        self.joints = [0.0, 0.0, 0.0, 0.0, float(self.GRIPPER_OPEN), 0.0]
        # last commanded joint destination (what DestJ 1 reports)
        self.joint_dest = list(self.joints)
        # when True, the gripper cannot fully close, as if it were holding a plate
        self.simulated_plate = simulated_plate



    def send_command(self, cmd: str) -> tuple[int, str]:
        """Mimics PFSocket.send_command: returns (error_code, answer)."""
        if "where" not in cmd and "Dest" not in cmd:
            print(f"{datetime.now()}: Simulate command {cmd}")
        parts = cmd.split()
        keyword = parts[0] if parts else ""

        if keyword in ("wherec", "DestC"):
            return 0, " ".join(str(v) for v in self.pos)
        if keyword == "wherej":
            return 0, " ".join(str(v) for v in self.joints)
        if keyword == "DestJ":
            # "DestJ 1" returns the target of the previous move; a bare "DestJ" returns
            # the current position while the robot is idle (see the TCS command reference)
            reads_target = len(parts) > 1 and parts[1] == "1"
            state = self.joint_dest if reads_target else self.joints
            return 0, " ".join(str(v) for v in state)
        if keyword == "MoveOneAxis":
            # MoveOneAxis <axis> <target> <profile>
            axis = int(parts[1]) - 1
            target = float(parts[2])
            self.joint_dest[axis] = target
            if axis == 4 and self.simulated_plate and target <= self.GRIPPER_CLOSED:
                # a held plate blocks the gripper from closing fully; the resulting gap
                # must stay above the detection threshold in currently_gripping_plate
                self.joints[axis] = self.GRIPPER_CLOSED + 3
            else:
                self.joints[axis] = target
            time.sleep(.2)
            return 0, ""
        if keyword == "MoveJ":
            targets = [float(v) for v in parts[2:]]
            for index, target in enumerate(targets):
                self.joint_dest[index] = target
                self.joints[index] = target
            time.sleep(.5)
            return 0, ""
        if keyword == "MoveC":
            time.sleep(1.5)
            target = [float(coord) for coord in parts[2:]]
            d = dist(self.pos, target)
            time.sleep(d / 50)
            self.pos = target
            return 0, ""
        if keyword == "Speed":
            print(f"{datetime.now()}: Setting Speed to {parts[-1]}")
            return 0, ""
        return 0, "OK"