How to use us.ihmc.sensorProcessing.model

public void copyFromController()
{
 controllerDataHolder.updateFromModel();
 for(int i = 0; i < controllerDataHolder.getJoints().length; i++)
 {
   intermediateDesiredJointData[i].set(controllerDataHolder.get(i));
 }
 
}

  public void readIntoEstimator()
  {
   for(int i = 0; i < estimatorDataHolder.getJoints().length; i++)
   {
     estimatorDataHolder.get(i).set(intermediateDesiredJointData[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();
}