estimatorCenterOfMassDataHolder = new CenterOfMassDataHolder(); estimatorRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(estimatorFullRobotModel); estimatorContactSensorHolder = new ContactSensorHolder(Arrays.asList(estimatorFullRobotModel.getContactSensorDefinitions()));
estimatorCenterOfMassDataHolder = new CenterOfMassDataHolder(); estimatorRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(estimatorFullRobotModel); estimatorContactSensorHolder = new ContactSensorHolder(Arrays.asList(estimatorFullRobotModel.getContactSensorDefinitions()));
CenterOfMassDataHolder centerOfMassDataHolderToUpdate = new CenterOfMassDataHolder();
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); }
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();