How to use

updateRobotModel

method

in

us.ihmc.humanoidRobotics.communication.subscribers.HumanoidRobotDataReceiver

public HumanoidReferenceFrames getUpdatedReferenceFramesCopy()
{
 updateRobotModel();
 HumanoidReferenceFrames ret = new HumanoidReferenceFrames(fullRobotModel);
 return ret;
}

public RigidBodyTransform getCurrentTestFrameTransformToWorld()
{
 robotDataReceiver.updateRobotModel();
 frameToKeepTrackOf.getTransformToDesiredFrame(currentTransformToWorld, worldFrame);
 return currentTransformToWorld;
}

public HumanoidReferenceFrames getUpdatedReferenceFramesCopy()
{
 updateRobotModel();
 HumanoidReferenceFrames ret = new HumanoidReferenceFrames(fullRobotModel);
 return ret;
}

public RigidBodyTransform getCurrentTestFrameTransformToWorld()
{
 robotDataReceiver.updateRobotModel();
 frameToKeepTrackOf.getTransformToDesiredFrame(currentTransformToWorld, worldFrame);
 return currentTransformToWorld;
}

public void updateRobotModel()
{
 yoTimeLastFullRobotModelUpdate.set(yoTimeRobot.getDoubleValue());
 robotDataReceiver.updateRobotModel();
}

public void run()
{
  while (isRunning)
  {
   robotDataReceiver.updateRobotModel();
   for (AbstractBehavior behavior : behaviors)
   {
     behavior.doControl();
   }
   for (Updatable updatable : updatables)
   {
     updatable.update(yoTimeRobot.getDoubleValue());
   }
   ThreadTools.sleep(1);
  }
}

public void update(MocapRigidBody mocapObject)
{
 if(enableMocapUpdates)
 {
   mocapObject.getPose(workingRigidBodyTransform);
   workingRigidBodyTransform.multiply(mocapJigCalibrationTransform);
   workingRigidBodyTransform.multiply(transformFromMocapHeadCentroidToHeadRoot);
   mocapHeadPoseInZUp.set(workingRigidBodyTransform);
      mocapHeadFrame.update();
   robotDataReceiver.updateRobotModel();
   mocapHeadFrame.getTransformToDesiredFrame(transformFromMocapHeadToRobotHead , robotHeadFrame);
   mocapOffsetFrame.update();
 }
}

public void update(MocapRigidBody mocapObject)
{
 if(enableMocapUpdates)
 {
   mocapObject.packPose(workingRigidBodyTransform);
   workingRigidBodyTransform.multiply(mocapJigCalibrationTransform);
   workingRigidBodyTransform.multiply(transformFromMocapHeadCentroidToHeadRoot);
   mocapHeadPoseInZUp.set(workingRigidBodyTransform);
      mocapHeadFrame.update();
   robotDataReceiver.updateRobotModel();
   mocapHeadFrame.getTransformToDesiredFrame(transformFromMocapHeadToRobotHead , robotHeadFrame);
   mocapOffsetFrame.update();
 }
}

public void update(MocapRigidBody mocapObject)
{
 if(enableMocapUpdates)
 {
   mocapObject.packPose(workingRigidBodyTransform);
   workingRigidBodyTransform.multiply(mocapJigCalibrationTransform);
   workingRigidBodyTransform.multiply(transformFromMocapHeadCentroidToHeadRoot);
   mocapHeadPoseInZUp.set(workingRigidBodyTransform);
      mocapHeadFrame.update();
   robotDataReceiver.updateRobotModel();
   mocapHeadFrame.getTransformToDesiredFrame(transformFromMocapHeadToRobotHead , robotHeadFrame);
   mocapOffsetFrame.update();
 }
}

/**
* received a point cloud from the multisense
* transform the points using the mocap data
*/
@Override
public void onNewMessage(PointCloud2 pointCloud)
{
 robotDataReceiver.updateRobotModel();
 pointCloudReferenceFrame.update();
 UnpackedPointCloud pointCloudData = unpackPointsAndIntensities(pointCloud);
 Point3d[] points = pointCloudData.getPoints();
 
 RigidBodyTransform pointTransform = new RigidBodyTransform(headInMocapFrame.get());
 RigidBodyTransform transformFromPointCloudOriginToHeadRoot = pointCloudReferenceFrame.getTransformToDesiredFrame(headRootReferenceFrame);
 pointTransform.multiply(transformFromPointCloudOriginToHeadRoot);
 
 for(int i = 0; i < points.length; i++)
 {
   pointTransform.transform(points[i]);
 }
 
 MultisenseMocapExperimentPacket pointCloudPacket = new MultisenseMocapExperimentPacket();
 pointCloudPacket.setDestination(PacketDestination.UI);
 pointCloudPacket.setPointCloud(points, testInfo);
 packetCommunicator.send(pointCloudPacket);
}

/**
* received a point cloud from the multisense
* transform the points using the mocap data
*/
@Override
public void onNewMessage(PointCloud2 pointCloud)
{
 robotDataReceiver.updateRobotModel();
 pointCloudReferenceFrame.update();
 UnpackedPointCloud pointCloudData = unpackPointsAndIntensities(pointCloud);
 Point3d[] points = pointCloudData.getPoints();
 
 RigidBodyTransform pointTransform = new RigidBodyTransform(headInMocapFrame.get());
 RigidBodyTransform transformFromPointCloudOriginToHeadRoot = pointCloudReferenceFrame.getTransformToDesiredFrame(headRootReferenceFrame);
 pointTransform.multiply(transformFromPointCloudOriginToHeadRoot);
 
 for(int i = 0; i < points.length; i++)
 {
   pointTransform.transform(points[i]);
 }
 
 MultisenseMocapExperimentPacket pointCloudPacket = new MultisenseMocapExperimentPacket();
 pointCloudPacket.setDestination(PacketDestination.UI);
 pointCloudPacket.setPointCloud(points, testInfo);
 packetCommunicator.send(pointCloudPacket);
}

humanoidRobotDataReceiver.updateRobotModel();

robotDataReceiver.updateRobotModel();