private void writeHeaderFile(File header, PlanarRegionSegmentationMessage[] segmentationData) throws IOException { FileWriter fileWriter = new FileWriter(header); for (PlanarRegionSegmentationMessage message : segmentationData) { Point3D32 origin = message.getOrigin(); Vector3D32 normal = message.getNormal(); fileWriter.write("regionId: "); fileWriter.write(Integer.toString(message.getRegionId())); fileWriter.write(", origin: "); fileWriter.write(origin.getX() + ", " + origin.getY() + ", " + origin.getZ()); fileWriter.write(", normal: "); fileWriter.write(normal.getX() + ", " + normal.getY() + ", " + normal.getZ()); fileWriter.write("\n"); } fileWriter.close(); }
Point3D32 boxCenter = newMessage.getCenter(); MeshDataHolder boxMeshDataHolder = MeshDataGenerator.Cube(boxSize.getX(), boxSize.getY(), boxSize.getZ(), true); boxMeshDataHolder = MeshDataHolder.rotate(boxMeshDataHolder, boxOrientation); boxMeshDataHolder = MeshDataHolder.translate(boxMeshDataHolder, boxCenter);
@Override public void receivedPacket(RobotConfigurationData packet) { latestRobotConfigurationData = packet; FloatingJointBasics rootJoint = fullRobotModel.getRootJoint(); TFloatArrayList newJointAngles = packet.getJointAngles(); TFloatArrayList newJointVelocities = packet.getJointAngles(); TFloatArrayList newJointTorques = packet.getJointTorques(); OneDoFJointBasics[] oneDoFJoints = fullRobotModel.getOneDoFJoints(); for (int i = 0; i < newJointAngles.size(); i++) { oneDoFJoints[i].setQ(newJointAngles.get(i)); oneDoFJoints[i].setQd(newJointVelocities.get(i)); oneDoFJoints[i].setTau(newJointTorques.get(i)); } pelvisTranslationFromRobotConfigurationData.set(packet.getRootTranslation()); pelvisOrientationFromRobotConfigurationData.set(packet.getRootOrientation()); rootJoint.getJointPose().setPosition(pelvisTranslationFromRobotConfigurationData.getX(), pelvisTranslationFromRobotConfigurationData.getY(), pelvisTranslationFromRobotConfigurationData.getZ()); rootJoint.getJointPose().getOrientation().setQuaternion(pelvisOrientationFromRobotConfigurationData.getX(), pelvisOrientationFromRobotConfigurationData.getY(), pelvisOrientationFromRobotConfigurationData.getZ(), pelvisOrientationFromRobotConfigurationData.getS()); computeDriftTransform(); rootJoint.getPredecessor().updateFramesRecursively(); yoVariableServer.update(System.currentTimeMillis()); }