Merge remote-tracking branch 'origin/simulation-armbot-v2' into v2

app/src/modules/simulation/roboticArm/instances/animator/materialAnimator.tsx

@@ -1,17 +1,16 @@
 import { useFrame } from '@react-three/fiber';
 import React, { useEffect, useRef, useState } from 'react';
 import * as THREE from 'three';
-import { useLogger } from '../../../../../components/ui/log/LoggerContext';
+import { MaterialModel } from '../../../materials/instances/material/materialModel';
 
 type MaterialAnimatorProps = {
     ikSolver: any;
-    armBot: any;
+    armBot: ArmBotStatus;
     currentPhase: string;
 };
 
 export default function MaterialAnimator({ ikSolver, armBot, currentPhase }: MaterialAnimatorProps) {
-    const sphereRef = useRef<THREE.Mesh>(null);
+    const sphereRef = useRef<any>(null);
-    const boneWorldPosition = new THREE.Vector3();
     const [isRendered, setIsRendered] = useState<boolean>(false);
 
     useEffect(() => {
@@ -41,7 +40,8 @@ export default function MaterialAnimator({ ikSolver, armBot, currentPhase }: Mat
             const direction = new THREE.Vector3();
             direction.subVectors(boneWorldPos, boneTargetWorldPos).normalize();
             const downwardDirection = direction.clone().negate();
-            const adjustedPosition = boneWorldPos.clone().addScaledVector(downwardDirection, -0.25);
+          
+            const adjustedPosition = boneWorldPos.clone().addScaledVector(downwardDirection, -0.01);
 
             //set position
             sphereRef.current.position.copy(adjustedPosition);
@@ -56,10 +56,10 @@ export default function MaterialAnimator({ ikSolver, armBot, currentPhase }: Mat
     return (
         <>
             {isRendered && (
-                <mesh ref={sphereRef}>
+                <MaterialModel
-                    <boxGeometry args={[0.5, 0.5, 0.5]} />
+                    matRef={sphereRef}
-                    <meshStandardMaterial color="yellow" />
+                    materialType={armBot.currentAction?.materialType || 'Default material'}
-                </mesh>
+                />
             )}
         </>
     );

app/src/modules/simulation/roboticArm/instances/animator/roboticArmAnimator.tsx

@@ -1,39 +1,34 @@
 import React, { useEffect, useMemo, useRef, useState } from 'react';
 import { useFrame } from '@react-three/fiber';
 import * as THREE from 'three';
-import { Line } from '@react-three/drei';
+import { Line, Text } from '@react-three/drei';
-import {
+import { useAnimationPlaySpeed, usePauseButtonStore, usePlayButtonStore, useResetButtonStore } from '../../../../../store/usePlayButtonStore';
-    useAnimationPlaySpeed,
+
-    usePauseButtonStore,
+type PointWithDegree = {
-    usePlayButtonStore,
+    position: [number, number, number];
-    useResetButtonStore
+    degree: number;
-} from '../../../../../store/usePlayButtonStore';
+};
+
+function RoboticArmAnimator({ HandleCallback, restPosition, ikSolver, targetBone, armBot, path }: any) {
 
-function RoboticArmAnimator({
-    HandleCallback,
-    restPosition,
-    ikSolver,
-    targetBone,
-    armBot,
-    path
-}: any) {
     const progressRef = useRef(0);
     const curveRef = useRef<THREE.Vector3[] | null>(null);
-    const [currentPath, setCurrentPath] = useState<[number, number, number][]>([]);
-    const [circlePoints, setCirclePoints] = useState<[number, number, number][]>([]);
-    const [customCurvePoints, setCustomCurvePoints] = useState<THREE.Vector3[] | null>(null);
-    let curveHeight = 1.75
     const totalDistanceRef = useRef(0);
     const startTimeRef = useRef<number | null>(null);
     const segmentDistancesRef = useRef<number[]>([]);
+    const [currentPath, setCurrentPath] = useState<[number, number, number][]>([]);
+    const [circlePoints, setCirclePoints] = useState<[number, number, number][]>([]);
+    const [circlePointsWithDegrees, setCirclePointsWithDegrees] = useState<PointWithDegree[]>([]);
+    const [customCurvePoints, setCustomCurvePoints] = useState<THREE.Vector3[] | null>(null);
+    let curveHeight = 1.75
+    const CIRCLE_RADIUS = 1.6
+
     // Zustand stores
     const { isPlaying } = usePlayButtonStore();
     const { isPaused } = usePauseButtonStore();
     const { isReset } = useResetButtonStore();
     const { speed } = useAnimationPlaySpeed();
 
-    const CIRCLE_RADIUS = 1.6
-
     // Update path state whenever `path` prop changes
     useEffect(() => {
         setCurrentPath(path);
@@ -47,7 +42,7 @@ function RoboticArmAnimator({
 
     //Handle Reset Animation
     useEffect(() => {
-        if (isReset) {
+        if (isReset || !isPlaying) {
             progressRef.current = 0;
             curveRef.current = null;
             setCurrentPath([]);
@@ -76,6 +71,29 @@ function RoboticArmAnimator({
         }
         return points;
     }
+
+    //Generate CirclePoints with Angle
+    function generateRingPointsWithDegrees(radius: number, segments: number, initialRotation: [number, number, number]) {
+        const points: { position: [number, number, number]; degree: number }[] = [];
+        for (let i = 0; i < segments; i++) {
+            const angleRadians = (i / segments) * Math.PI * 2;
+            const x = Math.cos(angleRadians) * radius;
+            const z = Math.sin(angleRadians) * radius;
+            const degree = (angleRadians * 180) / Math.PI; // Convert radians to degrees
+            points.push({
+                position: [x, 1.5, z],
+                degree,
+            });
+        }
+        return points;
+    }
+
+    // Handle circle points based on armBot position
+    useEffect(() => {
+        const points = generateRingPointsWithDegrees(CIRCLE_RADIUS, 64, armBot.rotation);
+        setCirclePointsWithDegrees(points)
+    }, [armBot.rotation]);
+
     // Function for find nearest Circlepoints Index
     const findNearestIndex = (nearestPoint: [number, number, number], points: [number, number, number][], epsilon = 1e-6) => {
         for (let i = 0; i < points.length; i++) {
@@ -100,6 +118,30 @@ function RoboticArmAnimator({
         }, circlePoints[0]);
     };
 
+    // Helper function to collect points and check forbidden degrees
+    const collectArcPoints = (startIdx: number, endIdx: number, clockwise: boolean) => {
+        const totalSegments = 64;
+        const arcPoints: [number, number, number][] = [];
+        let i = startIdx;
+
+        while (i !== (endIdx + (clockwise ? 1 : -1) + totalSegments) % totalSegments) {
+            const { degree, position } = circlePointsWithDegrees[i];
+            // Skip over
+            arcPoints.push(position);
+            i = (i + (clockwise ? 1 : -1) + totalSegments) % totalSegments;
+        }
+        return arcPoints;
+    };
+
+    //Range to restrict angle 
+    const hasForbiddenDegrees = (arc: [number, number, number][]) => {
+        return arc.some(p => {
+            const idx = findNearestIndex(p, circlePoints);
+            const degree = circlePointsWithDegrees[idx]?.degree || 0;
+            return degree >= 271 && degree <= 300;  // Forbidden range: 271° to 300°
+        });
+    };
+
     // Handle nearest points and final path (including arc points)
     useEffect(() => {
         if (circlePoints.length > 0 && currentPath.length > 0) {
@@ -116,31 +158,23 @@ function RoboticArmAnimator({
             const indexOfNearestStart = findNearestIndex(nearestToStart, circlePoints);
             const indexOfNearestEnd = findNearestIndex(nearestToEnd, circlePoints);
 
-            const clockwiseDistance = (indexOfNearestEnd - indexOfNearestStart + 64) % 64;
+            const totalSegments = 64;
-            const counterClockwiseDistance = (indexOfNearestStart - indexOfNearestEnd + 64) % 64;
+            const clockwiseDistance = (indexOfNearestEnd - indexOfNearestStart + totalSegments) % totalSegments;
-            const clockwiseIsShorter = clockwiseDistance <= counterClockwiseDistance;
+            const counterClockwiseDistance = (indexOfNearestStart - indexOfNearestEnd + totalSegments) % totalSegments;
+
+            // Try both directions
+            const arcClockwise = collectArcPoints(indexOfNearestStart, indexOfNearestEnd, true);
+            const arcCounterClockwise = collectArcPoints(indexOfNearestStart, indexOfNearestEnd, false);
+
+            const clockwiseForbidden = hasForbiddenDegrees(arcClockwise);
+            const counterClockwiseForbidden = hasForbiddenDegrees(arcCounterClockwise);
 
             let arcPoints: [number, number, number][] = [];
 
-            if (clockwiseIsShorter) {
+            if (!clockwiseForbidden && (clockwiseDistance <= counterClockwiseDistance || counterClockwiseForbidden)) {
-                if (indexOfNearestStart <= indexOfNearestEnd) {
+                arcPoints = arcClockwise;
-                    arcPoints = circlePoints.slice(indexOfNearestStart, indexOfNearestEnd + 1);
             } else {
-                    arcPoints = [
+                arcPoints = arcCounterClockwise;
-                        ...circlePoints.slice(indexOfNearestStart, 64),
-                        ...circlePoints.slice(0, indexOfNearestEnd + 1)
-                    ];
-                }
-            } else {
-                if (indexOfNearestStart >= indexOfNearestEnd) {
-                    for (let i = indexOfNearestStart; i !== (indexOfNearestEnd - 1 + 64) % 64; i = (i - 1 + 64) % 64) {
-                        arcPoints.push(circlePoints[i]);
-                    }
-                } else {
-                    for (let i = indexOfNearestStart; i !== (indexOfNearestEnd - 1 + 64) % 64; i = (i - 1 + 64) % 64) {
-                        arcPoints.push(circlePoints[i]);
-                    }
-                }
             }
 
             const pathVectors = [
@@ -153,9 +187,7 @@ function RoboticArmAnimator({
                 new THREE.Vector3(end[0], end[1], end[2])
             ];
 
-
             const pathSegments: [THREE.Vector3, THREE.Vector3][] = [];
-
             for (let i = 0; i < pathVectors.length - 1; i++) {
                 pathSegments.push([pathVectors[i], pathVectors[i + 1]]);
             }
@@ -165,13 +197,7 @@ function RoboticArmAnimator({
             const totalDistance = segmentDistances.reduce((sum, d) => sum + d, 0);
             totalDistanceRef.current = totalDistance;
 
-            const movementSpeed = speed * armBot.speed;
-            const totalMoveTime = totalDistance / movementSpeed;
-
-            const segmentTimes = segmentDistances.map(distance => (distance / totalDistance) * totalMoveTime);
-
             setCustomCurvePoints(pathVectors);
-
         }
     }, [circlePoints, currentPath]);
 
@@ -182,7 +208,6 @@ function RoboticArmAnimator({
         const bone = ikSolver.mesh.skeleton.bones.find((b: any) => b.name === targetBone);
 
         if (!bone) return;
-
         if (isPlaying) {
             if (isReset) {
                 bone.position.copy(restPosition);
@@ -227,12 +252,17 @@ function RoboticArmAnimator({
                 ikSolver.update();
             }
         } else if (!isPlaying && currentPath.length === 0) {
+            progressRef.current = 0;
+            startTimeRef.current = null;
+            setCurrentPath([]);
+            setCustomCurvePoints([]);
             bone.position.copy(restPosition);
+
         }
         ikSolver.update();
     });
 
-
+    //Helper to Visible the Circle and Curve
     return (
         <>
             {customCurvePoints && customCurvePoints?.length >= 2 && currentPath && isPlaying && (
@@ -245,10 +275,18 @@ function RoboticArmAnimator({
                     />
                 </mesh>
             )}
-            <mesh position={[armBot.position[0], armBot.position[1] + 1.5, armBot.position[2]]} rotation={[-Math.PI / 2, 0, 0]}>
+            <group
+                position={[armBot.position[0], armBot.position[1] + 1.5, armBot.position[2]]}
+                rotation={[armBot.rotation[0], armBot.rotation[1], armBot.rotation[2]]}
+            >
+                {/* Green ring */}
+                <mesh rotation={[-Math.PI / 2, 0, 0]}>
                     <ringGeometry args={[CIRCLE_RADIUS, CIRCLE_RADIUS + 0.02, 64]} />
                     <meshBasicMaterial color="green" side={THREE.DoubleSide} />
                 </mesh>
+
+            </group>
+
         </>
     );
 }

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

@@ -5,7 +5,6 @@ import { usePauseButtonStore, usePlayButtonStore, useResetButtonStore } from '..
 import { useArmBotStore } from '../../../../../store/simulation/useArmBotStore';
 import armModel from "../../../../../assets/gltf-glb/rigged/ik_arm_1.glb";
 import { useThree } from "@react-three/fiber";
-import useModuleStore from '../../../../../store/useModuleStore';
 import * as THREE from "three";
 import MaterialAnimator from '../animator/materialAnimator';
 
@@ -24,16 +23,15 @@ function RoboticArmInstance({ armBot }: { armBot: ArmBotStatus }) {
     let startTime: number;
     //zustand
     const { addCurrentAction, setArmBotActive, setArmBotState, removeCurrentAction } = useArmBotStore();
-    const { activeModule } = useModuleStore();
     const { isPlaying } = usePlayButtonStore();
-    const { isReset, setReset } = useResetButtonStore();
+    const { isReset } = useResetButtonStore();
     const { isPaused } = usePauseButtonStore();
 
-
     function firstFrame() {
         startTime = performance.now();
         step();
     }
+
     function step() {
         if (isPausedRef.current) {
             if (!pauseTimeRef.current) {
@@ -87,6 +85,7 @@ function RoboticArmInstance({ armBot }: { armBot: ArmBotStatus }) {
             logStatus(armBot.modelUuid, "Moving armBot from end point to rest position.")
         }
     }
+
     useEffect(() => {
         isPausedRef.current = isPaused;
     }, [isPaused]);
@@ -98,6 +97,7 @@ function RoboticArmInstance({ armBot }: { armBot: ArmBotStatus }) {
             setArmBotState(armBot.modelUuid, "idle")
             setCurrentPhase("init");
             setPath([])
+            setIkSolver(null);
             removeCurrentAction(armBot.modelUuid)
             isPausedRef.current = false
             pauseTimeRef.current = null
@@ -117,17 +117,17 @@ function RoboticArmInstance({ armBot }: { armBot: ArmBotStatus }) {
     useEffect(() => {
         const targetMesh = scene?.getObjectByProperty("uuid", armBot.modelUuid);
         if (targetMesh) {
-            targetMesh.visible = activeModule !== "simulation"
+            targetMesh.visible = (!isPlaying)
         }
         const targetBones = ikSolver?.mesh.skeleton.bones.find((b: any) => b.name === targetBone);
-        if (isPlaying) {
+        if (!isReset && isPlaying) {
             //Moving armBot from initial point to rest position.
             if (!armBot?.isActive && armBot?.state == "idle" && currentPhase == "init") {
+                if (targetBones) {
                     setArmBotActive(armBot.modelUuid, true)
                     setArmBotState(armBot.modelUuid, "running")
                     setCurrentPhase("init-to-rest");
-                if (targetBones) {
+                    let curve = createCurveBetweenTwoPoints(targetBones.position, restPosition)
-                    let curve = createCurveBetweenTwoPoints(targetBones.position, targetBones.position)
                     if (curve) {
                         setPath(curve.points.map(point => [point.x, point.y, point.z]));
                     }
@@ -142,9 +142,9 @@ function RoboticArmInstance({ armBot }: { armBot: ArmBotStatus }) {
                 }, 3000);
                 return () => clearTimeout(timeoutId);
             }
+            //Moving to pickup point
             else if (armBot && !armBot.isActive && armBot.state === "idle" && currentPhase === "rest" && armBot.currentAction) {
                 if (armBot.currentAction) {
-
                     setArmBotActive(armBot.modelUuid, true);
                     setArmBotState(armBot.modelUuid, "running");
                     setCurrentPhase("rest-to-start");
@@ -158,15 +158,29 @@ function RoboticArmInstance({ armBot }: { armBot: ArmBotStatus }) {
                 }
                 logStatus(armBot.modelUuid, "Moving armBot from rest point to start position.")
             }
+            // Moving to Pick to Drop position
             else if (armBot && !armBot.isActive && armBot.state === "idle" && currentPhase === "picking" && armBot.currentAction) {
                 requestAnimationFrame(firstFrame);
             }
+            //Moving to drop point to restPosition
             else if (armBot && !armBot.isActive && armBot.state === "idle" && currentPhase === "dropping" && armBot.currentAction) {
                 requestAnimationFrame(firstFrame);
             }
+        } else {
+            logStatus(armBot.modelUuid, "Simulation Play Exited")
+            setArmBotActive(armBot.modelUuid, false)
+            setArmBotState(armBot.modelUuid, "idle")
+            setCurrentPhase("init");
+            setIkSolver(null);
+            setPath([])
+            isPausedRef.current = false
+            pauseTimeRef.current = null
+            isPausedRef.current = false
+            startTime = 0
+            removeCurrentAction(armBot.modelUuid)
         }
 
-    }, [currentPhase, armBot, isPlaying, ikSolver])
+    }, [currentPhase, armBot, isPlaying, isReset, ikSolver])
 
 
     function createCurveBetweenTwoPoints(p1: any, p2: any) {
@@ -212,10 +226,14 @@ function RoboticArmInstance({ armBot }: { armBot: ArmBotStatus }) {
     }
 
     return (
+        <>
+            {!isReset && isPlaying && (
                 <> 
                     <IKInstance modelUrl={armModel} setIkSolver={setIkSolver} ikSolver={ikSolver} armBot={armBot} groupRef={groupRef} />
                     <RoboticArmAnimator HandleCallback={HandleCallback} restPosition={restPosition} ikSolver={ikSolver} targetBone={targetBone} armBot={armBot}
                         logStatus={logStatus} path={path} currentPhase={currentPhase} />
+                 </>
+            )} 
             <MaterialAnimator ikSolver={ikSolver} armBot={armBot} currentPhase={currentPhase} />
         </>
     )

app/src/modules/simulation/roboticArm/instances/ikInstance/ikInstance.tsx

@@ -11,7 +11,7 @@ type IKInstanceProps = {
     modelUrl: string;
     ikSolver: any;
     setIkSolver: any
-    armBot: any;
+    armBot: ArmBotStatus;
     groupRef: any;
 };
 function IKInstance({ modelUrl, setIkSolver, ikSolver, armBot, groupRef }: IKInstanceProps) {
@@ -57,12 +57,6 @@ function IKInstance({ modelUrl, setIkSolver, ikSolver, armBot, groupRef }: IKIns
                         rotationMin: new THREE.Vector3(0, 0, 0),
                         rotationMax: new THREE.Vector3(2, 0, 0),
                     },
-                    // {
-                    //     index: 1,
-                    //     enabled: true,
-                    //     rotationMin: new THREE.Vector3(0, -Math.PI, 0),
-                    //     rotationMax: new THREE.Vector3(0, Math.PI, 0),
-                    // },
                     { index: 1, enabled: true, limitation: new THREE.Vector3(0, 1, 0) },
                     { index: 0, enabled: false, limitation: new THREE.Vector3(0, 0, 0) },
                 ],
@@ -76,7 +70,7 @@ function IKInstance({ modelUrl, setIkSolver, ikSolver, armBot, groupRef }: IKIns
 
         setSelectedArm(OOI.Target_Bone);
 
-        scene.add(helper);
+        // scene.add(helper);
 
     }, [cloned, gltf, setIkSolver]);
 
@@ -86,10 +80,10 @@ function IKInstance({ modelUrl, setIkSolver, ikSolver, armBot, groupRef }: IKIns
                 setSelectedArm(groupRef.current?.getObjectByName(targetBoneName))
             }}>
                 <primitive
-                    uuid={"ArmBot-X200"}
+                    uuid={armBot.modelUuid}
                     object={cloned}
                     scale={[1, 1, 1]}
-                    name={`arm-bot11`}
+                    name={armBot.modelName}
                 />
             </group>
             {/* {selectedArm && <TransformControls object={selectedArm} />} */}

app/src/modules/simulation/ui/arm/useDraggableGLTF.ts

@@ -121,31 +121,65 @@ export default function useDraggableGLTF(onUpdate: OnUpdateCallback) {
             // CONSTRAIN MOVEMENT HERE:
             const centerX = selectedArmBot.position[0];
             const centerZ = selectedArmBot.position[2];
-      const minDistance = 1.2; // 1.4 radius
+            const minDistance = 1.2;
-      const maxDistance = 2; // 2 radius
+            const maxDistance = 2;
 
-      const deltaX = targetPosition.x - centerX;
+            const delta = new THREE.Vector3(targetPosition.x - centerX, 0, targetPosition.z - centerZ);
-      const deltaZ = targetPosition.z - centerZ;
-      const distance = Math.sqrt(deltaX * deltaX + deltaZ * deltaZ);
 
-      if (distance < minDistance || distance > maxDistance) {
+            // Create quaternion from rotation
-        const angle = Math.atan2(deltaZ, deltaX);
+            const robotEuler = new THREE.Euler(selectedArmBot.rotation[0], selectedArmBot.rotation[1], selectedArmBot.rotation[2]);
-        const clampedDistance = Math.min(
+            const robotQuaternion = new THREE.Quaternion().setFromEuler(robotEuler);
-          Math.max(distance, minDistance),
+
-          maxDistance
+            // Inverse rotate
+            const inverseQuaternion = robotQuaternion.clone().invert();
+            delta.applyQuaternion(inverseQuaternion);
+
+            // Angle in robot local space
+            let relativeAngle = Math.atan2(delta.z, delta.x);
+            let angleDeg = (relativeAngle * 180) / Math.PI;
+            if (angleDeg < 0) {
+                angleDeg += 360;
+            }
+
+            // Clamp angle
+            if (angleDeg < 0 || angleDeg > 270) {
+                const distanceTo90 = Math.abs(angleDeg - 0);
+                const distanceTo270 = Math.abs(angleDeg - 270);
+                if (distanceTo90 < distanceTo270) {
+                    angleDeg = 0;
+                } else {
+                    return;
+                }
+                relativeAngle = (angleDeg * Math.PI) / 180;
+            }
+
+            // Distance clamp
+            const distance = delta.length();
+            const clampedDistance = Math.min(Math.max(distance, minDistance), maxDistance);
+
+            // Calculate local target
+            const finalLocal = new THREE.Vector3(
+                Math.cos(relativeAngle) * clampedDistance,
+                0,
+                Math.sin(relativeAngle) * clampedDistance
             );
 
-        targetPosition.x = centerX + Math.cos(angle) * clampedDistance;
+            // Rotate back to world space
-        targetPosition.z = centerZ + Math.sin(angle) * clampedDistance;
+            finalLocal.applyQuaternion(robotQuaternion);
-      }
+
+            targetPosition.x = centerX + finalLocal.x;
+            targetPosition.z = centerZ + finalLocal.z;
+
+
+            // Clamp Y axis if needed
             targetPosition.y = Math.min(Math.max(targetPosition.y, 0.6), 1.9);
-      // Convert world position to local if object is nested inside a parent
+
+            // Convert to local if parent exists
             if (parent) {
                 parent.worldToLocal(targetPosition);
             }
 
-      // Update object position
+            // Update the object position
-
             activeObjRef.current.position.copy(targetPosition);
         }
     };