armbot trigger

app/src/modules/simulation/roboticArm/instances/armInstance/roboticArmInstance.tsx

@@ -1,58 +1,179 @@
-import React, { useEffect, useState } from 'react'
+import React, { useEffect, useRef, useState } from 'react'
 import IKInstance from '../ikInstance/ikInstance';
 import RoboticArmAnimator from '../animator/roboticArmAnimator';
 import { usePlayButtonStore } from '../../../../../store/usePlayButtonStore';
 import { useArmBotStore } from '../../../../../store/simulation/useArmBotStore';
+import armModel from "../../../../../assets/gltf-glb/rigged/ik_arm_4.glb";
+import { useThree } from "@react-three/fiber";
+import { useFloorItems } from '../../../../../store/store';
+import useModuleStore from '../../../../../store/useModuleStore';
+import { Vector3 } from "three";
+import * as THREE from "three";
+
+interface Process {
+    triggerId: string;
+    startPoint?: Vector3;
+    endPoint?: Vector3;
+    speed: number;
+}
+function RoboticArmInstance({ robot }: { robot: ArmBotStatus }) {
 
-function RoboticArmInstance({ armBot }: any) {
     const { isPlaying } = usePlayButtonStore();
     const [currentPhase, setCurrentPhase] = useState<(string)>("init");
-    console.log('currentPhase: ', currentPhase);
-    const { armBots, addArmBot, addCurrentAction } = useArmBotStore();
+    const { scene } = useThree();
+    const targetBone = "Target";
+    const { activeModule } = useModuleStore();
+    const [ikSolver, setIkSolver] = useState<any>(null);
+    const { addCurrentAction, setArmBotActive, setArmBotState, removeCurrentAction } = useArmBotStore();
+    const { floorItems } = useFloorItems();
+    const groupRef = useRef<any>(null);
+    const [processes, setProcesses] = useState<Process[]>([]);
+    const [armBotCurvePoints, setArmBotCurvePoints] = useState({ start: [], end: [] })
+    const restPosition = new THREE.Vector3(0, 2, 1.6);
+    let armBotCurveRef = useRef<THREE.CatmullRomCurve3 | null>(null)
+    const [path, setPath] = useState<[number, number, number][]>([]);
 
     useEffect(() => {
+        let armItems = floorItems?.filter((val: any) =>
+            val.modeluuid === "3abf5d46-b59e-4e6b-9c02-a4634b64b82d"
+        );
+        // Get the first matching item
+        let armItem = armItems?.[0];
+        if (armItem) {
+            const targetMesh = scene?.getObjectByProperty("uuid", armItem.modeluuid);
+            if (targetMesh) {
+                targetMesh.visible = activeModule !== "simulation"
+            }
+        }
+        const targetBones = ikSolver?.mesh.skeleton.bones.find(
+            (b: any) => b.name === targetBone
+        );
 
-        console.log('isPlaying: ', isPlaying);
         if (isPlaying) {
             //Moving armBot from initial point to rest position.
-
-            if (armBot?.isActive && armBot?.state == "idle" && currentPhase == "init") {
-                addCurrentAction(armBot.modelUuid, 'action-001');
-                setCurrentPhase("moving-to-rest");
-
+            if (!robot?.isActive && robot?.state == "idle" && currentPhase == "init") {
+           
+                setArmBotActive(robot.modelUuid, true)
+                setArmBotState(robot.modelUuid, "running")
+                setCurrentPhase("init-to-rest");
+                if (targetBones) {
+                    let curve = createCurveBetweenTwoPoints(targetBones.position, restPosition)
+                    if (curve) {
+                        setPath(curve.points.map(point => [point.x, point.y, point.z]));
+                    }
+                }
+                logStatus(robot.modelUuid, "Starting from init to rest")
             }
             //Waiting for trigger.
-            if (!armBot?.isActive && armBot?.state == "idle" && currentPhase == "moving-to-rest") {
-                setCurrentPhase("rest");
+            else if (robot && !robot.isActive && robot.state === "idle" && currentPhase === "rest" && !robot.currentAction) {
+                console.log("trigger");
+                setTimeout(() => {
+                    addCurrentAction(robot.modelUuid, 'action-003');
+                }, 3000);
             }
-            // Moving armBot from rest position to pick up point.
-            if (!armBot?.isActive && armBot?.state == "idle" && currentPhase == "rest") {
-
+            else if (robot && !robot.isActive && robot.state === "idle" && currentPhase === "rest" && robot.currentAction) {
+                if (robot.currentAction) {
+                    setArmBotActive(robot.modelUuid, true);
+                    setArmBotState(robot.modelUuid, "running");
+                    setCurrentPhase("rest-to-start");
+                    const startPoint = robot.point.actions[0].process.startPoint;
+                    if (startPoint) {
+                        let curve = createCurveBetweenTwoPoints(restPosition, new THREE.Vector3(startPoint[0], startPoint[1], startPoint[2]));
+                        if (curve) {
+                            setPath(curve.points.map(point => [point.x, point.y, point.z]));
+                        }
+                    }
+                }
+                logStatus(robot.modelUuid, "Starting from rest to start")
             }
-            //Moving arm from start point to end point.
-            if (armBot?.isActive && armBot?.state == "running " && currentPhase == "rest-to-start ") {
-
+            else if (robot && !robot.isActive && robot.state === "idle" && currentPhase === "picking" && robot.currentAction) {
+                setArmBotActive(robot.modelUuid, true);
+                setArmBotState(robot.modelUuid, "running");
+                setCurrentPhase("start-to-end");
+                const startPoint = robot.point.actions[0].process.startPoint;
+                const endPoint = robot.point.actions[0].process.endPoint;
+                if (startPoint && endPoint) {
+                    let curve = createCurveBetweenTwoPoints(
+                        new THREE.Vector3(startPoint[0], startPoint[1], startPoint[2]),
+                        new THREE.Vector3(endPoint[0], endPoint[1], endPoint[2])
+                    );
+                    if (curve) {
+                        setPath(curve.points.map(point => [point.x, point.y, point.z]));
+                    }
+                }
+                logStatus(robot.modelUuid, "Starting from start to end")
             }
-            //Moving arm from end point to idle. 
-            if (armBot?.isActive && armBot?.state == "running" && currentPhase == "end-to-start") {
-
+            else if (robot && !robot.isActive && robot.state === "idle" && currentPhase === "dropping" && robot.currentAction) {
+                setArmBotActive(robot.modelUuid, true);
+                setArmBotState(robot.modelUuid, "running");
+                setCurrentPhase("end-to-rest");
+                const endPoint = robot.point.actions[0].process.endPoint;
+                if (endPoint) {
+                    let curve = createCurveBetweenTwoPoints(new THREE.Vector3(endPoint[0], endPoint[1], endPoint[2]), restPosition
+                    );
+                    if (curve) {
+                        setPath(curve.points.map(point => [point.x, point.y, point.z]));
+                    }
+                }
+                logStatus(robot.modelUuid, "Starting from end to rest")
             }
-
         }
 
-    }, [currentPhase, armBot, isPlaying])
+    }, [currentPhase, robot, isPlaying, ikSolver])
+
+
+    function createCurveBetweenTwoPoints(p1: any, p2: any) {
+        const mid = new THREE.Vector3().addVectors(p1, p2).multiplyScalar(0.5);
+        mid.y += 0.5;
+
+        const points = [p1, mid, p2];
+        return new THREE.CatmullRomCurve3(points);
+    }
+
 
     const HandleCallback = () => {
-        if (armBot.isActive && armBot.state == "idle" && currentPhase == "init") {
-            addCurrentAction('armbot-xyz-001', 'action-001');
+        if (robot.isActive && robot.state == "running" && currentPhase == "init-to-rest") {
+            console.log("Callback triggered: rest");
+            setArmBotActive(robot.modelUuid, false)
+            setArmBotState(robot.modelUuid, "idle")
+            setCurrentPhase("rest");
+            setPath([])
         }
+        else if (robot.isActive && robot.state == "running" && currentPhase == "rest-to-start") {
+            console.log("Callback triggered: pick.");
+            setArmBotActive(robot.modelUuid, false)
+            setArmBotState(robot.modelUuid, "idle")
+            setCurrentPhase("picking");
+            setPath([])
+        }
+        else if (robot.isActive && robot.state == "running" && currentPhase == "start-to-end") {
+            console.log("Callback triggered: drop.");
+            setArmBotActive(robot.modelUuid, false)
+            setArmBotState(robot.modelUuid, "idle")
+            setCurrentPhase("dropping");
+            setPath([])
+        }
+        else if (robot.isActive && robot.state == "running" && currentPhase == "end-to-rest") {
+            console.log("Callback triggered: rest, cycle completed.");
+            setArmBotActive(robot.modelUuid, false)
+            setArmBotState(robot.modelUuid, "idle")
+            setCurrentPhase("rest");
+            setPath([])
+            removeCurrentAction(robot.modelUuid)
+        }
+    }
+    const logStatus = (id: string, status: string) => {
+        console.log(id +","+ status);
     }
 
     return (
         <>
 
-            <IKInstance />
-            <RoboticArmAnimator armUuid={armBot?.modelUuid} HandleCallback={HandleCallback} currentPhase={currentPhase} />
+            <IKInstance modelUrl={armModel} setIkSolver={setIkSolver} ikSolver={ikSolver} robot={robot} groupRef={groupRef} processes={processes}
+                setArmBotCurvePoints={setArmBotCurvePoints} />
+            <RoboticArmAnimator armUuid={robot?.modelUuid} HandleCallback={HandleCallback}
+                currentPhase={currentPhase} targetBone={targetBone} ikSolver={ikSolver} robot={robot}
+                logStatus={logStatus} groupRef={groupRef} processes={processes} armBotCurveRef={armBotCurveRef} path={path} />
 
         </>
     )