From ff9bb8f5664001d1ea78b26ce654e2e8c350c5a6 Mon Sep 17 00:00:00 2001 From: Gomathi9520 Date: Wed, 30 Apr 2025 15:27:02 +0530 Subject: [PATCH] circular curve for arm movements added --- .../instances/animator/roboticArmAnimator.tsx | 244 ++++++++++++++---- .../instances/ikInstance/ikInstance.tsx | 11 +- .../simulation/roboticArm/roboticArm.tsx | 20 +- 3 files changed, 201 insertions(+), 74 deletions(-) diff --git a/app/src/modules/simulation/roboticArm/instances/animator/roboticArmAnimator.tsx b/app/src/modules/simulation/roboticArm/instances/animator/roboticArmAnimator.tsx index 6db275f..d1bf523 100644 --- a/app/src/modules/simulation/roboticArm/instances/animator/roboticArmAnimator.tsx +++ b/app/src/modules/simulation/roboticArm/instances/animator/roboticArmAnimator.tsx @@ -1,13 +1,13 @@ import React, { useEffect, useRef, useState } from 'react'; import { useFrame } from '@react-three/fiber'; import * as THREE from 'three'; +import { Line } from '@react-three/drei'; import { useAnimationPlaySpeed, usePauseButtonStore, usePlayButtonStore, useResetButtonStore } from '../../../../../store/usePlayButtonStore'; -import { Line } from '@react-three/drei'; function RoboticArmAnimator({ HandleCallback, @@ -22,24 +22,35 @@ function RoboticArmAnimator({ const curveRef = useRef(null); const [currentPath, setCurrentPath] = useState<[number, number, number][]>([]); const [curvePoints, setCurvePoints] = useState(null); + const [circlePoints, setCirclePoints] = useState<[number, number, number][]>([]); + const [nearestStartPoint, setNearestStartPoint] = useState<[number, number, number] | null>(null); + const [nearestEndPoint, setNearestEndPoint] = useState<[number, number, number] | null>(null); + const [customCurvePoints, setCustomCurvePoints] = useState(null); + const stepSegmentsRef = useRef([]); + const [currentStep, setCurrentStep] = useState(0); + const elapsedTimeRef = useRef(0); // Zustand stores const { isPlaying } = usePlayButtonStore(); const { isPaused } = usePauseButtonStore(); - const { isReset } = useResetButtonStore(); // optional use depending on your logic + const { isReset } = useResetButtonStore(); const { speed } = useAnimationPlaySpeed(); + // Update path state whenever `path` prop changes useEffect(() => { - setCurrentPath(path) - }, [path]) + setCurrentPath(path); + }, [path]); + + // Reset logic when `isPlaying` changes useEffect(() => { if (!isPlaying) { - setCurrentPath([]) - curveRef.current = null + setCurrentPath([]); + curveRef.current = null; } - }, [isPlaying]) + }, [isPlaying]); + // Handle path generation (including CatmullRomCurve3) useEffect(() => { if (currentPath && currentPath.length === 3) { const [start, mid, end] = currentPath; @@ -51,18 +62,152 @@ function RoboticArmAnimator({ new THREE.Vector3(end[0], end[1], end[2]) ]; const generatedCurve = new THREE.CatmullRomCurve3(points, false, 'centripetal', 1).getPoints(100); - // console.log('generatedCurve: ', generatedCurve); curveRef.current = generatedCurve; setCurvePoints(generatedCurve); - } }, [currentPath]); + // Handle circle points based on armBot position + useEffect(() => { + const radius = 1.6; // Circle radius + const segments = 64; + const points: [number, number, number][] = []; + + for (let i = 0; i <= segments; i++) { + const theta = (i / segments) * Math.PI * 2; + const x = radius * Math.cos(theta) + armBot.position[0]; + const z = radius * Math.sin(theta) + armBot.position[2]; + const y = armBot.position[1]; + points.push([x, y, z]); + } + + setCirclePoints(points); + }, [armBot.position]); + + // Handle nearest points and final path (including arc points) + useEffect(() => { + if (circlePoints.length > 0 && currentPath.length > 0) { + const startPoint = [currentPath[0][0], currentPath[0][1] + 0.5, currentPath[0][2]] as [number, number, number]; + const endPoint = [currentPath[currentPath.length - 1][0], currentPath[currentPath.length - 1][1] + 0.5, currentPath[currentPath.length - 1][2]] as [number, number, number]; + + const findNearest = (target: [number, number, number]) => { + let nearestPoint = circlePoints[0]; + let minDistance = Infinity; + + for (const point of circlePoints) { + const dx = target[0] - point[0]; + const dy = target[1] - point[1]; + const dz = target[2] - point[2]; + const distance = Math.sqrt(dx * dx + dy * dy + dz * dz); + if (distance < minDistance) { + minDistance = distance; + nearestPoint = point; + } + } + return nearestPoint; + }; + + const nearestToStart = findNearest(startPoint); + const nearestToEnd = findNearest(endPoint); + + setNearestStartPoint(nearestToStart); + setNearestEndPoint(nearestToEnd); + + const radius = 1.6; + const segments = 64; + const center = armBot.position; + + const startAngle = Math.atan2(nearestToStart[2] - center[2], nearestToStart[0] - center[0]); + const endAngle = Math.atan2(nearestToEnd[2] - center[2], nearestToEnd[0] - center[0]); + let deltaAngle = endAngle - startAngle; + + if (deltaAngle > Math.PI) { + deltaAngle -= 2 * Math.PI; + } else if (deltaAngle < -Math.PI) { + deltaAngle += 2 * Math.PI; + } + + const arcPoints: [number, number, number][] = []; + + for (let i = 0; i <= segments; i++) { + const t = i / segments; + const angle = startAngle + deltaAngle * t; + const x = center[0] + radius * Math.cos(angle); + const z = center[2] + radius * Math.sin(angle); + const y = startPoint[1]; + arcPoints.push([x, y, z]); + } + const start = currentPath[0]; + const end = currentPath[currentPath.length - 1]; + + let step1 = [ + [start[0], start[1], start[2]], + [start[0], start[1] + 0.5, start[2]] + ]; + let step2 = [ + [start[0], start[1] + 0.5, start[2]], + [nearestToStart[0], start[1] + 0.5, nearestToStart[2]] + ]; + let step3 = [ + [nearestToStart[0], start[1] + 0.5, nearestToStart[2]], + [end[0], arcPoints[arcPoints.length - 1][1], end[2]] + ] + let step4 = [ + [end[0], arcPoints[arcPoints.length - 1][1], end[2]], + [end[0], end[1] + 0.5, end[2]] + ] + let step5 = [ + [end[0], end[1] + 0.5, end[2]], + [end[0], end[1], end[2]], + ] + + const points = [ + new THREE.Vector3(start[0], start[1], start[2]), + new THREE.Vector3(start[0], start[1] + 0.5, start[2]), + + new THREE.Vector3(start[0], start[1] + 0.5, start[2]), + new THREE.Vector3(nearestToStart[0], start[1] + 0.5, nearestToStart[2]), + + new THREE.Vector3(nearestToStart[0], start[1] + 0.5, nearestToStart[2]), + new THREE.Vector3(end[0], arcPoints[arcPoints.length - 1][1], end[2]), + + new THREE.Vector3(end[0], arcPoints[arcPoints.length - 1][1], end[2]), + new THREE.Vector3(end[0], end[1] + 0.5, end[2]), + + new THREE.Vector3(end[0], end[1] + 0.5, end[2]), + new THREE.Vector3(end[0], end[1], end[2]) + ]; + const steps = [ + [points[0], points[1]], // step1 + [points[2], points[3]], // step2 + [points[4], points[5]], // step3 + [points[6], points[7]], // step4 + [points[8], points[9]] // step5 + ]; + stepSegmentsRef.current = steps; + setCurrentStep(0); + elapsedTimeRef.current = 0; + + const finalPath = [ + new THREE.Vector3(start[0], start[1], start[2]), + new THREE.Vector3(start[0], start[1] + 0.5, start[2]), + new THREE.Vector3(nearestToStart[0], start[1] + 0.5, nearestToStart[2]), + ...arcPoints.map(([x, y, z]) => new THREE.Vector3(x, y, z)), + new THREE.Vector3(end[0], arcPoints[arcPoints.length - 1][1], end[2]), + new THREE.Vector3(end[0], end[1], end[2]) + ]; + + const customCurve = new THREE.CatmullRomCurve3(finalPath, false, 'centripetal', 1); + const customCurveGeneratedPoints = customCurve.getPoints(100); + + setCustomCurvePoints(customCurveGeneratedPoints); + } + }, [circlePoints, currentPath]); + + // Frame update for animation useFrame((_, delta) => { if (ikSolver && curveRef.current && currentPath.length >= 0) { - const bone = ikSolver.mesh.skeleton.bones.find( - (b: any) => b.name === targetBone - ); + const bone = ikSolver.mesh.skeleton.bones.find((b: any) => b.name === targetBone); if (!bone) return; if (isPlaying) { @@ -70,71 +215,62 @@ function RoboticArmAnimator({ const curvePoints = curveRef.current; const speedAdjustedProgress = progressRef.current + (speed * armBot.speed); const index = Math.floor(speedAdjustedProgress); - if (index >= curvePoints.length) { - HandleCallback(); - setCurrentPath([]); - curveRef.current = null; - progressRef.current = 0; - } else { - const point = curvePoints[index]; - bone.position.copy(point); - progressRef.current = speedAdjustedProgress; + + if (curvePoints) { + + if (index >= curvePoints.length) { + HandleCallback(); + setCurrentPath([]); + curveRef.current = null; + progressRef.current = 0; + } else { + const point = curvePoints[index]; + bone.position.copy(point); + progressRef.current = speedAdjustedProgress; + } } + + } else { + logStatus(armBot.modelUuid, 'Simulation Play Exited'); } - } else { - logStatus(armBot.modelUuid, 'Simulation Play Exited'); - // bone.position.copy(restPosition); + + ikSolver.update(); } - - ikSolver.update(); } else if (ikSolver && !isPlaying && currentPath.length === 0) { - const bone = ikSolver.mesh.skeleton.bones.find( - (b: any) => b.name === targetBone - ); - // bone.position.copy(restPosition); + const bone = ikSolver.mesh.skeleton.bones.find((b: any) => b.name === targetBone); + bone.position.copy(restPosition); ikSolver.update(); - }; - + } }); return ( + <> - {/* {currentPath.length > 0 && ( + {customCurvePoints && currentPath && ( - - {currentPath.map((point, index) => ( - - - - - ))} - - )} */} - {curvePoints && ( - - [p.x, p.y, p.z])} - color="green" - lineWidth={5} - dashed={false} - /> + [p.x, p.y, p.z])} color="green" lineWidth={5} dashed={false} /> + {/* */} {currentPath.length >= 1 && ( <> {/* First point */} - + {/* - + */} {/* Last point */} - + {/* - + */} )} )} + + + + ); } diff --git a/app/src/modules/simulation/roboticArm/instances/ikInstance/ikInstance.tsx b/app/src/modules/simulation/roboticArm/instances/ikInstance/ikInstance.tsx index d818777..c40bc92 100644 --- a/app/src/modules/simulation/roboticArm/instances/ikInstance/ikInstance.tsx +++ b/app/src/modules/simulation/roboticArm/instances/ikInstance/ikInstance.tsx @@ -67,21 +67,20 @@ function IKInstance({ modelUrl, setIkSolver, ikSolver, armBot, groupRef }: IKIns setIkSolver(solver); const helper = new CCDIKHelper(OOI.Skinned_Mesh, iks, 0.05) - groupRef.current.add(helper); - console.log('OOI.Target_Bone: ', OOI.Target_Bone); + // groupRef.current.add(helper); + setSelectedArm(OOI.Target_Bone); - scene.add(helper) + // scene.add(helper) }, [gltf]); return ( <> - { + { setSelectedArm(groupRef.current?.getObjectByName(targetBoneName)) - } - }> + }}>