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();