public void setDesiredJointState(List<OneDoFJoint> newJointData) { if (newJointData.size() != desiredJointAngles.length) throw new RuntimeException("Array size does not match"); for (int i = 0; i < desiredJointAngles.length; i++) desiredJointAngles[i] = (float) newJointData.get(i).getqDesired(); }
public void setDesiredPositionFromJoints(OneDoFJoint[] joints) { for (int i = 0; i < joints.length; i++) { OneDoFJoint joint = joints[i]; setDesiredJointPosition(joint, joint.getqDesired()); } }
public void setDesiredJointState(FloatingInverseDynamicsJoint rootJoint, OneDoFJoint[] newJointData) { if (newJointData.length != desiredJointAngles.length) throw new RuntimeException("Array size does not match"); for (int i = 0; i < desiredJointAngles.length; i++) desiredJointAngles[i] = (float) newJointData[i].getqDesired(); rootJoint.getTranslation(desiredRootTranslation); rootJoint.getRotation(desiredRootOrientation); }
public static void getJointDesiredPositions(OneDoFJoint[] joints, DenseMatrix64F jointPositionsToPack) { int rowStart = 0; for (OneDoFJoint joint : joints) { jointPositionsToPack.set(rowStart, 0, joint.getqDesired()); rowStart += joint.getDegreesOfFreedom(); } }
jointCommand.putDouble(joint.getqDesired()); else jointCommand.putDouble(joint.getTau());
private void setLocalJointAnglesToDesiredJointAngles() { for (int i = 0; i < numberOfDoF; i++) { localJoints[i].setQ(originalJoints[i].getqDesired()); localJoints[i].getFrameAfterJoint().update(); } }
public void updateSimulationBasedOnFullRobotModel(FloatingRootJointRobot sdfRobot) { for (OneDoFJoint joint : oneDoFJoints) { OneDegreeOfFreedomJoint simulatedJoint = (OneDegreeOfFreedomJoint) sdfRobot.getJoint(joint.getName()); simulatedJoint.setQ(joint.getqDesired()); } } }
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 < 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); } }
@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 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()); } }
lowLevelJointDesiredData.setDesiredJointPosition(joint, joint.getqDesired()); lowLevelJointDesiredData.setDesiredJointVelocity(joint, joint.getQdDesired()); double qDesired = joint.getqDesired(); double qdDesired = joint.getQdDesired();