public void setDesiredVelocityFromJoints(OneDoFJoint[] joints) { for (int i = 0; i < joints.length; i++) { OneDoFJoint joint = joints[i]; setDesiredJointVelocity(joint, joint.getQdDesired()); } }
@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 < revoluteJoints.size(); i++) { ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair = revoluteJoints.get(i); OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJoint revoluteJoint = jointPair.getRight(); double tau = revoluteJoint.getTau(); DoubleYoVariable rawJointTorque = rawJointTorques.get(revoluteJoint); DelayedDoubleYoVariable delayedJointTorque = delayedJointTorques.get(revoluteJoint); if (rawJointTorque != null) { rawJointTorque.set(tau); delayedJointTorque.update(); tau = delayedJointTorque.getDoubleValue(); } pinJoint.setTau(tau); pinJoint.setKp(revoluteJoint.getKp()); pinJoint.setKd(revoluteJoint.getKd()); pinJoint.setqDesired(revoluteJoint.getqDesired()); pinJoint.setQdDesired(revoluteJoint.getQdDesired()); } for (int i = 0; i < rawOutputWriters.size(); i++) { rawOutputWriters.get(i).write(); } }
public void doControl() { if (highLevelControllerOutputJoint.isUnderPositionControl()) { double currentPosition = simulatedJoint.getQYoVariable().getDoubleValue(); double desiredPosition = highLevelControllerOutputJoint.getqDesired(); double currentRate = simulatedJoint.getQDYoVariable().getDoubleValue(); double desiredRate = highLevelControllerOutputJoint.getQdDesired(); double desiredTau = jointController.compute(currentPosition, desiredPosition, currentRate, desiredRate, controlDT); simulatedJoint.setTau(desiredTau); } }
public void setDesiredsFromOneDoFJoint(OneDoFJoint jointToExtractDesiredsFrom) { setDesiredTorque(jointToExtractDesiredsFrom.getTau()); setDesiredPosition(jointToExtractDesiredsFrom.getqDesired()); setDesiredVelocity(jointToExtractDesiredsFrom.getQdDesired()); setDesiredAcceleration(jointToExtractDesiredsFrom.getQddDesired()); setResetIntegrators(jointToExtractDesiredsFrom.getResetIntegrator()); }
@Override public void doControl() { if (highLevelControllerOutputJoint.isUnderPositionControl()) { double currentPosition = simulatedJoint.getQYoVariable().getDoubleValue(); double desiredPosition = highLevelControllerOutputJoint.getqDesired(); double currentRate = simulatedJoint.getQDYoVariable().getDoubleValue(); double desiredRate = highLevelControllerOutputJoint.getQdDesired(); double desiredTau = jointController.compute(currentPosition, desiredPosition, currentRate, desiredRate, controlDT); simulatedJoint.setTau(desiredTau); } }
public void setDesiredsFromOneDoFJoint(OneDoFJoint jointToExtractDesiredsFrom) { setDesiredTorque(jointToExtractDesiredsFrom.getTau()); setDesiredPosition(jointToExtractDesiredsFrom.getqDesired()); setDesiredVelocity(jointToExtractDesiredsFrom.getQdDesired()); setDesiredAcceleration(jointToExtractDesiredsFrom.getQddDesired()); setResetIntegrators(jointToExtractDesiredsFrom.getResetIntegrator()); }
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()); } }
jointspaceAccelerationCommand.setOneDoFJointDesiredAcceleration(i, feedForwardAcceleration); lowLevelJointDesiredData.setDesiredJointPosition(joint, joint.getqDesired()); lowLevelJointDesiredData.setDesiredJointVelocity(joint, joint.getQdDesired()); double qdDesired = joint.getQdDesired();