public void setFingerJointsProvider(SideDependentList<List<OneDegreeOfFreedomJoint>> allFingerJoints) { fingerJointMap = new HashMap<String, OneDegreeOfFreedomJoint>(); for (RobotSide robotSide : RobotSide.values) { for (OneDegreeOfFreedomJoint joint : allFingerJoints.get(robotSide)) { fingerJointMap.put(joint.getName(), joint); } } }
public LowLevelActuatorSimulator(OneDegreeOfFreedomJoint simulatedJoint, LowLevelStateReadOnly jointDesiredOutput, double controlDT, double kp, double kd, LowLevelActuatorMode actuatorMode) { this.controlDT = controlDT; this.actuatorMode = actuatorMode; registry = new YoVariableRegistry(simulatedJoint.getName() + name); gains = new YoPIDGains(simulatedJoint.getName() + "Actuator", registry); jointController = new PIDController(gains, simulatedJoint.getName() + "LowLevelActuatorSimulator", registry); gains.setKp(kp); gains.setKd(kd); this.simulatedJoint = simulatedJoint; this.actuatorDesireds = jointDesiredOutput; }
public void setRobotTorquesToMatchOtherRobot(FloatingRootJointRobot otherRobot) { ArrayList<OneDegreeOfFreedomJoint> oneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); robot.getAllOneDegreeOfFreedomJoints(oneDegreeOfFreedomJoints); ArrayList<OneDegreeOfFreedomJoint> otherOneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); otherRobot.getAllOneDegreeOfFreedomJoints(otherOneDegreeOfFreedomJoints); for (int i = 0; i < oneDegreeOfFreedomJoints.size(); i++) { OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint = oneDegreeOfFreedomJoints.get(i); OneDegreeOfFreedomJoint otherOneDegreeOfFreedomJoint = otherOneDegreeOfFreedomJoints.get(i); if (!oneDegreeOfFreedomJoint.getName().equals(otherOneDegreeOfFreedomJoint.getName())) { throw new RuntimeException(); } oneDegreeOfFreedomJoint.setTau(otherOneDegreeOfFreedomJoint.getTauYoVariable().getDoubleValue()); } }
public PassiveJointController(OneDegreeOfFreedomJoint simulatedJoint, YoPDGains gains) { registry = new YoVariableRegistry(simulatedJoint.getName() + name); jointController = new PDController(gains.getYoKp(), gains.getYoKd(), simulatedJoint.getName() + "PassiveController", registry); this.simulatedJoint = simulatedJoint; jointController.setProportionalGain(36000.0); jointController.setDerivativeGain(1000.0); }
public PassiveJointController(OneDegreeOfFreedomJoint simulatedJoint, YoPDGains gains) { registry = new YoVariableRegistry(simulatedJoint.getName() + name); jointController = new PDController(gains.getYoKp(), gains.getYoKd(), simulatedJoint.getName() + "PassiveController", registry); this.simulatedJoint = simulatedJoint; jointController.setProportionalGain(36000.0); jointController.setDerivativeGain(1000.0); }
public PassiveJointController(OneDegreeOfFreedomJoint simulatedJoint, YoPDGains gains) { registry = new YoVariableRegistry(simulatedJoint.getName() + name); jointController = new PDController(gains.getYoKp(), gains.getYoKd(), simulatedJoint.getName() + "PassiveController", registry); this.simulatedJoint = simulatedJoint; jointController.setProportionalGain(36000.0); jointController.setDerivativeGain(1000.0); }
public String getDerivativeCompError() { if (hasDerivativeComparisonErrorOccurredAnyJoint()) { OneDegreeOfFreedomJoint failedJoint = getJointWhichHasDerivativeComparisonError(); return failedJoint.getName() + " experienced a derivative computation error / discontinuity at t = " + getSimTimeDeriveCompErrorOfJoint(failedJoint) + " seconds."; } return ""; }
public String getMaxDerivativeExceededError() { if (hasMaxDerivativeExeededAnyJoint()) { OneDegreeOfFreedomJoint failedJoint = getJointWhichExceededMaxDerivative(); return failedJoint.getName() + " reached " + getMaxDerivativeOfJoint(failedJoint) + " radians/sec, at t = " + getSimTimeMaxDerivativeOfJoint(failedJoint) + ", which exceeds the maximum joint velocity."; } return ""; }
public String getMaxSecondDerivativeExceededError() { if (hasMaxSecondDerivativeExeededAnyJoint()) { OneDegreeOfFreedomJoint failedJoint = getJointWhichExceededMaxSecondDerivative(); return failedJoint.getName() + " reached " + getMaxSecondDerivativeOfJoint(failedJoint) + " radians/sec/sec, at t = " + getSimTimeMaxSecondDerivativeOfJoint(failedJoint) + ", which exceeds the maximum joint acceleration."; } return ""; }
@ContinuousIntegrationTest(estimatedDuration = 21.5) @Test(timeout = 64580) public void testDoNothingBahviourState() throws SimulationExceededMaximumTimeException { testState(HighLevelControllerName.DO_NOTHING_BEHAVIOR); OneDegreeOfFreedomJoint[] oneDofJoints = drcBehaviorTestHelper.getRobot().getOneDegreeOfFreedomJoints(); for (OneDegreeOfFreedomJoint joint : oneDofJoints) { String jointName = joint.getName(); double tau = joint.getTauYoVariable().getDoubleValue(); if (!jointName.contains("hokuyo")) { assertTrue(joint.getName() + " tau : " + tau, tau == 0.0); } } }
public void setFullRobotModelAccelerationRandomly(Random random, double maxPelvisLinearAcceleration, double maxPelvisAngularAcceleration, double maxJointAcceleration) { FloatingJointBasics sixDoFJoint = fullRobotModel.getRootJoint(); setSixDoFJointAccelerationRandomly(sixDoFJoint, random, maxPelvisLinearAcceleration, maxPelvisAngularAcceleration); ArrayList<OneDegreeOfFreedomJoint> oneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); robot.getAllOneDegreeOfFreedomJoints(oneDegreeOfFreedomJoints); for (OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint : oneDegreeOfFreedomJoints) { OneDoFJointBasics oneDoFJoint = fullRobotModel.getOneDoFJointByName(oneDegreeOfFreedomJoint.getName()); oneDoFJoint.setQdd(RandomNumbers.nextDouble(random, maxJointAcceleration)); } }
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 ""; }
public void setFullRobotModelAccelerationToMatchRobot() { robot.update(); FloatingJointBasics sixDoFJoint = fullRobotModel.getRootJoint(); FloatingJoint floatingJoint = robot.getRootJoint(); fullRobotModel.updateFrames(); copyAccelerationFromForwardToInverse(floatingJoint, sixDoFJoint); ArrayList<OneDegreeOfFreedomJoint> oneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); robot.getAllOneDegreeOfFreedomJoints(oneDegreeOfFreedomJoints); for (OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint : oneDegreeOfFreedomJoints) { OneDoFJointBasics oneDoFJoint = fullRobotModel.getOneDoFJointByName(oneDegreeOfFreedomJoint.getName()); double robotJointAcceleration = oneDegreeOfFreedomJoint.getQDDYoVariable().getDoubleValue(); oneDoFJoint.setQdd(robotJointAcceleration); YoDouble computedJointAcceleration = computedJointAccelerations.get(oneDoFJoint); computedJointAcceleration.set(robotJointAcceleration); } }
public void setRobotStateToMatchFullRobotModel() { FloatingJointBasics sixDoFJoint = fullRobotModel.getRootJoint(); FloatingJoint floatingJoint = robot.getRootJoint(); fullRobotModel.updateFrames(); setRobotRootJointPositionAndOrientationToMatchFullRobotModel(sixDoFJoint, floatingJoint); setRobotRootJointVelocityAndAngularVelocityToMatchFullRobotModel(sixDoFJoint, floatingJoint); ArrayList<OneDegreeOfFreedomJoint> oneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); robot.getAllOneDegreeOfFreedomJoints(oneDegreeOfFreedomJoints); for (OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint : oneDegreeOfFreedomJoints) { OneDoFJointBasics oneDoFJoint = fullRobotModel.getOneDoFJointByName(oneDegreeOfFreedomJoint.getName()); oneDegreeOfFreedomJoint.setQ(oneDoFJoint.getQ()); oneDegreeOfFreedomJoint.setQd(oneDoFJoint.getQd()); } robot.update(); }
public void setFullRobotModelStateToMatchRobot() { robot.update(); FloatingJointBasics sixDoFJoint = fullRobotModel.getRootJoint(); FloatingJoint floatingJoint = robot.getRootJoint(); setFullRobotModelRootJointPositionAndOrientationToMatchRobot(sixDoFJoint, floatingJoint); fullRobotModel.updateFrames(); setFullRobotModelRootJointVelocityAndAngularVelocityToMatchRobot(sixDoFJoint, floatingJoint); ArrayList<OneDegreeOfFreedomJoint> oneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); robot.getAllOneDegreeOfFreedomJoints(oneDegreeOfFreedomJoints); for (OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint : oneDegreeOfFreedomJoints) { OneDoFJointBasics oneDoFJoint = fullRobotModel.getOneDoFJointByName(oneDegreeOfFreedomJoint.getName()); oneDoFJoint.setQ(oneDegreeOfFreedomJoint.getQYoVariable().getDoubleValue()); oneDoFJoint.setQd(oneDegreeOfFreedomJoint.getQDYoVariable().getDoubleValue()); } }
private SDFJoint createSDFJoint(OneDegreeOfFreedomJoint scsJoint) { SDFJoint sdfJoint = new SDFJoint(); sdfJoint.setAxis(createSDFJointAxis(scsJoint)); sdfJoint.setChild(scsJoint.getLink().getName()); sdfJoint.setName(scsJoint.getName()); sdfJoint.setParent(scsJoint.getParentJoint().getLink().getName()); RigidBodyTransform scsJointOffset = new RigidBodyTransform(); scsJoint.getTransformToWorld(scsJointOffset); sdfJoint.setPose(getPoseFromTransform3D(scsJointOffset)); String type; if (scsJoint instanceof PinJoint) { type = "revolute"; } else { throw new RuntimeException("Implement me!"); } sdfJoint.setType(type); return sdfJoint; }
private SDFJoint createSDFJoint(OneDegreeOfFreedomJoint scsJoint) { SDFJoint sdfJoint = new SDFJoint(); sdfJoint.setAxis(createSDFJointAxis(scsJoint)); sdfJoint.setChild(scsJoint.getLink().getName()); sdfJoint.setName(scsJoint.getName()); sdfJoint.setParent(scsJoint.getParentJoint().getLink().getName()); RigidBodyTransform scsJointOffset = new RigidBodyTransform(); scsJoint.getTransformToWorld(scsJointOffset); sdfJoint.setPose(getPoseFromTransform3D(scsJointOffset)); String type; if (scsJoint instanceof PinJoint) { type = "revolute"; } else { throw new RuntimeException("Implement me!"); } sdfJoint.setType(type); return sdfJoint; }