public static void setRandomConfiguration(Random random, Robot robot) { FloatingJoint rootJoint = (FloatingJoint) robot.getRootJoints().get(0); rootJoint.setRotationAndTranslation(EuclidCoreRandomTools.nextRigidBodyTransform(random)); ArrayList<OneDegreeOfFreedomJoint> joints = new ArrayList<>(); robot.getAllOneDegreeOfFreedomJoints(joints); for (OneDegreeOfFreedomJoint joint : joints) { double jointLowerLimit = Math.max(joint.getJointLowerLimit(), -2.0 * Math.PI); double jointUpperLimit = Math.min(joint.getJointUpperLimit(), 2.0 * Math.PI); double q = EuclidCoreRandomTools.nextDouble(random, jointLowerLimit, jointUpperLimit); joint.setQ(q); } }
private final Limit createJointLimit(OneDegreeOfFreedomJoint scsJoint) { Limit sdfJointLimit = new Limit(); String effort = String.valueOf(scsJoint.getTorqueLimit()); sdfJointLimit.setEffort(effort); String velocity = String.valueOf(scsJoint.getVelocityLimit()); sdfJointLimit.setVelocity(velocity); String lower = String.valueOf(scsJoint.getJointLowerLimit()); sdfJointLimit.setLower(lower); String upper = String.valueOf(scsJoint.getJointUpperLimit()); sdfJointLimit.setUpper(upper); double jointRange = scsJoint.getJointUpperLimit() - scsJoint.getJointLowerLimit(); if (jointRange == 0.0) PrintTools.debug(this, scsJoint.getName() + " upper joint limit equals lower joint limit!"); return sdfJointLimit; }
private final Limit createJointLimit(OneDegreeOfFreedomJoint scsJoint) { Limit sdfJointLimit = new Limit(); String effort = String.valueOf(scsJoint.getTorqueLimit()); sdfJointLimit.setEffort(effort); String velocity = String.valueOf(scsJoint.getVelocityLimit()); sdfJointLimit.setVelocity(velocity); String lower = String.valueOf(scsJoint.getJointLowerLimit()); sdfJointLimit.setLower(lower); String upper = String.valueOf(scsJoint.getJointUpperLimit()); sdfJointLimit.setUpper(upper); double jointRange = scsJoint.getJointUpperLimit() - scsJoint.getJointLowerLimit(); if (jointRange == 0.0) PrintTools.debug(this, scsJoint.getName() + " upper joint limit equals lower joint limit!"); return sdfJointLimit; }
public String getMaxValueExceededError() { if (hasMaxValueExeededAnyJoint()) { OneDegreeOfFreedomJoint failedJoint = getJointWhichExceededMaxValue(); return failedJoint.getName() + " reached " + getMaxValueOfJoint(failedJoint) + " radians, at t = " + getSimTimeMaxValueOfJoint(failedJoint) + ", which is outside the allowable range of [" + failedJoint.getJointLowerLimit() + ", " + failedJoint.getJointUpperLimit() + "]"; } return ""; }
public String getMinValueExceededError() { if (hasMinValueExeededAnyJoint()) { OneDegreeOfFreedomJoint failedJoint = getJointWhichExceededMinValue(); return failedJoint.getName() + " reached " + getMinValueOfJoint(failedJoint) + " radians, at t = " + getSimTimeMinValueOfJoint(failedJoint) + ", which is outside the allowable range of [" + failedJoint.getJointLowerLimit() + ", " + failedJoint.getJointUpperLimit() + "]"; } return ""; }
double lowerLimit = joint.getJointLowerLimit(); double range = upperLimit - lowerLimit;
public void setRobotStateRandomly(Random random, double maxJointVelocity, double maxRootJointLinearAndAngularVelocity) { FloatingJoint rootJoint = robot.getRootJoint(); rootJoint.setVelocity(RandomGeometry.nextVector3D(random, maxRootJointLinearAndAngularVelocity)); rootJoint.setAngularVelocityInBody(RandomGeometry.nextVector3D(random, maxRootJointLinearAndAngularVelocity)); rootJoint.setPosition(RandomGeometry.nextVector3D(random)); double yaw = RandomNumbers.nextDouble(random, Math.PI / 20.0); double pitch = RandomNumbers.nextDouble(random, Math.PI / 20.0); double roll = RandomNumbers.nextDouble(random, Math.PI / 20.0); rootJoint.setYawPitchRoll(yaw, pitch, roll); ArrayList<OneDegreeOfFreedomJoint> oneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); robot.getAllOneDegreeOfFreedomJoints(oneDegreeOfFreedomJoints); for (OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint : oneDegreeOfFreedomJoints) { double lowerLimit = oneDegreeOfFreedomJoint.getJointLowerLimit(); double upperLimit = oneDegreeOfFreedomJoint.getJointUpperLimit(); double delta = upperLimit - lowerLimit; lowerLimit = lowerLimit + 0.05 * delta; upperLimit = upperLimit - 0.05 * delta; oneDegreeOfFreedomJoint.setQ(RandomNumbers.nextDouble(random, lowerLimit, upperLimit)); oneDegreeOfFreedomJoint.setQd(RandomNumbers.nextDouble(random, maxJointVelocity)); } }
sliderBoardConfigurationManager.setSlider(1, variables.q_d, oneDoFJoint.getJointLowerLimit(), oneDoFJoint.getJointUpperLimit()); sliderBoardConfigurationManager.setSlider(2, variables.qd_d, -1, 1); sliderBoardConfigurationManager.setSlider(3, variables.kp, 0, 10 * joint.getRatio() * joint.getRatio());
sliderBoardConfigurationManager.setSlider(1, variables.q_d, oneDoFJoint.getJointLowerLimit(), oneDoFJoint.getJointUpperLimit()); sliderBoardConfigurationManager.setSlider(2, variables.qd_d, -1, 1); sliderBoardConfigurationManager.setSlider(3, variables.kp, 0, 10 * joint.getRatio() * joint.getRatio());
OneDegreeOfFreedomJoint oneDoFJoint = ((FloatingRootJointRobot)robot).getOneDegreeOfFreedomJoint(aJoint.getSdfName()); sliderBoardConfigurationManager.setKnob(1, selectedJointPair, 0, StepprJoint.values.length); sliderBoardConfigurationManager.setSlider(1, variables.q_d, oneDoFJoint.getJointLowerLimit(), oneDoFJoint.getJointUpperLimit()); sliderBoardConfigurationManager.setSlider(2, variables.kp, 0, 100 * aJoint.getRatio() * aJoint.getRatio()); sliderBoardConfigurationManager.setSlider(3, variables.damping, 0, 5 * aJoint.getRatio() * aJoint.getRatio());
OneDegreeOfFreedomJoint oneDoFJoint = ((FloatingRootJointRobot)robot).getOneDegreeOfFreedomJoint(aJoint.getSdfName()); sliderBoardConfigurationManager.setKnob(1, selectedJointPair, 0, WandererJoint.values.length); sliderBoardConfigurationManager.setSlider(1, variables.q_d, oneDoFJoint.getJointLowerLimit(), oneDoFJoint.getJointUpperLimit()); sliderBoardConfigurationManager.setSlider(2, variables.kp, 0, 20 * aJoint.getRatio() * aJoint.getRatio()); sliderBoardConfigurationManager.setSlider(3, variables.damping, 0, 0.5 * aJoint.getRatio() * aJoint.getRatio());