public DesiredJointDataHolder(OneDoFJoint[] joints) { this.joints = joints; for (int i = 0; i < joints.length; i++) { DesiredJointData value = new DesiredJointData(); desiredJointDataList.add(new ImmutablePair<OneDoFJoint, DesiredJointDataHolder.DesiredJointData>(joints[i], value)); desiredJointData.put(joints[i], value); } }
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()); } }
public IntermediateDesiredJointDataHolder(DesiredJointDataHolder estimatorDataHolder, DesiredJointDataHolder controllerDataHolder) { this.estimatorDataHolder = estimatorDataHolder; this.controllerDataHolder = controllerDataHolder; for(int i = 0; i < estimatorDataHolder.getJoints().length; i++) { if(estimatorDataHolder.getJoints()[i].getName() != controllerDataHolder.getJoints()[i].getName()) { throw new RuntimeException("Models do not match"); } } intermediateDesiredJointData = new DesiredJointData[estimatorDataHolder.getJoints().length]; for(int i = 0; i < estimatorDataHolder.getJoints().length; i++) { intermediateDesiredJointData[i] = new DesiredJointData(); } }
public void readIntoEstimator() { for(int i = 0; i < estimatorDataHolder.getJoints().length; i++) { estimatorDataHolder.get(i).set(intermediateDesiredJointData[i]); } } }
public void set(DesiredJointData desiredJointData) { setTauDesired(desiredJointData.getTauDesired()); setQddDesired(desiredJointData.getQddDesired()); setPositionDesired(desiredJointData.getPositionDesired()); }
public void copyFromController() { controllerDataHolder.updateFromModel(); for(int i = 0; i < controllerDataHolder.getJoints().length; i++) { intermediateDesiredJointData[i].set(controllerDataHolder.get(i)); } }