private void setBestJointAngles() { for (int i = 0; i < oneDoFJoints.length; i++) { oneDoFJoints[i].setQ(bestJointAngles[i]); } }
private void readAndUpdateOneDoFJointPositionsVelocitiesAndAccelerations() { for (int i = 0; i < revoluteJoints.size(); i++) { ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair = revoluteJoints.get(i); OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJoint revoluteJoint = jointPair.getRight(); if (pinJoint == null) continue; revoluteJoint.setQ(pinJoint.getQYoVariable().getDoubleValue()); revoluteJoint.setQd(pinJoint.getQDYoVariable().getDoubleValue()); revoluteJoint.setQdd(pinJoint.getQDDYoVariable().getDoubleValue()); } }
private void updateDesiredFullRobotModelState() { RootJointDesiredConfigurationDataReadOnly outputForRootJoint = wholeBodyInverseKinematicsSolver.getOutputForRootJoint(); desiredRootJoint.setConfiguration(outputForRootJoint.getDesiredConfiguration(), 0); desiredRootJoint.setVelocity(outputForRootJoint.getDesiredVelocity(), 0); LowLevelOneDoFJointDesiredDataHolderReadOnly output = wholeBodyInverseKinematicsSolver.getOutput(); for (OneDoFJoint joint : oneDoFJoints) { if (output.hasDataForJoint(joint)) { joint.setQ(output.getDesiredJointPosition(joint)); joint.setqDesired(output.getDesiredJointPosition(joint)); joint.setQd(output.getDesiredJointVelocity(joint)); } } }
@Override public void setConfiguration(DenseMatrix64F matrix, int rowStart) { setQ(matrix.get(rowStart, 0)); }
@Override public void setJointPositionVelocityAndAcceleration(InverseDynamicsJoint originalJoint) { OneDoFJoint oneDoFOriginalJoint = checkAndGetAsOneDoFJoint(originalJoint); setQ(oneDoFOriginalJoint.getQ()); setQd(oneDoFOriginalJoint.getQd()); setQdd(oneDoFOriginalJoint.getQdd()); setTauMeasured(oneDoFOriginalJoint.getTauMeasured()); setEnabled(oneDoFOriginalJoint.isEnabled()); }
private void updateDesiredFullRobotModelState() { RootJointDesiredConfigurationDataReadOnly outputForRootJoint = wholeBodyInverseKinematicsSolver.getOutputForRootJoint(); desiredRootJoint.setConfiguration(outputForRootJoint.getDesiredConfiguration(), 0); desiredRootJoint.setVelocity(outputForRootJoint.getDesiredVelocity(), 0); LowLevelOneDoFJointDesiredDataHolderReadOnly output = wholeBodyInverseKinematicsSolver.getOutput(); for (OneDoFJoint joint : oneDoFJoints) { if (output.hasDataForJoint(joint)) { joint.setQ(output.getDesiredJointPosition(joint)); joint.setqDesired(output.getDesiredJointPosition(joint)); joint.setQd(output.getDesiredJointVelocity(joint)); } } }
public void introduceRandomArmePose(RigidBodyTransform desiredTransform) { for (int i = 0; i < oneDoFJoints.length; i++) { oneDoFJoints[i].setQ(random.nextDouble()); } solve(desiredTransform); }
private void updateJointPositions() { for (int i = 0; i < numJoints; i++) { ikJoints[i].setQ(robotJoints[i].getQ()); } }
public static void setRandomPosition(OneDoFJoint joint, Random random, double boundaryOne, double boundaryTwo) { joint.setQ(RandomTools.generateRandomDouble(random, boundaryOne, boundaryTwo)); }
public void setJointAngles(DenseMatrix64F angles) { for (int i = 0; i < angles.getNumRows(); i++) { oneDoFJoints[i].setQ(angles.get(i, 0)); } }
private void setJointAngles(DenseMatrix64F angles) { for (int i = 0; i < angles.getNumRows(); i++) { oneDoFJoints[i].setQ(angles.get(i, 0)); } }
public static void integrateVelocity(OneDoFJoint joint, double dt) { double oldQ = joint.getQ(); double deltaQ = joint.getQd() * dt; double newQ = oldQ + deltaQ; joint.setQ(newQ); }
private void introduceRandomPose() { for (int i = 0; i < oneDoFJoints.length; i++) { double newQ = generateRandomDoubleInRange(oneDoFJoints[i].getJointLimitUpper(), oneDoFJoints[i].getJointLimitLower()); oneDoFJoints[i].setQ(newQ); } }
private void setLocalJointAnglesToDesiredJointAngles() { for (int i = 0; i < numberOfDoF; i++) { localJoints[i].setQ(originalJoints[i].getqDesired()); localJoints[i].getFrameAfterJoint().update(); } }
private void setLocalJointAnglesToCurrentJointAngles() { for (int i = 0; i < numberOfDoF; i++) { localJoints[i].setQ(originalJoints[i].getQ()); localJoints[i].getFrameAfterJoint().update(); } }
private void updateState(double[] parameters) { for (int i = 0; i < parameters.length; i++) { OneDoFJoint oneDoFJoint = oneDoFJoints[i]; // apply constraints to the input parameters double newQ = UtilAngle.bound(parameters[i]); if (Double.isNaN(newQ)) continue; newQ = parameters[i] = MathTools.clipToMinMax(newQ, oneDoFJoint.getJointLimitLower(), oneDoFJoint.getJointLimitUpper()); oneDoFJoint.setQ(newQ); oneDoFJoint.getFrameAfterJoint().update(); } }
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 updateJointAngles() { for (int i = 0; i < oneDoFJoints.length; i++) { double newQ = oneDoFJoints[i].getQ() + correction.get(i, 0); newQ = MathTools.clipToMinMax(newQ, oneDoFJoints[i].getJointLimitLower(), oneDoFJoints[i].getJointLimitUpper()); oneDoFJoints[i].setQ(newQ); oneDoFJoints[i].getFrameAfterJoint().update(); } }
private void applyCorrection() { for (int i = 0; i < oneDoFJoints.length; i++) { OneDoFJoint oneDoFJoint; if (useSeed) { oneDoFJoint = oneDoFJointsSeed[i]; } else { oneDoFJoint = oneDoFJoints[i]; } double newQ = oneDoFJoint.getQ() - correction.get(i, 0); newQ = MathTools.clipToMinMax(newQ, oneDoFJoint.getJointLimitLower(), oneDoFJoint.getJointLimitUpper()); oneDoFJoint.setQ(newQ); oneDoFJoint.getFrameAfterJoint().update(); } }
public void updateFullRobotModel(KinematicsToolboxOutputStatus solution) { if (jointsHashCode != solution.jointNameHash) throw new RuntimeException("Hashes are different."); for (int i = 0; i < oneDoFJoints.length; i++) { float q = solution.getJointAngles()[i]; OneDoFJoint joint = oneDoFJoints[i]; joint.setQ(q); } Vector3f translation = solution.getPelvisTranslation(); rootJoint.setPosition(translation.getX(), translation.getY(), translation.getZ()); Quat4f orientation = solution.getPelvisOrientation(); rootJoint.setRotation(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getW()); fullRobotModelToUseForConversion.updateFrames(); }