@Override public void computeAchievedAcceleration() { qDDAchieved.set(joint.getQddDesired()); }
public void setDesiredAccelerationFromJoints(OneDoFJoint[] joints) { for (int i = 0; i < joints.length; i++) { OneDoFJoint joint = joints[i]; setDesiredJointAcceleration(joint, joint.getQddDesired()); } }
public static void copyDesiredAccelerationsToActual(Iterable<? extends OneDoFJoint> joints) { for (OneDoFJoint joint : joints) { joint.setQdd(joint.getQddDesired()); } }
@Override public void setQddDesired(InverseDynamicsJoint originalJoint) { OneDoFJoint oneDoFOriginalJoint = checkAndGetAsOneDoFJoint(originalJoint); setQddDesired(oneDoFOriginalJoint.getQddDesired()); }
public void updateFromModel() { for (int i = 0; i < desiredJointDataList.size(); i++) { ImmutablePair<OneDoFJoint, DesiredJointData> desiredJointData = desiredJointDataList.get(i); OneDoFJoint joint = desiredJointData.getLeft(); DesiredJointData data = desiredJointData.getRight(); data.setQddDesired(joint.getQddDesired()); data.setTauDesired(joint.getTau()); data.setPositionDesired(joint.getqDesired()); } }
@Override public void write() { for (int i = 0; i < revoluteJoints.size(); i++) { ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair = revoluteJoints.get(i); OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJoint revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getqDesired()); pinJoint.setQd(revoluteJoint.getQdDesired()); pinJoint.setQdd(revoluteJoint.getQddDesired()); } } }
@Override public void writeAfterController(long timestamp) { for (int i = 0; i < oneDoFJoints.size(); i++) { OneDoFJoint oneDoFJoint = oneDoFJoints.get(i); double desiredAcceleration = oneDoFJoint.getQddDesired(); DoubleYoVariable torqueOffsetVariable = torqueOffsetMap.get(oneDoFJoint); if (resetTorqueOffsets.getBooleanValue()) torqueOffsetVariable.set(0.0); double offsetTorque = torqueOffsetVariable.getDoubleValue(); double ditherTorque = 0.0; double alpha = alphaTorqueOffset.getDoubleValue(); offsetTorque = alpha * (offsetTorque + desiredAcceleration * updateDT) + (1.0 - alpha) * offsetTorque; torqueOffsetVariable.set(offsetTorque); oneDoFJoint.setTau(oneDoFJoint.getTau() + offsetTorque + ditherTorque); } drcOutputWriter.writeAfterController(timestamp); }
public void startComputation() { for (OneDoFJoint joint : oneDoFJoints) { if (joint == null) throw new RuntimeException(); ControlFlowInputPort<double[]> positionSensorPort = positionSensorInputPorts.get(joint); ControlFlowInputPort<double[]> velocitySensorPort = velocitySensorInputPorts.get(joint); double positionSensorData = positionSensorPort.getData()[0]; double velocitySensorData = velocitySensorPort.getData()[0]; joint.setQ(positionSensorData); joint.setQd(velocitySensorData); joint.setQdd(joint.getQddDesired()); } // TODO: Does it make sense to do this yet if the orientation of the pelvis isn't known yet? FullInverseDynamicsStructure inverseDynamicsStructure = inverseDynamicsStructureOutputPort.getData(); TwistCalculator twistCalculator = inverseDynamicsStructure.getTwistCalculator(); SpatialAccelerationCalculator spatialAccelerationCalculator = inverseDynamicsStructure.getSpatialAccelerationCalculator(); twistCalculator.getRootBody().updateFramesRecursively(); twistCalculator.compute(); spatialAccelerationCalculator.compute(); inverseDynamicsStructureOutputPort.setData(inverseDynamicsStructure); }
public void setDesiredsFromOneDoFJoint(OneDoFJoint jointToExtractDesiredsFrom) { setDesiredTorque(jointToExtractDesiredsFrom.getTau()); setDesiredPosition(jointToExtractDesiredsFrom.getqDesired()); setDesiredVelocity(jointToExtractDesiredsFrom.getQdDesired()); setDesiredAcceleration(jointToExtractDesiredsFrom.getQddDesired()); setResetIntegrators(jointToExtractDesiredsFrom.getResetIntegrator()); }
public void setDesiredsFromOneDoFJoint(OneDoFJoint jointToExtractDesiredsFrom) { setDesiredTorque(jointToExtractDesiredsFrom.getTau()); setDesiredPosition(jointToExtractDesiredsFrom.getqDesired()); setDesiredVelocity(jointToExtractDesiredsFrom.getQdDesired()); setDesiredAcceleration(jointToExtractDesiredsFrom.getQddDesired()); setResetIntegrators(jointToExtractDesiredsFrom.getResetIntegrator()); }
private void integrateAccelerationsToGetDesiredVelocities(OneDoFJoint oneDoFJoint, DoubleYoVariable qd_d_joint, DoubleYoVariable q_d_joint) { double currentPosition = oneDoFJoint.getQ(); double currentVelocity = oneDoFJoint.getQd(); if (oneDoFJoint.getResetDesiredAccelerationIntegrator()) { // qd_d_joint.set(currentVelocity); qd_d_joint.set(0.0); q_d_joint.set(currentPosition); return; } double previousDesiredVelocity = qd_d_joint.getDoubleValue(); double previousDesiredPosition = q_d_joint.getDoubleValue(); double desiredAcceleration = oneDoFJoint.getQddDesired(); double alphaVel = alphaDesiredVelocityMap.get(oneDoFJoint).getDoubleValue(); double alphaPos = alphaDesiredPositionMap.get(oneDoFJoint).getDoubleValue(); double desiredVelocity = doCleverIntegration(previousDesiredVelocity, desiredAcceleration, currentVelocity, alphaVel); double desiredPosition = doCleverIntegration(previousDesiredPosition, desiredVelocity, currentPosition, alphaPos); qd_d_joint.set(desiredVelocity); q_d_joint.set(desiredPosition); }
public void extractAllDataFromJoints(OneDoFJoint[] joints, LowLevelJointControlMode controlMode) { for (int i = 0; i < joints.length; i++) { OneDoFJoint joint = joints[i]; setJointControlMode(joint, controlMode); setDesiredJointTorque(joint, joint.getTau()); setDesiredJointPosition(joint, joint.getqDesired()); setDesiredJointVelocity(joint, joint.getQdDesired()); setDesiredJointAcceleration(joint, joint.getQddDesired()); setResetJointIntegrators(joint, joint.getResetDesiredAccelerationIntegrator()); } }
jointAccelerationsSolution.get(joint).set(joint.getQddDesired());