private void applyJointAngleCorrection() { for (int i = 0; i < oneDoFJoints.length; i++) { OneDoFJointBasics oneDoFJoint = oneDoFJoints[i]; double newQ = oneDoFJoint.getQ() - jointAnglesCorrection.get(i, 0); if (limitJointAngles) newQ = Math.min(oneDoFJoint.getJointLimitUpper(), Math.max(newQ, oneDoFJoint.getJointLimitLower())); oneDoFJoint.setQ(newQ); oneDoFJoint.getFrameAfterJoint().update(); } if (stepListener != null) { stepListener.didAnInverseKinemticsStep(errorScalar); } }
private void applyJointAngleCorrection() { for (int i = 0; i < oneDoFJoints.length; i++) { OneDoFJoint oneDoFJoint = oneDoFJoints[i]; double newQ = oneDoFJoint.getQ() - jointAnglesCorrection.get(i, 0); if (limitJointAngles) newQ = Math.min(oneDoFJoint.getJointLimitUpper(), Math.max(newQ, oneDoFJoint.getJointLimitLower())); oneDoFJoint.setQ(newQ); oneDoFJoint.getFrameAfterJoint().update(); } if (stepListener != null) { stepListener.didAnInverseKinemticsStep(errorScalar); } }
private void solveForArmPoseWithInverseKinematics(Random random, FrameQuaternion desiredOrientation, FramePoint3D desiredPosition, InitialGuessForTests initialGuessForTests, boolean updateListenersEachStep) { FramePose3D handPose = new FramePose3D(worldFrame); handPose.setOrientation(desiredOrientation); handPose.setPosition(desiredPosition); handPose.changeFrame(fullRobotModel.getChest().getBodyFixedFrame()); RigidBodyTransform transform = new RigidBodyTransform(); handPose.get(transform); createInitialGuess(random, initialGuessForTests); if (VISUALIZE && updateListenersEachStep) { inverseKinematicsStepListener.didAnInverseKinemticsStep(0.0); } long start = System.nanoTime(); solve(transform); long end = System.nanoTime(); solvingTime.add((long) ((end - start) * 1e-6)); }
inverseKinematicsStepListener.didAnInverseKinemticsStep(-1.0);
stepListener.didAnInverseKinemticsStep(errorScalar);
stepListener.didAnInverseKinemticsStep(errorScalar);