Source code for genericroboticarm.robo_APIs.precise_flex.pf_simulation

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




class PFSimulation:
    def __init__(self):
        self.pos = [0, 0, 0, 0, 0, 0]



    def send_command(self, cmd: str):
        if "where" not in cmd and "Dest" not in cmd:
            print(f"Simulate command {cmd}")
        if cmd.startswith("wherec") or cmd.startswith("DestC"):
            return "0 " + " ".join(str(v) for v in self.pos)
        if cmd.startswith("wherej") or cmd.startswith("DestJ"):
            return "0 0 0 0 0 100 0"
        elif cmd.startswith("MoveJ"):
            joint_targets = cmd.split()
            if float(joint_targets[-1]) > 100:
                print("Opening gripper.")
            else:
                print("Closing gripper.")
            time.sleep(.5)
        elif cmd.startswith("Speed"):
            print(f"Setting Speed to {cmd.split()[-1]}")
        elif cmd.startswith("MoveC"):
            time.sleep(1.5)
            coords = cmd.split(' ')[2:]
            target = [float(coord) for coord in coords]
            d = dist(self.pos, target)
            time.sleep(d/50)
            self.pos = target
        else:
            return "OK"