public boolean solve(RigidBodyTransform desiredTransform) { currentBest = Double.MAX_VALUE; iterationNumber = 0; errorCourse.clear(); desiredReached = false; do { calculateErrorTransform(desiredTransform); minimizeError(); updateJointAngles(); monitorProgress(); desiredReached = correctionNearZero(); } while ((iterationNumber++ < maxIterations) &&!desiredReached); setBestJointAngles(); return desiredReached; }
private void monitorProgress() { double converge = 10; double spatialErrorNorm = NormOps.normP2(spatialError); errorCourse.add(spatialErrorNorm); int lastIndex = errorCourse.size(); int limit = 10; if (lastIndex >= limit) { converge = errorCourse.get(0) - errorCourse.get(limit - 1); errorCourse.remove(0); } if (converge < convergeRate) { introduceRandomPose(); } checkAndSetIfPoseIsBest(spatialErrorNorm); }
private void minimizeError() { jacobianMethod.set(getUpdatedJacobianMatrix()); spatialError.set(getSpatialErrorEndEffectorDesired()); jacobianOperation(solveMethod); CommonOps.mult(jacobianMethod, spatialError, correction); }
private void introduceRandomPose() { for (int i = 0; i < oneDoFJoints.length; i++) { double newQ = generateRandomDoubleInRange(oneDoFJoints[i].getJointLimitUpper(), oneDoFJoints[i].getJointLimitLower()); oneDoFJoints[i].setQ(newQ); } }
alpha = getDotProduct(spatialError, jJTe) / getDotProduct(jJTe, jJTe); CommonOps.transpose(jacobianMethod); CommonOps.scale(alpha, jacobianMethod);
private void introduceRandomPose() { for (int i = 0; i < oneDoFJoints.length; i++) { double newQ = generateRandomDoubleInRange(oneDoFJoints[i].getJointLimitUpper(), oneDoFJoints[i].getJointLimitLower()); oneDoFJoints[i].setQ(newQ); } }
alpha = getDotProduct(spatialError, jJTe) / getDotProduct(jJTe, jJTe); CommonOps.transpose(jacobianMethod); CommonOps.scale(alpha, jacobianMethod);
public boolean solve(RigidBodyTransform desiredTransform) { currentBest = Double.MAX_VALUE; iterationNumber = 0; errorCourse.clear(); desiredReached = false; do { calculateErrorTransform(desiredTransform); minimizeError(); updateJointAngles(); monitorProgress(); desiredReached = correctionNearZero(); } while ((iterationNumber++ < maxIterations) &&!desiredReached); setBestJointAngles(); return desiredReached; }
private void minimizeError() { jacobianMethod.set(getUpdatedJacobianMatrix()); spatialError.set(getSpatialErrorEndEffectorDesired()); jacobianOperation(solveMethod); CommonOps.mult(jacobianMethod, spatialError, correction); }
private void monitorProgress() { double converge = 10; double spatialErrorNorm = NormOps.normP2(spatialError); errorCourse.add(spatialErrorNorm); int lastIndex = errorCourse.size(); int limit = 10; if (lastIndex >= limit) { converge = errorCourse.get(0) - errorCourse.get(limit - 1); errorCourse.remove(0); } if (converge < convergeRate) { introduceRandomPose(); } checkAndSetIfPoseIsBest(spatialErrorNorm); }