/** * Compute the gradient of q times the p-norm |q - q_midRange|_p * @param privilegedJointVelocitiesToPack * @param weight */ private void computePrivilegedVelocitiesForStayingAwayFromJointLimits(DenseMatrix64F privilegedJointVelocitiesToPack, double weight) { int p = exponentForPNorm.getIntegerValue(); double sumOfPows = 0.0; double pThRootOfSumOfPows = 0.0; for (int i = 0; i < numberOfDoF; i++) { sumOfPows += MathTools.powWithInteger(Math.abs(localJoints[i].getQ() - jointAnglesAtMidRangeOfMotion.get(i, 0)), p); } pThRootOfSumOfPows = Math.pow(sumOfPows, 1.0 / ((double) p)); for (int i = 0; i < numberOfDoF; i++) { double numerator = MathTools.powWithInteger(Math.abs(localJoints[i].getQ() - jointAnglesAtMidRangeOfMotion.get(i, 0)), p - 1) * pThRootOfSumOfPows; double qDotPrivileged = -weight * numerator / sumOfPows; privilegedJointVelocitiesToPack.set(i, 0, qDotPrivileged); } }
/** * I use the developed equations to compute the Bezier basis functions. It should be faster. * */ private void updateBezierCoefficients(double time) { time *= trajectoryTimeScale.getDoubleValue(); double timeSquare = time * time; double timeCube = timeSquare * time; cumulativeBeziers[1].set(1 - MathTools.powWithInteger(1 - time, 3)); cumulativeBeziers[2].set(3.0 * timeSquare - 2 * timeCube); cumulativeBeziers[3].set(timeCube); }
private void getParameters(ArrayList<Double> time, ArrayList<Vector3d> waypoints, double[] parametersXToPack, double[] parametersYToPack, double[] parametersZToPack) { for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { timeMatrix.setElement(i, j, MathTools.powWithInteger(time.get(i), 2 - j)); } } System.out.println(timeMatrix); timeMatrix.invert(); Vector3d tempVector = new Vector3d(); for (int i = 0; i < 3; i++) { timeMatrix.getRow(i, tempVector); parametersXToPack[i] = (tempVector.dot(new Vector3d(waypoints.get(0).getX(), waypoints.get(1).getX(), waypoints.get(2).getX()))); } for (int i = 0; i < 3; i++) { timeMatrix.getRow(i, tempVector); parametersYToPack[i] = (tempVector.dot(new Vector3d(waypoints.get(0).getY(), waypoints.get(1).getY(), waypoints.get(2).getY()))); } for (int i = 0; i < 3; i++) { timeMatrix.getRow(i, tempVector); parametersZToPack[i] = (tempVector.dot(new Vector3d(waypoints.get(0).getZ(), waypoints.get(1).getZ(), waypoints.get(2).getZ()))); } }