estimatorCenterOfMassDataHolder = new CenterOfMassDataHolder(); estimatorRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(estimatorFullRobotModel); estimatorContactSensorHolder = new ContactSensorHolder(Arrays.asList(estimatorFullRobotModel.getContactSensorDefinitions()));
private long calculateEstimatorChecksum() { estimatorChecksumCalculator.reset(); estimatorChecksum.calculate(); estimatorForceSensorDataHolder.calculateChecksum(estimatorChecksumCalculator); estimatorCenterOfMassDataHolder.calculateChecksum(estimatorChecksumCalculator); return estimatorChecksumCalculator.getValue(); }
public void getIntoControllerModel() { intermediateToControllerCopier.copy(); rawDataIntermediateToControllerCopier.copy(); controllerForceSensorDataHolder.set(intermediateForceSensorDataHolder); controllerCenterOfMassDataHolder.set(intermediateCenterOfMassDataHolder); controllerContactSensorHolder.set(intermediateContactSensorHolder); }
if (estimatorCenterOfMassDataHolderToUpdate != null) estimatorCenterOfMassDataHolderToUpdate.setCenterOfMassVelocity(yoCenterOfMassVelocity.getFrameTuple());
estimatorCenterOfMassDataHolder = new CenterOfMassDataHolder(); estimatorRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(estimatorFullRobotModel); estimatorContactSensorHolder = new ContactSensorHolder(Arrays.asList(estimatorFullRobotModel.getContactSensorDefinitions()));
public void getIntoControllerModel() { intermediateToControllerCopier.copy(); rawDataIntermediateToControllerCopier.copy(); controllerForceSensorDataHolder.set(intermediateForceSensorDataHolder); controllerCenterOfMassDataHolder.set(intermediateCenterOfMassDataHolder); controllerContactSensorHolder.set(intermediateContactSensorHolder); }
private long calculateControllerChecksum() { controllerChecksumCalculator.reset(); controllerChecksum.calculate(); controllerForceSensorDataHolder.calculateChecksum(controllerChecksumCalculator); controllerCenterOfMassDataHolder.calculateChecksum(controllerChecksumCalculator); return controllerChecksumCalculator.getValue(); }
CenterOfMassDataHolder centerOfMassDataHolderToUpdate = new CenterOfMassDataHolder();
public void setFromEstimatorModel(long timestamp, long estimatorTick, long estimatorClockStartTime) { this.timestamp = timestamp; this.estimatorTick = estimatorTick; this.estimatorClockStartTime = estimatorClockStartTime; checksum = calculateEstimatorChecksum(); estimatorToIntermediateCopier.copy(); rawDataEstimatorToIntermadiateCopier.copy(); intermediateForceSensorDataHolder.set(estimatorForceSensorDataHolder); intermediateCenterOfMassDataHolder.set(estimatorCenterOfMassDataHolder); intermediateContactSensorHolder.set(estimatorContactSensorHolder); }
private long calculateControllerChecksum() { controllerChecksumCalculator.reset(); controllerChecksum.calculate(); controllerForceSensorDataHolder.calculateChecksum(controllerChecksumCalculator); controllerCenterOfMassDataHolder.calculateChecksum(controllerChecksumCalculator); return controllerChecksumCalculator.getValue(); }
public IntermediateEstimatorStateHolder(WholeBodyControllerParameters wholeBodyControlParameters, RigidBody estimatorRootBody, RigidBody controllerRootBody, ForceSensorDataHolder estimatorForceSensorDataHolder, ForceSensorDataHolder controllerForceSensorDataHolder, CenterOfMassDataHolder estimatorCenterOfMassDataHolder, CenterOfMassDataHolder controllerCenterOfMassDataHolder, ContactSensorHolder estimatorContactSensorHolder, ContactSensorHolder controllerContactSensorHolder, RawJointSensorDataHolderMap estimatorRawJointSensorDataHolderMap, RawJointSensorDataHolderMap controllerRawJointSensorDataHolderMap) { FullHumanoidRobotModel intermediateModel = wholeBodyControlParameters.createFullRobotModel(); RigidBody intermediateRootBody = intermediateModel.getElevator(); estimatorChecksum = new InverseDynamicsJointStateChecksum(estimatorRootBody, estimatorChecksumCalculator); controllerChecksum = new InverseDynamicsJointStateChecksum(controllerRootBody, controllerChecksumCalculator); estimatorToIntermediateCopier = new InverseDynamicsJointStateCopier(estimatorRootBody, intermediateRootBody); intermediateToControllerCopier = new InverseDynamicsJointStateCopier(intermediateRootBody, controllerRootBody); this.estimatorForceSensorDataHolder = estimatorForceSensorDataHolder; this.intermediateForceSensorDataHolder = new ForceSensorDataHolder(Arrays.asList(intermediateModel.getForceSensorDefinitions())); this.controllerForceSensorDataHolder = controllerForceSensorDataHolder; this.estimatorCenterOfMassDataHolder = estimatorCenterOfMassDataHolder; this.intermediateCenterOfMassDataHolder = new CenterOfMassDataHolder(); this.controllerCenterOfMassDataHolder = controllerCenterOfMassDataHolder; this.estimatorContactSensorHolder = estimatorContactSensorHolder; this.intermediateContactSensorHolder = new ContactSensorHolder(Arrays.asList(intermediateModel.getContactSensorDefinitions())); this.controllerContactSensorHolder = controllerContactSensorHolder; RawJointSensorDataHolderMap intermediateRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(intermediateModel); rawDataEstimatorToIntermadiateCopier = new RawJointSensorDataHolderMapCopier(estimatorRawJointSensorDataHolderMap, intermediateRawJointSensorDataHolderMap); rawDataIntermediateToControllerCopier = new RawJointSensorDataHolderMapCopier(intermediateRawJointSensorDataHolderMap, controllerRawJointSensorDataHolderMap); }
public void setFromEstimatorModel(long timestamp, long estimatorTick, long estimatorClockStartTime) { this.timestamp = timestamp; this.estimatorTick = estimatorTick; this.estimatorClockStartTime = estimatorClockStartTime; checksum = calculateEstimatorChecksum(); estimatorToIntermediateCopier.copy(); rawDataEstimatorToIntermadiateCopier.copy(); intermediateForceSensorDataHolder.set(estimatorForceSensorDataHolder); intermediateCenterOfMassDataHolder.set(estimatorCenterOfMassDataHolder); intermediateContactSensorHolder.set(estimatorContactSensorHolder); }
private long calculateEstimatorChecksum() { estimatorChecksumCalculator.reset(); estimatorChecksum.calculate(); estimatorForceSensorDataHolder.calculateChecksum(estimatorChecksumCalculator); estimatorCenterOfMassDataHolder.calculateChecksum(estimatorChecksumCalculator); return estimatorChecksumCalculator.getValue(); }
estimatorCenterOfMassDataHolder = new CenterOfMassDataHolder(); estimatorRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(estimatorFullRobotModel); estimatorContactSensorHolder = new ContactSensorHolder(Arrays.asList(estimatorFullRobotModel.getContactSensorDefinitions())); controllerCenterOfMassDataHolder = new CenterOfMassDataHolder(); controllerContactSensorHolder = new ContactSensorHolder(Arrays.asList(controllerFullRobotModel.getContactSensorDefinitions())); controllerRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(controllerFullRobotModel);
estimatorCenterOfMassDataHolder = new CenterOfMassDataHolder(); estimatorRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(estimatorFullRobotModel); estimatorContactSensorHolder = new ContactSensorHolder(Arrays.asList(estimatorFullRobotModel.getContactSensorDefinitions())); controllerCenterOfMassDataHolder = new CenterOfMassDataHolder(); controllerContactSensorHolder = new ContactSensorHolder(Arrays.asList(controllerFullRobotModel.getContactSensorDefinitions())); controllerRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(controllerFullRobotModel);
public IntermediateEstimatorStateHolder(FullHumanoidRobotModelFactory fullRobotModelFactory, RigidBodyBasics estimatorRootBody, RigidBodyBasics controllerRootBody, ForceSensorDataHolder estimatorForceSensorDataHolder, ForceSensorDataHolder controllerForceSensorDataHolder, CenterOfMassDataHolder estimatorCenterOfMassDataHolder, CenterOfMassDataHolder controllerCenterOfMassDataHolder, ContactSensorHolder estimatorContactSensorHolder, ContactSensorHolder controllerContactSensorHolder, RawJointSensorDataHolderMap estimatorRawJointSensorDataHolderMap, RawJointSensorDataHolderMap controllerRawJointSensorDataHolderMap) { FullHumanoidRobotModel intermediateModel = fullRobotModelFactory.createFullRobotModel(); RigidBodyBasics intermediateRootBody = intermediateModel.getElevator(); estimatorChecksum = new InverseDynamicsJointStateChecksum(estimatorRootBody, estimatorChecksumCalculator); controllerChecksum = new InverseDynamicsJointStateChecksum(controllerRootBody, controllerChecksumCalculator); estimatorToIntermediateCopier = new InverseDynamicsJointStateCopier(estimatorRootBody, intermediateRootBody); intermediateToControllerCopier = new InverseDynamicsJointStateCopier(intermediateRootBody, controllerRootBody); this.estimatorForceSensorDataHolder = estimatorForceSensorDataHolder; this.intermediateForceSensorDataHolder = new ForceSensorDataHolder(Arrays.asList(intermediateModel.getForceSensorDefinitions())); this.controllerForceSensorDataHolder = controllerForceSensorDataHolder; this.estimatorCenterOfMassDataHolder = estimatorCenterOfMassDataHolder; this.intermediateCenterOfMassDataHolder = new CenterOfMassDataHolder(); this.controllerCenterOfMassDataHolder = controllerCenterOfMassDataHolder; this.estimatorContactSensorHolder = estimatorContactSensorHolder; this.intermediateContactSensorHolder = new ContactSensorHolder(Arrays.asList(intermediateModel.getContactSensorDefinitions())); this.controllerContactSensorHolder = controllerContactSensorHolder; RawJointSensorDataHolderMap intermediateRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(intermediateModel); rawDataEstimatorToIntermadiateCopier = new RawJointSensorDataHolderMapCopier(estimatorRawJointSensorDataHolderMap, intermediateRawJointSensorDataHolderMap); rawDataIntermediateToControllerCopier = new RawJointSensorDataHolderMapCopier(intermediateRawJointSensorDataHolderMap, controllerRawJointSensorDataHolderMap); }
CenterOfMassDataHolder centerOfMassDataHolderToUpdate = new CenterOfMassDataHolder();
CenterOfMassDataHolder centerOfMassDataHolderToUpdate = new CenterOfMassDataHolder();
CenterOfMassDataHolder centerOfMassDataHolderToUpdate = new CenterOfMassDataHolder();
CenterOfMassDataHolder centerOfMassDataHolderToUpdate = new CenterOfMassDataHolder();