Hi, I am developing a robot arm simulation that requires both translation and rotational movements.
This simulation’s translation code works as expected (see code below), but I’m unsure how to begin coding the rotational movement. Any guidance or examples on implementing rotation would be greatly appreciated.

Code for IK

JAVASCRIPTCopied
import { Vector3 } from 'three'

class CCDIK {
    solve(target, robot, joints, {
        maxIterations = 1,
        stopDistance = 0.001,
        apply = false
    } = {}) {
        let movable = [];
        // get the movable joints
        if (joints.length) {
            for (let joint of robot.arm.movable) {
                if (joints.includes(joint.name)) {
                    movable.push(joint);
                }
            }
        } else {
            movable = robot.arm.movable;
        }
        const orignalMovment = {};
        const solution = {};

        let targetPosition = new Vector3();
        let tipPosition = new Vector3();
        let deltaNormal = new Vector3();

        for (const joint of movable) {
            orignalMovment[joint.name] = joint.angle;
        }

        target.getWorldPosition(targetPosition);
    
        for (let iter = 0; iter < maxIterations; iter++) {
            for (let i = movable.length - 1; i >= 0; i--) {
                const joint = movable[i];
              
                robot.tcp.object.getWorldPosition(tipPosition);
              
                let tcpDirection = joint.worldToLocal(tipPosition.clone()).normalize(); // Vector from the joint to the current TCP.
                let targetDirection = joint.worldToLocal(targetPosition.clone()).normalize(); // Vector from the joint to the target position.
                // Project direction vectors onto the joint's rotation plane
                tcpDirection = tcpDirection.projectOnPlane(joint.axis);
                targetDirection = targetDirection.projectOnPlane(joint.axis);
                // deltaNormal is the cross product of the direction vectors, 
                // providing a normal vector to the plane formed by these vectors
                deltaNormal.crossVectors(tcpDirection, targetDirection).normalize();
                // Depending on which direction we have to turn, the rotation axes will be parallel or anti-parallel
                let alignment = deltaNormal.dot(joint.axis);
                let delta = Math.sign(alignment) * tcpDirection.angleTo(targetDirection);
                let angle = joint.angle + delta;
      
                const limit = joint.limit;
  
                if (limit) {
                    if ('lower' in limit && angle < limit['lower']) {
                        angle = Math.max(limit['lower'], angle);
                    }
                    if ('upper' in limit && angle > limit['upper']) {
                        angle = Math.min(limit['upper'], angle);
                    }
                }

                joint.setJointValue(angle);
                solution[joint.name] = joint.angle;
            }

            robot.tcp.object.getWorldPosition(tipPosition);
            if (targetPosition.distanceTo(tipPosition) < stopDistance) {
                console.log('IK solution found after ' + iter + ' iterations');
                break;
            }
        }

        if (!apply) {
            // Reset all movable joints to their original angles 
            for (const joint of movable) {
                joint.setJointValue(orignalMovment[joint.name]);
            }
        }
        return solution;
    }
};

export default CCDIK;

Code for robot controller

JAVASCRIPTCopied
import * as UIL from "uil";
import { default as IKSolver } from "../ik/ccdik";
import * as THREE from 'three'

var gui = null;
var ik = null;

export function initRoboController(robot, renderCall, scene, tcptarget) {
    gui = new UIL.Gui(
        { css: 'position: absolute; right: 0px;top:150px;  width: 350px; z-index: 98;overflow:auto;background: rgba(0,0,0,0.8);' }
    );
    gui.content.id = 'robo-controller';
    ik = new IKSolver(scene, robot);

    gui.add('title', { name: 'Controller', prefix: 'v1.0' });
    // translation controls
    gui.add('slide', { name: 'Move X', min: -1, max: 1, value: 0 }).onChange((v) => {
        moveTCP(robot, v, null, null);
    });
    gui.add('slide', { name: 'Move Y', min: -1, max: 1, value: 0 }).onChange((v) => {
        moveTCP(robot, null, v, null);
    });
    gui.add('slide', { name: 'Move Z', min: -1, max: 1, value: 0 }).onChange((v) => {
        moveTCP(robot, null, null, v);
    });
    // rotational control
    gui.add('slide', { name: 'Roll(X)', min: -180, max: 180, value: 0 }).onChange((v) => {
        rotateTCP(robot, v, null, null);
    });
    gui.add('slide', { name: 'Pitch(Y)', min: -180, max: 180, value: 0 }).onChange((v) => {
        rotateTCP(robot, null, v, null);
    });
    gui.add('slide', { name: 'Yaw(Z)', min: -180, max: 180, value: 0 }).onChange((v) => {
        rotateTCP(robot, null, null, v);
    });

    function moveTCP(robot, x, y, z) {
        let minYValue = -3.14159265
        robot.tcp.object.getWorldPosition(tcptarget.position);
        // Update TCP target position
        if (x !== null) tcptarget.position.x = x * 10;
        if (y !== null) tcptarget.position.y = Math.max(y * 10, minYValue);;
        if (z !== null) tcptarget.position.z = z * 10;

        // Solve inverse kinematics to find new joint angles
        if (typeof ik.solve === 'function') {
            ik.solve(
                tcptarget,
                robot,
                robot.ikEnabled,
                {
                    // we don't have to be all that precise here
                    maxIterations: 10,
                    stopDistance: 0.01,
                    apply: true
                }
            );
        } else {
            console.warn("IK solver function 'solveIK' is missing!");
        }
        renderCall();
    }

    function rotateTCP(robot, roll, pitch, yaw) {
        let euler = new THREE.Euler(
            roll !== null ? THREE.MathUtils.degToRad(roll) : tcptarget.rotation.x,
            pitch !== null ? THREE.MathUtils.degToRad(pitch) : tcptarget.rotation.y,
            yaw !== null ? THREE.MathUtils.degToRad(yaw) : tcptarget.rotation.z,
            "XYZ"
        );

        tcptarget.quaternion.setFromEuler(euler); // Apply rotation to TCP

        console.log("New TCP Rotation: ", euler);

        // Solve IK for rotation
        if (typeof ik.solve === 'function') {
            ik.solve(
                tcptarget,
                robot,
                robot.ikEnabled,
                {
                    maxIterations: 10,
                    stopDistance: 0.01,
                    apply: true
                }
            );
        } else {
            console.warn("IK solver function 'solveIK' is missing!");
        }
        renderCall(); // Update the scene
    }

    gui.isOpen = true;
    gui.setHeight();
    gui.bottomText = ['Move', 'Close'];
    gui.bottom.textContent = 'Move';
}