public JointLimit(OneDoFJoint oneDoFJoint) { upperPositionLimit = oneDoFJoint.getJointLimitUpper(); lowerPositionLimit = oneDoFJoint.getJointLimitLower(); rangeOfMotion = upperPositionLimit - lowerPositionLimit; softLimitPercentageOfFullRangeOfMotion = 1.0; softUpperPositionLimit = upperPositionLimit; softLowerPositionLimit = lowerPositionLimit; softLimitRangeOfMotion = softUpperPositionLimit - softLowerPositionLimit; velocityLimit = Double.POSITIVE_INFINITY; torqueLimit = Double.POSITIVE_INFINITY; }
public RobotJointLimitWatcherForUI(OneDoFJoint[] oneDoFJoints) { super(oneDoFJoints); int numberOfJoints = oneDoFJoints.length; closeToLimitCheckers = new YoVariableLimitChecker[numberOfJoints]; for (int i = 0; i < numberOfJoints; i++) { OneDoFJoint oneDoFJoint = oneDoFJoints[i]; double thresholdPercentage = 0.02; double range = oneDoFJoint.getJointLimitUpper() - oneDoFJoint.getJointLimitLower(); double thresholdAmount = range * thresholdPercentage; double lowerLimit = oneDoFJoint.getJointLimitLower() + thresholdAmount + (range * 0.1); double upperLimit = oneDoFJoint.getJointLimitUpper() - thresholdAmount - (range * 0.1); closeToLimitCheckers[i] = new YoVariableLimitChecker(variablesToTrack[i], "approaching", lowerLimit, upperLimit, registry); } }
public static void setRandomPositionWithinJointLimits(OneDoFJoint joint, Random random) { setRandomPosition(joint, random, joint.getJointLimitLower(), joint.getJointLimitUpper()); }
public static PlaybackPose createRandomPosePlaybackRobotPose(Random random, FullRobotModel fullRobotModel, double poseDelay, double trajectoryTime) { LinkedHashMap<OneDoFJoint, Double> pose = new LinkedHashMap<>(); OneDoFJoint[] oneDoFJoints = fullRobotModel.getOneDoFJoints(); for (OneDoFJoint joint : oneDoFJoints) { double jointLimitLower = joint.getJointLimitLower(); double jointLimitUpper = joint.getJointLimitUpper(); if (jointLimitLower < -Math.PI) jointLimitLower = -Math.PI; if (jointLimitUpper > Math.PI) jointLimitUpper = Math.PI; pose.put(joint, RandomTools.generateRandomDouble(random, jointLimitLower, jointLimitUpper)); } return new PlaybackPose(pose, poseDelay, trajectoryTime); } }
public static PlaybackPose createRandomPosePlaybackRobotPose(Random random, FullRobotModel fullRobotModel, double poseDelay, double trajectoryTime) { LinkedHashMap<OneDoFJoint, Double> pose = new LinkedHashMap<>(); OneDoFJoint[] oneDoFJoints = fullRobotModel.getOneDoFJoints(); for (OneDoFJoint joint : oneDoFJoints) { double jointLimitLower = joint.getJointLimitLower(); double jointLimitUpper = joint.getJointLimitUpper(); if (jointLimitLower < -Math.PI) jointLimitLower = -Math.PI; if (jointLimitUpper > Math.PI) jointLimitUpper = Math.PI; pose.put(joint, RandomTools.generateRandomDouble(random, jointLimitLower, jointLimitUpper)); } return new PlaybackPose(pose, poseDelay, trajectoryTime); } }
private void introduceRandomPose() { for (int i = 0; i < oneDoFJoints.length; i++) { double newQ = generateRandomDoubleInRange(oneDoFJoints[i].getJointLimitUpper(), oneDoFJoints[i].getJointLimitLower()); oneDoFJoints[i].setQ(newQ); } }
public RobotJointLimitWatcher(OneDoFJoint[] oneDoFJoints, SensorRawOutputMapReadOnly sensorRawOutputMapReadOnly) { this.oneDoFJoints = oneDoFJoints; this.sensorRawOutputMapReadOnly = sensorRawOutputMapReadOnly; int numberOfJoints = oneDoFJoints.length; variablesToTrack = new DoubleYoVariable[numberOfJoints]; limitCheckers = new YoVariableLimitChecker[numberOfJoints]; for (int i = 0; i < numberOfJoints; i++) { OneDoFJoint oneDoFJoint = oneDoFJoints[i]; variablesToTrack[i] = new DoubleYoVariable(oneDoFJoint.getName(), doNotRegister); double thresholdPercentage = 0.02; double range = oneDoFJoint.getJointLimitUpper() - oneDoFJoint.getJointLimitLower(); double thresholdAmount = range * thresholdPercentage; double lowerLimit = oneDoFJoint.getJointLimitLower() + thresholdAmount; double upperLimit = oneDoFJoint.getJointLimitUpper() - thresholdAmount; limitCheckers[i] = new YoVariableLimitChecker(variablesToTrack[i], "limit", lowerLimit, upperLimit, registry); } }
private void setupSliderForChest() { int sliderChannel = 1; for (SpineJointName spineJointName : spineJointNames) { OneDegreeOfFreedomJoint spineSCSJoint = spineSCSJoints.get(spineJointName); OneDoFJoint spineIDJoint = fullRobotModel.getSpineJoint(spineJointName); sliderBoard.setSlider(sliderChannel++, spineSCSJoint.getQYoVariable(), spineIDJoint.getJointLimitLower(), spineIDJoint.getJointLimitUpper()); } for (NeckJointName neckJointName : neckJointNames) { OneDegreeOfFreedomJoint neckSCSJoint = neckSCSJoints.get(neckJointName); OneDoFJoint neckIDJoint = fullRobotModel.getNeckJoint(neckJointName); sliderBoard.setSlider(sliderChannel++, neckSCSJoint.getQYoVariable(), neckIDJoint.getJointLimitLower(), neckIDJoint.getJointLimitUpper()); } }
private void setupSliderForChest() { int sliderChannel = 1; for (SpineJointName spineJointName : spineJointNames) { OneDegreeOfFreedomJoint spineSCSJoint = spineSCSJoints.get(spineJointName); OneDoFJoint spineIDJoint = fullRobotModel.getSpineJoint(spineJointName); sliderBoard.setSlider(sliderChannel++, spineSCSJoint.getQYoVariable(), spineIDJoint.getJointLimitLower(), spineIDJoint.getJointLimitUpper()); } for (NeckJointName neckJointName : neckJointNames) { OneDegreeOfFreedomJoint neckSCSJoint = neckSCSJoints.get(neckJointName); OneDoFJoint neckIDJoint = fullRobotModel.getNeckJoint(neckJointName); sliderBoard.setSlider(sliderChannel++, neckSCSJoint.getQYoVariable(), neckIDJoint.getJointLimitLower(), neckIDJoint.getJointLimitUpper()); } }
private static OneDoFJoint cloneOneDoFJoint(OneDoFJoint original, String cloneSuffix, RigidBody clonePredecessor) { String jointNameOriginal = original.getName(); RigidBodyTransform jointTransform = original.getOffsetTransform3D(); Vector3d jointAxisCopy = original.getJointAxis().getVectorCopy(); OneDoFJoint clone; if (original instanceof RevoluteJoint) clone = ScrewTools.addRevoluteJoint(jointNameOriginal + cloneSuffix, clonePredecessor, jointTransform, jointAxisCopy); else if (original instanceof PrismaticJoint) clone = ScrewTools.addPrismaticJoint(jointNameOriginal + cloneSuffix, clonePredecessor, jointTransform, jointAxisCopy); else throw new RuntimeException("Unhandled type of " + OneDoFJoint.class.getSimpleName() + ": " + original.getClass().getSimpleName()); clone.setJointLimitLower(original.getJointLimitLower()); clone.setJointLimitUpper(original.getJointLimitUpper()); return clone; }
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 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 boolean checkAnkleLimitForToeOff(RobotSide trailingLeg) { OneDoFJoint anklePitch = fullRobotModel.getLegJoint(trailingLeg, LegJointName.ANKLE_PITCH); double lowerLimit = Math.max(anklePitch.getJointLimitLower() + 0.02, ankleLowerLimitToTriggerToeOff.getDoubleValue()); isRearAnklePitchHittingLimit.set(anklePitch.getQ() < lowerLimit); isRearAnklePitchHittingLimitFilt.update(); if (!doToeOffWhenHittingAnkleLimit.getBooleanValue()) return false; if (!isDesiredICPOKForToeOff.getBooleanValue() || !isCurrentICPOKForToeOff.getBooleanValue()) return false; return isRearAnklePitchHittingLimitFilt.getBooleanValue(); }
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(); } }
public static boolean checkJointspaceTrajectoryPointList(OneDoFJoint joint, SimpleTrajectoryPoint1DList trajectoryPointList) { for (int i = 0; i < trajectoryPointList.getNumberOfTrajectoryPoints(); i++) { double waypointPosition = trajectoryPointList.getTrajectoryPoint(i).getPosition(); double jointLimitLower = joint.getJointLimitLower(); double jointLimitUpper = joint.getJointLimitUpper(); if (!MathTools.isInsideBoundsInclusive(waypointPosition, jointLimitLower, jointLimitUpper)) { PrintTools.warn("Joint out of bounds: "+joint.getName()+" (" +jointLimitLower+", "+jointLimitUpper+ ") = "+waypointPosition+" (t="+i+")"); return false; } } return true; } }
public void submitJointLimitReductionCommand(JointLimitReductionCommand command) { for (int commandJointIndex = 0; commandJointIndex < command.getNumberOfJoints(); commandJointIndex++) { OneDoFJoint joint = command.getJoint(commandJointIndex); int jointIndex = jointIndexHandler.getOneDoFJointIndex(joint); double originalJointLimitLower = joint.getJointLimitLower(); double originalJointLimitUpper = joint.getJointLimitUpper(); double limitReduction = command.getJointLimitReductionFactor(commandJointIndex) * jointsRangeOfMotion.get(jointIndex, 0); jointLowerLimits.set(jointIndex, 0, originalJointLimitLower + limitReduction); jointUpperLimits.set(jointIndex, 0, originalJointLimitUpper - limitReduction); } }
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 computeArmTrajectoryMessage(RobotSide robotSide) { RigidBody hand = fullRobotModelToUseForConversion.getHand(robotSide); RigidBody chest = fullRobotModelToUseForConversion.getChest(); OneDoFJoint[] armJoints = ScrewTools.createOneDoFJointPath(chest, hand); int numberOfArmJoints = armJoints.length; double[] desiredJointPositions = new double[numberOfArmJoints]; for (int i = 0; i < numberOfArmJoints; i++) { OneDoFJoint armJoint = armJoints[i]; desiredJointPositions[i] = MathTools.clipToMinMax(armJoint.getQ(), armJoint.getJointLimitLower(), armJoint.getJointLimitUpper()); } ArmTrajectoryMessage armTrajectoryMessage = new ArmTrajectoryMessage(robotSide, trajectoryTime, desiredJointPositions); output.setArmTrajectoryMessage(armTrajectoryMessage); }
double jointLimitLower = joint.getJointLimitLower(); double jointLimitUpper = joint.getJointLimitUpper();
desiredPosition = MathTools.clipToMinMax(desiredPosition, joint.getJointLimitLower(), joint.getJointLimitUpper()); desiredPosition = alphaPosition * desiredPosition + (1.0 - alphaPosition) * joint.getQ(); desiredVelocity = (desiredPosition - lowLevelJointData.getDesiredPosition()) / controlDT;