private void computePrivilegedJointVelocitiesForPriviligedJointAngles(DenseMatrix64F privilegedJointVelocitiesToPack, double weight) { for (int i = 0; i < numberOfDoF; i++) { yoPrivilegedJointPositionsFiltered[i].update(); privilegedJointVelocitiesToPack.set(i, 0, -2.0 * weight * (localJoints[i].getQ() - yoPrivilegedJointPositionsFiltered[i].getDoubleValue()) / jointSquaredRangeOfMotions.get(i, 0)); } }
@Override public void notifyOfVariableChange(YoVariable<?> v) { alphaFilteredHeadYawPercentage.update(headYawPercentage.getDoubleValue()); NeckJointName headYaw = NeckJointName.DISTAL_NECK_YAW; double neckYawJointRange = sliderBoardControlledNeckJointsWithLimits.get(headYaw).getRight() - sliderBoardControlledNeckJointsWithLimits.get(headYaw).getLeft(); YoDouble desiredAngle = (YoDouble) registry.getVariable(drcRobotModel.getJointMap().getNeckJointName(headYaw) + "_unconstrained" + CommonNames.q_d); desiredAngle.set(alphaFilteredHeadYawPercentage.getDoubleValue() * neckYawJointRange + sliderBoardControlledNeckJointsWithLimits.get(headYaw).getLeft()); } });
@Override public void notifyOfVariableChange(YoVariable<?> v) { alphaFilteredUpperHeadPitchPercentage.update(upperHeadPitchPercentage.getDoubleValue()); NeckJointName upperHeadPitch = NeckJointName.DISTAL_NECK_PITCH; double jointRange = sliderBoardControlledNeckJointsWithLimits.get(upperHeadPitch).getRight() - sliderBoardControlledNeckJointsWithLimits.get(upperHeadPitch).getLeft(); YoDouble desiredAngle = (YoDouble) registry.getVariable(drcRobotModel.getJointMap().getNeckJointName(upperHeadPitch) + "_unconstrained" + CommonNames.q_d); desiredAngle.set(alphaFilteredUpperHeadPitchPercentage.getDoubleValue() * jointRange + sliderBoardControlledNeckJointsWithLimits.get(upperHeadPitch).getLeft()); } });
@Override public void notifyOfVariableChange(YoVariable<?> v) { alphaFilteredLowerHeadPitchYawPercentage.update(lowerHeadPitchPercentage.getDoubleValue()); NeckJointName lowerHeadPitch = NeckJointName.PROXIMAL_NECK_PITCH; double jointRange = sliderBoardControlledNeckJointsWithLimits.get(lowerHeadPitch).getRight() - sliderBoardControlledNeckJointsWithLimits.get(lowerHeadPitch).getLeft(); YoDouble desiredAngle = (YoDouble) registry.getVariable(drcRobotModel.getJointMap().getNeckJointName(lowerHeadPitch) + "_unconstrained" + CommonNames.q_d); desiredAngle.set(alphaFilteredLowerHeadPitchYawPercentage.getDoubleValue() * jointRange + sliderBoardControlledNeckJointsWithLimits.get(lowerHeadPitch).getLeft()); } });
@Override public void variableChanged(YoVariable<?> v) { alphaFilteredHeadYawPercentage.update(headYawPercentage.getDoubleValue()); NeckJointName headYaw = NeckJointName.DISTAL_NECK_YAW; double neckYawJointRange = sliderBoardControlledNeckJointsWithLimits.get(headYaw).getRight() - sliderBoardControlledNeckJointsWithLimits.get(headYaw).getLeft(); DoubleYoVariable desiredAngle = (DoubleYoVariable) registry.getVariable(drcRobotModel.getJointMap().getNeckJointName(headYaw) + "_unconstrained" + CommonNames.q_d); desiredAngle.set(alphaFilteredHeadYawPercentage.getDoubleValue() * neckYawJointRange + sliderBoardControlledNeckJointsWithLimits.get(headYaw).getLeft()); } });
@Override public void variableChanged(YoVariable<?> v) { alphaFilteredLowerHeadPitchYawPercentage.update(lowerHeadPitchPercentage.getDoubleValue()); NeckJointName lowerHeadPitch = NeckJointName.PROXIMAL_NECK_PITCH; double jointRange = sliderBoardControlledNeckJointsWithLimits.get(lowerHeadPitch).getRight() - sliderBoardControlledNeckJointsWithLimits.get(lowerHeadPitch).getLeft(); DoubleYoVariable desiredAngle = (DoubleYoVariable) registry.getVariable(drcRobotModel.getJointMap().getNeckJointName(lowerHeadPitch) + "_unconstrained" + CommonNames.q_d); desiredAngle.set(alphaFilteredLowerHeadPitchYawPercentage.getDoubleValue() * jointRange + sliderBoardControlledNeckJointsWithLimits.get(lowerHeadPitch).getLeft()); } });