public void set(ContactSensorHolder otherContactSensorHolder) { for(int i = 0; i < contactSensorDefinitions.size(); i++) { final ContactSensorDefinition contactSensorDefinition = contactSensorDefinitions.get(i); ContactSensor otherContactSensor = otherContactSensorHolder.getByDefinition(contactSensorDefinition); contactSensors.get(contactSensorDefinition).setIsInContact(otherContactSensor.isInContact()); } } }
public void getIntoControllerModel() { intermediateToControllerCopier.copy(); rawDataIntermediateToControllerCopier.copy(); controllerForceSensorDataHolder.set(intermediateForceSensorDataHolder); controllerCenterOfMassDataHolder.set(intermediateCenterOfMassDataHolder); controllerContactSensorHolder.set(intermediateContactSensorHolder); }
estimatorCenterOfMassDataHolder = new CenterOfMassDataHolder(); estimatorRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(estimatorFullRobotModel); estimatorContactSensorHolder = new ContactSensorHolder(Arrays.asList(estimatorFullRobotModel.getContactSensorDefinitions())); estimatorRobotMotionStatusHolder = new RobotMotionStatusHolder(); estimatorDesiredJointDataHolder = new JointDesiredOutputList(estimatorFullRobotModel.getControllableOneDoFJoints());
footForceSensorForEstimator, contactSensorHolder.getByName(footContactSensorName), footSwitchCoPThresholdFraction, totalRobotWeight, bipedFeet.get(robotSide), null, contactThresholdForce, registry); footSwitchMap.put(foot, wrenchAndContactSensorBasedFootswitch);
estimatorCenterOfMassDataHolder = new CenterOfMassDataHolder(); estimatorRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(estimatorFullRobotModel); estimatorContactSensorHolder = new ContactSensorHolder(Arrays.asList(estimatorFullRobotModel.getContactSensorDefinitions())); estimatorRobotMotionStatusHolder = new RobotMotionStatusHolder(); estimatorDesiredJointDataHolder = new DesiredJointDataHolder(estimatorFullRobotModel.getOneDoFJoints());
footForceSensorForEstimator, contactSensorHolder.getByName(footContactSensorName), footSwitchCoPThresholdFraction, totalRobotWeight, bipedFeet.get(robotSide), null, contactThresholdForce, registry); footSwitchMap.put(foot, wrenchAndContactSensorBasedFootswitch);
estimatorCenterOfMassDataHolder = new CenterOfMassDataHolder(); estimatorRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(estimatorFullRobotModel); estimatorContactSensorHolder = new ContactSensorHolder(Arrays.asList(estimatorFullRobotModel.getContactSensorDefinitions())); estimatorRobotMotionStatusHolder = new RobotMotionStatusHolder(); estimatorDesiredJointDataHolder = new JointDesiredOutputList(estimatorFullRobotModel.getControllableOneDoFJoints()); controllerForceSensorDataHolder = new ForceSensorDataHolder(Arrays.asList(controllerFullRobotModel.getForceSensorDefinitions())); controllerCenterOfMassDataHolder = new CenterOfMassDataHolder(); controllerContactSensorHolder = new ContactSensorHolder(Arrays.asList(controllerFullRobotModel.getContactSensorDefinitions())); controllerRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(controllerFullRobotModel); controllerRobotMotionStatusHolder = new RobotMotionStatusHolder();
contactSensorHolder.getByName(footContactSensorNames.get(robotSide)), footSwitchCoPThresholdFraction, totalRobotWeight, bipedFeet.get(robotSide), yoGraphicsListRegistry, contactThresholdForce, registry); break;
public void getIntoControllerModel() { intermediateToControllerCopier.copy(); rawDataIntermediateToControllerCopier.copy(); controllerForceSensorDataHolder.set(intermediateForceSensorDataHolder); controllerCenterOfMassDataHolder.set(intermediateCenterOfMassDataHolder); controllerContactSensorHolder.set(intermediateContactSensorHolder); }
public void set(ContactSensorHolder otherContactSensorHolder) { for(int i = 0; i < contactSensorDefinitions.size(); i++) { final ContactSensorDefinition contactSensorDefinition = contactSensorDefinitions.get(i); ContactSensor otherContactSensor = otherContactSensorHolder.getByDefinition(contactSensorDefinition); contactSensors.get(contactSensorDefinition).setIsInContact(otherContactSensor.isInContact()); } } }
estimatorCenterOfMassDataHolder = new CenterOfMassDataHolder(); estimatorRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(estimatorFullRobotModel); estimatorContactSensorHolder = new ContactSensorHolder(Arrays.asList(estimatorFullRobotModel.getContactSensorDefinitions())); estimatorRobotMotionStatusHolder = new RobotMotionStatusHolder(); estimatorDesiredJointDataHolder = new DesiredJointDataHolder(estimatorFullRobotModel.getOneDoFJoints()); controllerForceSensorDataHolder = new ForceSensorDataHolder(Arrays.asList(controllerFullRobotModel.getForceSensorDefinitions())); controllerCenterOfMassDataHolder = new CenterOfMassDataHolder(); controllerContactSensorHolder = new ContactSensorHolder(Arrays.asList(controllerFullRobotModel.getContactSensorDefinitions())); controllerRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(controllerFullRobotModel); controllerRobotMotionStatusHolder = new RobotMotionStatusHolder();
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); }
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); }
IMUDefinition[] imuDefinitions = fullRobotModel.getIMUDefinitions(); ForceSensorDefinition[] forceSensorDefinitions = fullRobotModel.getForceSensorDefinitions(); ContactSensorHolder contactSensorHolder = new ContactSensorHolder(Arrays.asList(fullRobotModel.getContactSensorDefinitions())); RawJointSensorDataHolderMap rawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(fullRobotModel); sensorReaderFactory.build(rootJoint, imuDefinitions, forceSensorDefinitions, contactSensorHolder, rawJointSensorDataHolderMap,
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); }