@Override public LowLevelJointDataReadOnly getLowLevelJointData(OneDoFJoint joint) { return lowLevelJointDataMap.get(joint.getName()); }
public void addLimitEnforcementMethod(OneDoFJoint joint, JointLimitEnforcement method, JointLimitParameters limitParameters) { jointNames.add(joint.getName()); joints.add(joint); methods.add(method); parameters.add(limitParameters); }
@Override public boolean hasDesiredVelocityForJoint(OneDoFJoint joint) { YoLowLevelJointData lowLevelJointData = lowLevelJointDataMap.get(joint.getName()); if (lowLevelJointData == null) return false; else return lowLevelJointData.hasDesiredVelocity(); }
public void clear() { for (int i = 0; i < jointsWithDesiredData.size(); i++) { OneDoFJoint joint = jointsWithDesiredData.get(i); YoLowLevelJointData jointDataToReset = lowLevelJointDataMap.get(joint.getName()); jointDataToReset.clear(); } }
@Override public boolean hasDesiredTorqueForJoint(OneDoFJoint joint) { YoLowLevelJointData lowLevelJointData = lowLevelJointDataMap.get(joint.getName()); if (lowLevelJointData == null) return false; else return lowLevelJointData.hasDesiredTorque(); }
@Override public boolean hasDesiredAcceleration(OneDoFJoint joint) { YoLowLevelJointData lowLevelJointData = lowLevelJointDataMap.get(joint.getName()); if (lowLevelJointData == null) return false; else return lowLevelJointData.hasDesiredAcceleration(); }
public void addReductionFactor(OneDoFJoint joint, double reductionFactor) { MathTools.checkIfInRange(reductionFactor, 0.0, 1.0); jointNames.add(joint.getName()); joints.add(joint); jointReductionFactors.add(reductionFactor); }
public SlowLoopControllerCoreCommandHolder(FullHumanoidRobotModel fastLoopFullRobotModel) { setupRigidBodyMap(fastLoopFullRobotModel, fastLoopRigidBodyMap); for (OneDoFJoint joint : fastLoopFullRobotModel.getOneDoFJoints()) fastLoopJointMap.put(joint.getName(), joint); }
public void setDesiredJointPosition(OneDoFJoint joint, double desiredPosition) { YoLowLevelJointData lowLevelJointData = lowLevelJointDataMap.get(joint.getName()); if (lowLevelJointData == null) throwJointNotRegisteredException(joint); lowLevelJointData.setDesiredPosition(desiredPosition); }
@Override public LowLevelJointControlMode getJointControlMode(OneDoFJoint joint) { YoLowLevelJointData lowLevelJointData = lowLevelJointDataMap.get(joint.getName()); if (lowLevelJointData == null) throwJointNotRegisteredException(joint); return lowLevelJointData.getControlMode(); }
@Override public double getDesiredJointTorque(OneDoFJoint joint) { YoLowLevelJointData lowLevelJointData = lowLevelJointDataMap.get(joint.getName()); if (lowLevelJointData == null) throwJointNotRegisteredException(joint); return lowLevelJointData.getDesiredTorque(); }
public OneDoFJointForceTrackingDelayEstimator(OneDoFJoint joint, double dt, YoVariableRegistry parentRegistry) { this.joint = joint; String jointName = joint.getName(); registry = new YoVariableRegistry(jointName + "ForceTrackingDelayEstimator"); delayEstimator = new DelayEstimatorBetweenTwoSignals(jointName + "ForceTracking", dt, registry); }
public void setupForForceControl(OneDoFJoint joint, double desiredTorque) { YoLowLevelJointData lowLevelJointData = lowLevelJointDataMap.get(joint.getName()); if (lowLevelJointData == null) throwJointNotRegisteredException(joint); lowLevelJointData.setControlMode(LowLevelJointControlMode.FORCE_CONTROL); lowLevelJointData.setDesiredTorque(desiredTorque); }
public PlaybackPose(FullRobotModel fullRobotModel, OneDegreeOfFreedomJointHolder oneDegreeOfFreedomJointHolder) { playbackPoseMap = new LinkedHashMap<OneDoFJoint, Double>(); OneDoFJoint[] oneDoFJoints = fullRobotModel.getOneDoFJoints(); for (OneDoFJoint oneDoFJoint : oneDoFJoints) { String jointName = oneDoFJoint.getName(); OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint = oneDegreeOfFreedomJointHolder.getOneDegreeOfFreedomJoint(jointName); double jointAngle = oneDegreeOfFreedomJoint.getQYoVariable().getDoubleValue(); playbackPoseMap.put(oneDoFJoint, jointAngle); } }
public void addJoint(OneDoFJoint joint, double privilegedConfiguration) { enable(); joints.add(joint); jointNames.add(joint.getName()); privilegedOneDoFJointConfigurations.add(privilegedConfiguration); privilegedOneDoFJointConfigurationOptions.put(joint.nameBasedHashCode(), null); }
public void addJoint(OneDoFJoint joint, PrivilegedConfigurationOption privilegedConfiguration) { enable(); joints.add(joint); jointNames.add(joint.getName()); privilegedOneDoFJointConfigurations.add(Double.NaN); privilegedOneDoFJointConfigurationOptions.put(joint.nameBasedHashCode(), privilegedConfiguration); }
public void addJointToComputeDesiredPositionFor(OneDoFJoint joint) { jointNamesToComputeDesiredPositionFor.add(joint.getName()); jointsToComputeDesiredPositionFor.add(joint); jointAlphaPosition.add(Double.NaN); jointAlphaVelocity.add(Double.NaN); jointMaxPositionError.add(Double.NaN); jointMaxVelocity.add(Double.NaN); }
public void copyMeasuredTorqueToAppliedTorque() { for (OneDoFJoint oneDoFJoint : oneDoFJoints) { String measuredTorqueName = "raw_tau_" + oneDoFJoint.getName(); DoubleYoVariable measuredTorque = (DoubleYoVariable) simulationConstructionSet.getVariable(measuredTorqueName); controller.setAppliedTorque(oneDoFJoint, measuredTorque.getDoubleValue()); } }
public OneDoFJointSensorValidityChecker(OneDoFJoint jointToCheck, YoVariableRegistry parentRegistry) { hasYoVariables = false; this.joint = jointToCheck; String jointName = jointToCheck.getName(); registry = new YoVariableRegistry(jointName + "SensorValidityChecker"); parentRegistry.addChild(registry); positionChecker = new DoubleYoVariableValidityChecker(jointName + "Position", registry); velocityChecker = new DoubleYoVariableValidityChecker(jointName + "Velocity", registry); tauChecker = new DoubleYoVariableValidityChecker(jointName + "Tau", registry); }