public void writeControllerDataFromController() { for (int i = 0; i < controllerFeet.size(); i++) { RigidBody foot = controllerFeet.get(i); controllerCenterOfPressureDataHolder.getCenterOfPressureByName(centerOfPressure.get(foot.getName()), foot); } robotMotionStatus.set(controllerRobotMotionStatusHolder.getCurrentRobotMotionStatus()); intermediateDesiredJointDataHolder.copyFromController(); }
@Override public void robotMotionStatusHasChanged(RobotMotionStatus newStatus, double time) { controllerRobotMotionStatusHolder.setCurrentRobotMotionStatus(newStatus); } };
imuSensorsToUseInStateEstimator, gravitationalAcceleration, footSwitchMap, centerOfPressureDataHolder, new RobotMotionStatusHolder(), bipedFeetMap, yoGraphicsListRegistry); return stateEstimator;
estimatorRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(estimatorFullRobotModel); estimatorContactSensorHolder = new ContactSensorHolder(Arrays.asList(estimatorFullRobotModel.getContactSensorDefinitions())); estimatorRobotMotionStatusHolder = new RobotMotionStatusHolder(); estimatorDesiredJointDataHolder = new JointDesiredOutputList(estimatorFullRobotModel.getControllableOneDoFJoints());
estimatorRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(estimatorFullRobotModel); estimatorContactSensorHolder = new ContactSensorHolder(Arrays.asList(estimatorFullRobotModel.getContactSensorDefinitions())); estimatorRobotMotionStatusHolder = new RobotMotionStatusHolder(); estimatorDesiredJointDataHolder = new DesiredJointDataHolder(estimatorFullRobotModel.getOneDoFJoints());
public void writeControllerDataFromController() { for (int i = 0; i < controllerFeet.size(); i++) { RigidBodyBasics foot = controllerFeet.get(i); controllerCenterOfPressureDataHolder.getCenterOfPressureByName(centerOfPressure.get(foot.getName()), foot); } robotMotionStatus.set(controllerRobotMotionStatusHolder.getCurrentRobotMotionStatus()); intermediateDesiredJointDataHolder.copyFromController(); }
@Override public void robotMotionStatusHasChanged(RobotMotionStatus newStatus, double time) { controllerRobotMotionStatusHolder.setCurrentRobotMotionStatus(newStatus); } };
estimatorRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(estimatorFullRobotModel); estimatorContactSensorHolder = new ContactSensorHolder(Arrays.asList(estimatorFullRobotModel.getContactSensorDefinitions())); estimatorRobotMotionStatusHolder = new RobotMotionStatusHolder(); estimatorDesiredJointDataHolder = new JointDesiredOutputList(estimatorFullRobotModel.getControllableOneDoFJoints()); controllerContactSensorHolder = new ContactSensorHolder(Arrays.asList(controllerFullRobotModel.getContactSensorDefinitions())); controllerRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(controllerFullRobotModel); controllerRobotMotionStatusHolder = new RobotMotionStatusHolder(); controllerDesiredJointDataHolder = new JointDesiredOutputList(controllerFullRobotModel.getControllableOneDoFJoints());
state.setRobotMotionStatus(robotMotionStatusFromController.getCurrentRobotMotionStatus());
public void readControllerDataIntoEstimator() { for (int i = 0; i < estimatorFeet.size(); i++) { RigidBody foot = estimatorFeet.get(i); estimatorCenterOfPressureDataHolder.setCenterOfPressure(centerOfPressure.get(foot.getName()), foot); } if (robotMotionStatus.get() != null) estimatorRobotMotionStatusHolder.setCurrentRobotMotionStatus(robotMotionStatus.getAndSet(null)); intermediateDesiredJointDataHolder.readIntoEstimator(); }
estimatorRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(estimatorFullRobotModel); estimatorContactSensorHolder = new ContactSensorHolder(Arrays.asList(estimatorFullRobotModel.getContactSensorDefinitions())); estimatorRobotMotionStatusHolder = new RobotMotionStatusHolder(); estimatorDesiredJointDataHolder = new DesiredJointDataHolder(estimatorFullRobotModel.getOneDoFJoints()); controllerContactSensorHolder = new ContactSensorHolder(Arrays.asList(controllerFullRobotModel.getContactSensorDefinitions())); controllerRawJointSensorDataHolderMap = new RawJointSensorDataHolderMap(controllerFullRobotModel); controllerRobotMotionStatusHolder = new RobotMotionStatusHolder(); controllerDesiredJointDataHolder = new DesiredJointDataHolder(controllerFullRobotModel.getOneDoFJoints());
public void readControllerDataIntoEstimator() { for (int i = 0; i < estimatorFeet.size(); i++) { RigidBodyBasics foot = estimatorFeet.get(i); estimatorCenterOfPressureDataHolder.setCenterOfPressure(centerOfPressure.get(foot.getName()), foot); } if (robotMotionStatus.get() != null) estimatorRobotMotionStatusHolder.setCurrentRobotMotionStatus(robotMotionStatus.getAndSet(null)); intermediateDesiredJointDataHolder.readIntoEstimator(); }
sensorOutputMapReadOnly, centerOfMassDataHolderToUpdate, imuSensorsToUseInStateEstimator, gravityMagnitude, footSwitchMap, null, new RobotMotionStatusHolder(), bipedFeetMap, yoGraphicsListRegistry);
imuSensorsToUseInStateEstimator, gravitationalAcceleration, footSwitchMap, null, new RobotMotionStatusHolder(), bipedFeetMap, null); simulationRegistry.addChild(stateEstimator.getYoVariableRegistry());
imuSensorsToUseInStateEstimator, gravitationalAcceleration, footSwitchMap, null, new RobotMotionStatusHolder(), bipedFeetMap, null); simulationRegistry.addChild(stateEstimator.getYoVariableRegistry());
footSwitchMap, null, new RobotMotionStatusHolder(), bipedFeetMap, null); simulationRegistry.addChild(stateEstimator.getYoVariableRegistry());
SensorOutputMapReadOnly sensorOutputMapReadOnly = initializeSensorOutputMapReadOnly(); SensorRawOutputMapReadOnly sensorRawOutputMapReadOnly = initializeSensorRawOutputMapReadOnly(); RobotMotionStatusHolder robotMotionStatusFromController = new RobotMotionStatusHolder(); DRCRobotSensorInformation sensorInformation = drcRobotModel.getSensorInformation(); PeriodicNonRealtimeThreadScheduler scheduler = new PeriodicNonRealtimeThreadScheduler(threadName);
SensorOutputMapReadOnly sensorOutputMapReadOnly = initializeSensorOutputMapReadOnly(); SensorRawOutputMapReadOnly sensorRawOutputMapReadOnly = initializeSensorRawOutputMapReadOnly(); RobotMotionStatusHolder robotMotionStatusFromController = new RobotMotionStatusHolder(); DRCRobotSensorInformation sensorInformation = drcRobotModel.getSensorInformation(); MessageTopicNameGenerator publisherTopicNameGenerator = ControllerAPIDefinition.getPublisherTopicNameGenerator(drcRobotModel.getSimpleRobotName());