Hi, I am trying to make a robot arm simulation. In this I have to create translation and rotational movement. I have written the code for translation and its working fine but I am confused how to start the code of rotational code. Please help me if you know anything regarding the same. I am adding my translation code below.
Code for IK
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++) {
x
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
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';
}
