public void copyFromController() { controllerDataHolder.updateFromModel(); for(int i = 0; i < controllerDataHolder.getJoints().length; i++) { intermediateDesiredJointData[i].set(controllerDataHolder.get(i)); } }
@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 DesiredJointDataHolder(estimatorFullRobotModel.getOneDoFJoints());
public void set(DesiredJointData desiredJointData) { setTauDesired(desiredJointData.getTauDesired()); setQddDesired(desiredJointData.getQddDesired()); setPositionDesired(desiredJointData.getPositionDesired()); }
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(); }
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 static RobotConfigurationData nextRobotConfigurationData(Random random) { RobotConfigurationData next = new RobotConfigurationData(); int size = random.nextInt(10000); next.setTimestamp(random.nextLong()); next.setSensorHeadPpsTimestamp(random.nextLong()); next.setJointNameHash(random.nextInt(10000)); next.getJointAngles().add(RandomNumbers.nextFloatArray(random, size, 1.0f)); next.getJointVelocities().add(RandomNumbers.nextFloatArray(random, size, 1.0f)); next.getJointTorques().add(RandomNumbers.nextFloatArray(random, size, 1.0f)); next.getRootTranslation().set(EuclidCoreRandomTools.nextVector3D32(random)); next.getPelvisLinearVelocity().set(EuclidCoreRandomTools.nextVector3D32(random)); next.getPelvisAngularVelocity().set(EuclidCoreRandomTools.nextVector3D32(random)); next.getRootOrientation().set(EuclidCoreRandomTools.nextQuaternion32(random)); next.getPelvisLinearAcceleration().set(EuclidCoreRandomTools.nextVector3D32(random)); size = Math.abs(random.nextInt(1000)); for (int i = 0; i < next.getForceSensorData().size(); i++) next.getForceSensorData().add().set(nextSpatialVectorMessage(random)); for (IMUPacket imuPacket : nextIMUPackets(random)) next.getImuSensorData().add().set(imuPacket); next.setRobotMotionStatus(RandomNumbers.nextEnum(random, RobotMotionStatus.class).toByte()); next.setLastReceivedPacketTypeId(random.nextInt(1000)); next.setLastReceivedPacketUniqueId(random.nextLong()); next.setLastReceivedPacketRobotTimestamp(random.nextLong()); return next; }
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()); } }
@Override public void publishControllerData() { controllerDesiredJointDataHolder.updateFromModel(); }
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 reportChangeOfRobotMotionStatus(RobotMotionStatus newStatus) { for (int i = 0; i < robotMotionStatusChangedListeners.size(); i++) { robotMotionStatusChangedListeners.get(i).robotMotionStatusHasChanged(newStatus, yoTime.getDoubleValue()); } }
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 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); } };
public void sendMockRobotConfiguration(long totalNsecs) { IMUDefinition[] imuDefinitions = fullRobotModel.getIMUDefinitions(); RobotConfigurationData robotConfigurationData = RobotConfigurationDataFactory.create(FullRobotModelUtils.getAllJointsExcludingHands(fullRobotModel), forceSensorDefinitions, imuDefinitions); for (int sensorNumber = 0; sensorNumber < imuDefinitions.length; sensorNumber++) { robotConfigurationData.getImuSensorData().add(); } robotConfigurationData.setRobotMotionStatus(RobotMotionStatus.STANDING.toByte()); robotConfigurationData.setTimestamp(totalNsecs); Vector3D translation = new Vector3D(); Quaternion orientation = new Quaternion(); robotConfigurationData.getRootTranslation().set(translation); robotConfigurationData.getRootOrientation().set(orientation); publisher.publish(robotConfigurationData); }
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(); }
public void receivedClockMessage(long totalNsecs) { RigidBodyTransform pelvisPoseInMocapFrame = atomicPelvisPose.get(); IMUDefinition[] imuDefinitions = fullRobotModel.getIMUDefinitions(); RobotConfigurationData robotConfigurationData = RobotConfigurationDataFactory.create(fullRobotModel.getOneDoFJoints(), forceSensorDefinitions, imuDefinitions); for(int sensorNumber = 0; sensorNumber < imuDefinitions.length; sensorNumber++) { IMUPacket imuPacket = robotConfigurationData.getImuSensorData().add(); imuPacket.getLinearAcceleration().set(RandomGeometry.nextVector3D32(random)); imuPacket.getOrientation().set(RandomGeometry.nextQuaternion32(random)); imuPacket.getAngularVelocity().set(RandomGeometry.nextVector3D32(random)); } robotConfigurationData.setRobotMotionStatus(RobotMotionStatus.STANDING.toByte()); robotConfigurationData.setTimestamp(totalNsecs); if(pelvisPoseInMocapFrame != null) { Vector3D translation = new Vector3D(); Quaternion orientation = new Quaternion(); pelvisPoseInMocapFrame.getTranslation(translation); pelvisPoseInMocapFrame.getRotation(orientation); robotConfigurationData.getRootTranslation().set(translation); robotConfigurationData.getRootOrientation().set(orientation); } fullRobotModel.updateFrames(); packetCommunicator.send(robotConfigurationData); }
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(); }