diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index ce9fd75f5d..aedbac59e7 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -184,18 +184,84 @@ struct AirSimSettings { }; struct BarometerSetting : SensorSetting { + real_T qnh = EarthUtils::SeaLevelPressure / 100.0f; // hPa + real_T pressure_factor_sigma = 0.0365f / 20; + real_T pressure_factor_tau = 3600; + real_T unnorrelated_noise_sigma = 0.027f * 100; + real_T update_latency = 0.0f; //sec + real_T update_frequency = 50; //Hz + real_T startup_delay = 0; //sec + Vector3r position = VectorMath::nanVector(); + Rotation rotation = Rotation::nanRotation(); }; struct ImuSetting : SensorSetting { + //angule random walk (ARW) + real_T gyro_arw = 0.30f / sqrt(3600.0f) * M_PIf / 180; //deg/sqrt(hour) converted to rad/sqrt(sec) + //Bias Stability (tau = 500s) + real_T gyro_tau = 500; + real_T gyro_bias_stability = 4.6f / 3600 * M_PIf / 180; //deg/hr converted to rad/sec + Vector3r gyro_turn_on_bias = Vector3r::Zero(); //assume calibration is done + + //velocity random walk (ARW) + real_T accel_vrw = 0.24f * EarthUtils::Gravity / 1.0E3f; //mg converted to m/s^2 + //Bias Stability (tau = 800s) + real_T accel_tau = 800; + real_T accel_bias_stability = 36.0f * 1E-6f * 9.80665f; //ug converted to m/s^2 + Vector3r accel_turn_on_bias = Vector3r::Zero(); //assume calibration is done + Vector3r position = VectorMath::nanVector(); + Rotation rotation = Rotation::nanRotation(); + }; struct GpsSetting : SensorSetting { + + real_T eph_time_constant = 0.9f, epv_time_constant = 0.9f; + real_T eph_initial = 100.0f, epv_initial = 100.0f; //initially fully diluted positions + real_T eph_final = 0.3f, epv_final = 0.4f; + real_T eph_min_3d = 3.0f, eph_min_2d = 4.0f; + + real_T update_latency = 0.2f; //sec + real_T update_frequency = 50; //Hz + real_T startup_delay = 1; //sec + + Vector3r position = VectorMath::nanVector(); + Rotation rotation = Rotation::nanRotation(); + }; struct MagnetometerSetting : SensorSetting { + Vector3r noise_sigma = Vector3r(0.005f, 0.005f, 0.005f); //5 mgauss as per specs sheet (RMS is same as stddev) https://goo.gl/UOz6FT + real_T scale_factor = 1.0f; + Vector3r noise_bias = Vector3r(0.0f, 0.0f, 0.0f); //no offset as per specsheet (zero gauss level) https://goo.gl/UOz6FT + float ref_update_frequency = 0.2f; //Hz + + //use dipole model if there is enough compute power available + bool dynamic_reference_source = true; + // ref_source 0==ConstantModel; 1==DipoleModel + int ref_source = 1; + + //see PX4 param reference for EKF: https://dev.px4.io/en/advanced/parameter_reference.html + real_T update_latency = 0.0f; //sec: from PX4 doc + real_T update_frequency = 50; //Hz + real_T startup_delay = 0; //sec + + Vector3r position = VectorMath::nanVector(); + Rotation rotation = Rotation::nanRotation(); + }; struct DistanceSetting : SensorSetting { + //nan means keep the default values set in components + Vector3r position = VectorMath::nanVector(); + Rotation rotation = Rotation::nanRotation(); + + real_T min_distance = 20.0f / 100; //m + real_T max_distance = 4000.0f / 100; //m + real_T unnorrelated_noise_sigma = 0.002f * 100; + real_T update_latency = 0.0f; //sec + real_T update_frequency = 50; //Hz + real_T startup_delay = 0; //sec }; struct LidarSetting : SensorSetting { @@ -694,6 +760,7 @@ struct AirSimSettings { settings_json.getFloat("Y", default_vec.y()), settings_json.getFloat("Z", default_vec.z())); } + static Rotation createRotationSetting(const Settings& settings_json, const Rotation& default_rot) { return Rotation(settings_json.getFloat("Yaw", default_rot.yaw), @@ -701,6 +768,34 @@ struct AirSimSettings { settings_json.getFloat("Roll", default_rot.roll)); } + static Vector3r createGyroBiasSetting(const Settings& settings_json, const Vector3r& default_vec) + { + return Vector3r(settings_json.getFloat("GyroBias_X", default_vec.x()), + settings_json.getFloat("GyroBias_Y", default_vec.y()), + settings_json.getFloat("GyroBias_Z", default_vec.z())); + } + + static Vector3r createAccelBiasSetting(const Settings& settings_json, const Vector3r& default_vec) + { + return Vector3r(settings_json.getFloat("AccelBias_X", default_vec.x()), + settings_json.getFloat("AccelBias_Y", default_vec.y()), + settings_json.getFloat("AccelBias_Z", default_vec.z())); + } + + static Vector3r createNoiseSigmaSetting(const Settings& settings_json, const Vector3r& default_vec) + { + return Vector3r(settings_json.getFloat("NoiseSigma_X", default_vec.x()), + settings_json.getFloat("NoiseSigma_Y", default_vec.y()), + settings_json.getFloat("NoiseSigma_Z", default_vec.z())); + } + + static Vector3r createNoiseBiasSetting(const Settings& settings_json, const Vector3r& default_vec) + { + return Vector3r(settings_json.getFloat("NoiseBias_X", default_vec.x()), + settings_json.getFloat("NoiseBias_Y", default_vec.y()), + settings_json.getFloat("NoiseBias_Z", default_vec.z())); + } + static std::unique_ptr createVehicleSetting(const std::string& simmode_name, const Settings& settings_json, const std::string vehicle_name) { @@ -1122,42 +1217,75 @@ struct AirSimSettings { static void initializeBarometerSetting(BarometerSetting& barometer_setting, const Settings& settings_json) { - unused(barometer_setting); - unused(settings_json); - - //TODO: set from json as needed + barometer_setting.qnh = settings_json.getFloat("SeaLevelPressure", barometer_setting.qnh); + barometer_setting.pressure_factor_sigma = settings_json.getFloat("PressureFactorSigma", barometer_setting.pressure_factor_sigma); + barometer_setting.pressure_factor_tau = settings_json.getFloat("PressureFactorTau", barometer_setting.pressure_factor_tau); + barometer_setting.unnorrelated_noise_sigma = settings_json.getFloat("NoiseSigma", barometer_setting.unnorrelated_noise_sigma); + barometer_setting.update_latency = settings_json.getFloat("UpdateLatency", barometer_setting.update_latency); + barometer_setting.update_frequency = settings_json.getFloat("UpdateFrequency", barometer_setting.update_frequency); + barometer_setting.startup_delay = settings_json.getFloat("StartupDelay", barometer_setting.startup_delay); + barometer_setting.position = createVectorSetting(settings_json, barometer_setting.position); + barometer_setting.rotation = createRotationSetting(settings_json, barometer_setting.rotation); } static void initializeImuSetting(ImuSetting& imu_setting, const Settings& settings_json) { - unused(imu_setting); - unused(settings_json); - - //TODO: set from json as needed + imu_setting.gyro_arw = settings_json.getFloat("GyroAngleRandomWalk", imu_setting.gyro_arw); + imu_setting.gyro_tau = settings_json.getFloat("GyroBiasTau", imu_setting.gyro_tau); + imu_setting.gyro_bias_stability = settings_json.getFloat("GyroBiasStability", imu_setting.gyro_bias_stability); + imu_setting.gyro_turn_on_bias = createGyroBiasSetting(settings_json, imu_setting.gyro_turn_on_bias); + + imu_setting.accel_vrw = settings_json.getFloat("AccelerometerVelocityRandomWalk", imu_setting.accel_vrw); + imu_setting.accel_tau = settings_json.getFloat("AccelerometerBiasTau", imu_setting.accel_tau); + imu_setting.accel_bias_stability = settings_json.getFloat("AccelerometerBiasStability", imu_setting.accel_bias_stability); + imu_setting.accel_turn_on_bias = createAccelBiasSetting(settings_json, imu_setting.accel_turn_on_bias); + + imu_setting.position = createVectorSetting(settings_json, imu_setting.position); + imu_setting.rotation = createRotationSetting(settings_json, imu_setting.rotation); } static void initializeGpsSetting(GpsSetting& gps_setting, const Settings& settings_json) { - unused(gps_setting); - unused(settings_json); + gps_setting.eph_time_constant = settings_json.getFloat("EphTimeConstant", gps_setting.eph_time_constant); + gps_setting.epv_time_constant = settings_json.getFloat("EpvTimeConstant", gps_setting.epv_time_constant); + gps_setting.eph_initial = settings_json.getFloat("EphInitial", gps_setting.eph_initial); + gps_setting.epv_initial = settings_json.getFloat("EpvInitial", gps_setting.epv_initial); + gps_setting.eph_final = settings_json.getFloat("EphFinal", gps_setting.eph_final); + gps_setting.epv_final = settings_json.getFloat("EpvFinal", gps_setting.epv_final); + gps_setting.eph_min_3d = settings_json.getFloat("EphMin3d", gps_setting.eph_min_3d); + gps_setting.eph_min_2d = settings_json.getFloat("EphMin2d", gps_setting.eph_min_2d); + gps_setting.update_latency = settings_json.getFloat("UpdateLatency", gps_setting.update_latency); + gps_setting.update_frequency = settings_json.getFloat("UpdateFrequency", gps_setting.update_frequency); + gps_setting.startup_delay = settings_json.getFloat("StartupDelay", gps_setting.startup_delay); + gps_setting.position = createVectorSetting(settings_json, gps_setting.position); + gps_setting.rotation = createRotationSetting(settings_json, gps_setting.rotation); - //TODO: set from json as needed } static void initializeMagnetometerSetting(MagnetometerSetting& magnetometer_setting, const Settings& settings_json) { - unused(magnetometer_setting); - unused(settings_json); - - //TODO: set from json as needed + magnetometer_setting.noise_sigma = createNoiseSigmaSetting(settings_json, magnetometer_setting.noise_sigma); + magnetometer_setting.scale_factor = settings_json.getFloat("ScaleFactor", magnetometer_setting.scale_factor); + magnetometer_setting.dynamic_reference_source = settings_json.getBool("DynamicReferenceSource", magnetometer_setting.dynamic_reference_source); + magnetometer_setting.update_frequency = settings_json.getFloat("UpdateFrequency", magnetometer_setting.update_frequency); + magnetometer_setting.ref_source = settings_json.getInt("ReferenceSource", magnetometer_setting.ref_source); + magnetometer_setting.update_latency = settings_json.getFloat("UpdateLatency", magnetometer_setting.update_latency); + magnetometer_setting.update_frequency = settings_json.getFloat("UpdateFrequency", magnetometer_setting.update_frequency); + magnetometer_setting.startup_delay = settings_json.getFloat("StartupDelay", magnetometer_setting.startup_delay); + magnetometer_setting.position = createVectorSetting(settings_json, magnetometer_setting.position); + magnetometer_setting.rotation = createRotationSetting(settings_json, magnetometer_setting.rotation); } static void initializeDistanceSetting(DistanceSetting& distance_setting, const Settings& settings_json) { - unused(distance_setting); - unused(settings_json); - - //TODO: set from json as needed + distance_setting.min_distance = settings_json.getFloat("MinDistance", distance_setting.min_distance); + distance_setting.max_distance = settings_json.getFloat("MaxDistance", distance_setting.max_distance); + distance_setting.unnorrelated_noise_sigma = settings_json.getFloat("Noise", distance_setting.unnorrelated_noise_sigma); + distance_setting.update_latency = settings_json.getFloat("UpdateLatency", distance_setting.update_latency); + distance_setting.update_frequency = settings_json.getFloat("UpdateFrequency", distance_setting.update_frequency); + distance_setting.startup_delay = settings_json.getFloat("StartupDelay", distance_setting.startup_delay); + distance_setting.position = createVectorSetting(settings_json, distance_setting.position); + distance_setting.rotation = createRotationSetting(settings_json, distance_setting.rotation); } static void initializeLidarSetting(LidarSetting& lidar_setting, const Settings& settings_json) diff --git a/AirLib/include/sensors/barometer/BarometerSimpleParams.hpp b/AirLib/include/sensors/barometer/BarometerSimpleParams.hpp index 62082342dd..84e219f387 100644 --- a/AirLib/include/sensors/barometer/BarometerSimpleParams.hpp +++ b/AirLib/include/sensors/barometer/BarometerSimpleParams.hpp @@ -47,9 +47,34 @@ struct BarometerSimpleParams { real_T update_frequency = 50; //Hz real_T startup_delay = 0; //sec + Pose relative_pose; + void initializeFromSettings(const AirSimSettings::BarometerSetting& settings) { - unused(settings); + qnh = settings.qnh; + pressure_factor_sigma = settings.pressure_factor_sigma; + pressure_factor_tau = settings.pressure_factor_tau; + unnorrelated_noise_sigma = settings.unnorrelated_noise_sigma; + update_latency = settings.update_latency; + update_frequency = settings.update_frequency; + startup_delay = settings.startup_delay; + + relative_pose.position = settings.position; + if (std::isnan(relative_pose.position.x())) + relative_pose.position.x() = 0; + if (std::isnan(relative_pose.position.y())) + relative_pose.position.y() = 0; + if (std::isnan(relative_pose.position.z())) + relative_pose.position.z() = 0; + + float pitch, roll, yaw; + pitch = !std::isnan(settings.rotation.pitch) ? settings.rotation.pitch : 0; + roll = !std::isnan(settings.rotation.roll) ? settings.rotation.roll : 0; + yaw = !std::isnan(settings.rotation.yaw) ? settings.rotation.yaw : 0; + relative_pose.orientation = VectorMath::toQuaternion( + Utils::degreesToRadians(pitch), //pitch - rotation around Y axis + Utils::degreesToRadians(roll), //roll - rotation around X axis + Utils::degreesToRadians(yaw)); //yaw - rotation around Z axis } }; diff --git a/AirLib/include/sensors/distance/DistanceSimpleParams.hpp b/AirLib/include/sensors/distance/DistanceSimpleParams.hpp index a028365f18..f8babedf00 100644 --- a/AirLib/include/sensors/distance/DistanceSimpleParams.hpp +++ b/AirLib/include/sensors/distance/DistanceSimpleParams.hpp @@ -40,7 +40,29 @@ struct DistanceSimpleParams { void initializeFromSettings(const AirSimSettings::DistanceSetting& settings) { - unused(settings); + min_distance = settings.min_distance; + max_distance = settings.max_distance; + unnorrelated_noise_sigma = settings.unnorrelated_noise_sigma; + update_latency = settings.update_latency; + update_frequency = settings.update_frequency; + startup_delay = settings.startup_delay; + + relative_pose.position = settings.position; + if (std::isnan(relative_pose.position.x())) + relative_pose.position.x() = 0; + if (std::isnan(relative_pose.position.y())) + relative_pose.position.y() = 0; + if (std::isnan(relative_pose.position.z())) + relative_pose.position.z() = 0; + + float pitch, roll, yaw; + pitch = !std::isnan(settings.rotation.pitch) ? settings.rotation.pitch : 0; + roll = !std::isnan(settings.rotation.roll) ? settings.rotation.roll : 0; + yaw = !std::isnan(settings.rotation.yaw) ? settings.rotation.yaw : 0; + relative_pose.orientation = VectorMath::toQuaternion( + Utils::degreesToRadians(pitch), //pitch - rotation around Y axis + Utils::degreesToRadians(roll), //roll - rotation around X axis + Utils::degreesToRadians(yaw)); //yaw - rotation around Z axis } }; diff --git a/AirLib/include/sensors/gps/GpsSimpleParams.hpp b/AirLib/include/sensors/gps/GpsSimpleParams.hpp index 52d8cf88c8..078622bee9 100644 --- a/AirLib/include/sensors/gps/GpsSimpleParams.hpp +++ b/AirLib/include/sensors/gps/GpsSimpleParams.hpp @@ -5,7 +5,7 @@ #define msr_airlib_GpsSimpleParams_hpp #include "common/Common.hpp" - +#include "common/AirSimSettings.hpp" namespace msr { namespace airlib { @@ -19,9 +19,39 @@ struct GpsSimpleParams { real_T update_frequency = 50; //Hz real_T startup_delay = 1; //sec + Pose relative_pose; + void initializeFromSettings(const AirSimSettings::GpsSetting& settings) { - unused(settings); + eph_time_constant = settings.eph_time_constant; + epv_time_constant = settings.epv_time_constant; + eph_initial = settings.eph_initial; + epv_initial = settings.epv_initial; + eph_final = settings.eph_final; + epv_final = settings.epv_final; + eph_min_3d = settings.eph_min_3d; + eph_min_2d = settings.eph_min_2d; + + update_latency = settings.update_latency; + update_frequency = settings.update_frequency; + startup_delay = settings.startup_delay; + + relative_pose.position = settings.position; + if (std::isnan(relative_pose.position.x())) + relative_pose.position.x() = 0; + if (std::isnan(relative_pose.position.y())) + relative_pose.position.y() = 0; + if (std::isnan(relative_pose.position.z())) + relative_pose.position.z() = 0; + + float pitch, roll, yaw; + pitch = !std::isnan(settings.rotation.pitch) ? settings.rotation.pitch : 0; + roll = !std::isnan(settings.rotation.roll) ? settings.rotation.roll : 0; + yaw = !std::isnan(settings.rotation.yaw) ? settings.rotation.yaw : 0; + relative_pose.orientation = VectorMath::toQuaternion( + Utils::degreesToRadians(pitch), //pitch - rotation around Y axis + Utils::degreesToRadians(roll), //roll - rotation around X axis + Utils::degreesToRadians(yaw)); //yaw - rotation around Z axis } }; diff --git a/AirLib/include/sensors/imu/ImuSimpleParams.hpp b/AirLib/include/sensors/imu/ImuSimpleParams.hpp index 6cbe166a4e..74fdc2bfa4 100644 --- a/AirLib/include/sensors/imu/ImuSimpleParams.hpp +++ b/AirLib/include/sensors/imu/ImuSimpleParams.hpp @@ -24,6 +24,8 @@ struct ImuSimpleParams { https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Datasheet1.pdf For Allan Variance/Deviation plots see http://www.invensense.com/wp-content/uploads/2015/02/MPU-3300-Datasheet.pdf */ + Pose relative_pose; + struct Gyroscope { //angule random walk (ARW) real_T arw = 0.30f / sqrt(3600.0f) * M_PIf / 180; //deg/sqrt(hour) converted to rad/sqrt(sec) @@ -43,10 +45,35 @@ struct ImuSimpleParams { } accel; real_T min_sample_time = 1 / 1000.0f; //internal IMU frequency - + void initializeFromSettings(const AirSimSettings::ImuSetting& settings) { - unused(settings); + gyro.arw = settings.gyro_arw; + gyro.tau = settings.gyro_tau; + gyro.bias_stability = settings.gyro_bias_stability; + gyro.turn_on_bias = settings.gyro_turn_on_bias; + + accel.vrw = settings.accel_vrw; + accel.tau = settings.accel_tau; + accel.bias_stability = settings.accel_bias_stability; + accel.turn_on_bias = settings.accel_turn_on_bias; + + relative_pose.position = settings.position; + if (std::isnan(relative_pose.position.x())) + relative_pose.position.x() = 0; + if (std::isnan(relative_pose.position.y())) + relative_pose.position.y() = 0; + if (std::isnan(relative_pose.position.z())) + relative_pose.position.z() = 0; + + float pitch, roll, yaw; + pitch = !std::isnan(settings.rotation.pitch) ? settings.rotation.pitch : 0; + roll = !std::isnan(settings.rotation.roll) ? settings.rotation.roll : 0; + yaw = !std::isnan(settings.rotation.yaw) ? settings.rotation.yaw : 0; + relative_pose.orientation = VectorMath::toQuaternion( + Utils::degreesToRadians(pitch), //pitch - rotation around Y axis + Utils::degreesToRadians(roll), //roll - rotation around X axis + Utils::degreesToRadians(yaw)); //yaw - rotation around Z axis } }; diff --git a/AirLib/include/sensors/magnetometer/MagnetometerSimpleParams.hpp b/AirLib/include/sensors/magnetometer/MagnetometerSimpleParams.hpp index 652cb3257c..ee1f1b212f 100644 --- a/AirLib/include/sensors/magnetometer/MagnetometerSimpleParams.hpp +++ b/AirLib/include/sensors/magnetometer/MagnetometerSimpleParams.hpp @@ -5,6 +5,7 @@ #define msr_airlib_MagnetometerSimpleParams_hpp #include "common/Common.hpp" +#include "common/AirSimSettings.hpp" namespace msr { namespace airlib { @@ -32,9 +33,38 @@ struct MagnetometerSimpleParams { real_T update_frequency = 50; //Hz real_T startup_delay = 0; //sec + Pose relative_pose; + void initializeFromSettings(const AirSimSettings::MagnetometerSetting& settings) { - unused(settings); + noise_sigma = settings.noise_sigma; + noise_bias = settings.noise_bias; + scale_factor = settings.scale_factor; + dynamic_reference_source = settings.dynamic_reference_source; + ref_update_frequency = settings.update_frequency; + if (settings.ref_source == 0){ + ref_source = ReferenceSource::ReferenceSource_Constant; + } + update_latency = settings.update_latency; + update_frequency = settings.update_frequency; + startup_delay = settings.startup_delay; + + relative_pose.position = settings.position; + if (std::isnan(relative_pose.position.x())) + relative_pose.position.x() = 0; + if (std::isnan(relative_pose.position.y())) + relative_pose.position.y() = 0; + if (std::isnan(relative_pose.position.z())) + relative_pose.position.z() = 0; + + float pitch, roll, yaw; + pitch = !std::isnan(settings.rotation.pitch) ? settings.rotation.pitch : 0; + roll = !std::isnan(settings.rotation.roll) ? settings.rotation.roll : 0; + yaw = !std::isnan(settings.rotation.yaw) ? settings.rotation.yaw : 0; + relative_pose.orientation = VectorMath::toQuaternion( + Utils::degreesToRadians(pitch), //pitch - rotation around Y axis + Utils::degreesToRadians(roll), //roll - rotation around X axis + Utils::degreesToRadians(yaw)); //yaw - rotation around Z axis } }; diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp index 6bd1d4c191..ea6fa074e4 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp @@ -683,9 +683,9 @@ void MultirotorApiBase::moveToPathPosition(const Vector3r& dest, float velocity, } //send commands - //try to maintain altitude if path was in XY plan only, velocity based control is not as good + //try to maintain altitude if path was in XY plane only, velocity based control is not as good if (std::abs(cur.z() - dest.z()) <= getDistanceAccuracy()) //for paths in XY plan current code leaves z untouched, so we can compare with strict equality - moveByVelocityZInternal(velocity_vect.x(), velocity_vect.y(), dest.z(), yaw_mode); + moveByVelocityInternal(velocity_vect.x(), velocity_vect.y(), 0, yaw_mode); else moveByVelocityInternal(velocity_vect.x(), velocity_vect.y(), velocity_vect.z(), yaw_mode); } diff --git a/PythonClient/airsim/types.py b/PythonClient/airsim/types.py index 25e359843c..297fb4645c 100644 --- a/PythonClient/airsim/types.py +++ b/PythonClient/airsim/types.py @@ -400,9 +400,9 @@ class GpsData(MsgpackMixin): class DistanceSensorData(MsgpackMixin): time_stamp = np.uint64(0) - distance = Quaternionr() - min_distance = Quaternionr() - max_distance = Quaternionr() + distance = np.float32(0) + min_distance = np.float32(0) + max_distance = np.float32(0) relative_pose = Pose() class PIDGains(): diff --git a/PythonClient/multirotor/path.py b/PythonClient/multirotor/path.py index d36fc6a900..960581afee 100644 --- a/PythonClient/multirotor/path.py +++ b/PythonClient/multirotor/path.py @@ -30,9 +30,9 @@ # AirSim uses NED coordinates so negative axis is up. # z of -7 is 7 meters above the original launch point. -z = -7 +z = -70 print("make sure we are hovering at 7 meters...") -client.moveToZAsync(z, 1).join() +client.moveToZAsync(z, 5).join() # see https://github.com/Microsoft/AirSim/wiki/moveOnPath-demo diff --git a/Unreal/Plugins/AirSim/AirSim.uplugin b/Unreal/Plugins/AirSim/AirSim.uplugin index bbbf5cc8ba..a467e41876 100644 --- a/Unreal/Plugins/AirSim/AirSim.uplugin +++ b/Unreal/Plugins/AirSim/AirSim.uplugin @@ -1,7 +1,7 @@ { "FileVersion" : 3, - "Version" : "1.3.1", - "VersionName": "1.3.1", + "Version" : "1.2.0", + "VersionName": "1.2.0", "FriendlyName": "AirSim", "Description": "AirSim - Autonomous Aerial Vehicles Simulator Plugin", "Category" : "Science", diff --git a/auto/airsim/__init__.py b/auto/airsim/__init__.py new file mode 100644 index 0000000000..4e89c22980 --- /dev/null +++ b/auto/airsim/__init__.py @@ -0,0 +1,4 @@ +from .client import * +from .utils import * +from .types import * + diff --git a/auto/airsim/__init__.pyc b/auto/airsim/__init__.pyc new file mode 100644 index 0000000000..60005acc5d Binary files /dev/null and b/auto/airsim/__init__.pyc differ diff --git a/auto/airsim/__pycache__/__init__.cpython-36.pyc b/auto/airsim/__pycache__/__init__.cpython-36.pyc new file mode 100644 index 0000000000..54adf0289a Binary files /dev/null and b/auto/airsim/__pycache__/__init__.cpython-36.pyc differ diff --git a/auto/airsim/__pycache__/client.cpython-36.pyc b/auto/airsim/__pycache__/client.cpython-36.pyc new file mode 100644 index 0000000000..3b803e597f Binary files /dev/null and b/auto/airsim/__pycache__/client.cpython-36.pyc differ diff --git a/auto/airsim/__pycache__/utils.cpython-36.pyc b/auto/airsim/__pycache__/utils.cpython-36.pyc new file mode 100644 index 0000000000..99e91c1d54 Binary files /dev/null and b/auto/airsim/__pycache__/utils.cpython-36.pyc differ diff --git a/auto/airsim/client.py b/auto/airsim/client.py new file mode 100644 index 0000000000..b8dde7f0f5 --- /dev/null +++ b/auto/airsim/client.py @@ -0,0 +1,1127 @@ +from __future__ import print_function + +from .utils import * +from .types import * + +import msgpackrpc #install as admin: pip install msgpack-rpc-python +import numpy as np #pip install numpy +import msgpack +import time +import math +import logging + +class VehicleClient: + def __init__(self, ip = "", port = 41451, timeout_value = 3600): + if (ip == ""): + ip = "127.0.0.1" + self.client = msgpackrpc.Client(msgpackrpc.Address(ip, port), timeout = timeout_value, pack_encoding = 'utf-8', unpack_encoding = 'utf-8') + + # ----------------------------------- Common vehicle APIs --------------------------------------------- + def reset(self): + """ + Reset the vehicle to its original starting state + + Note that you must call `enableApiControl` and `armDisarm` again after the call to reset + """ + self.client.call('reset') + + def ping(self): + """ + If connection is established then this call will return true otherwise it will be blocked until timeout + + Returns: + bool: + """ + return self.client.call('ping') + + def getClientVersion(self): + return 1 # sync with C++ client + + def getServerVersion(self): + return self.client.call('getServerVersion') + + def getMinRequiredServerVersion(self): + return 1 # sync with C++ client + + def getMinRequiredClientVersion(self): + return self.client.call('getMinRequiredClientVersion') + + # basic flight control + def enableApiControl(self, is_enabled, vehicle_name = ''): + """ + Enables or disables API control for vehicle corresponding to vehicle_name + + Args: + is_enabled (bool): True to enable, False to disable API control + vehicle_name (str, optional): Name of the vehicle to send this command to + """ + self.client.call('enableApiControl', is_enabled, vehicle_name) + + def isApiControlEnabled(self, vehicle_name = ''): + """ + Returns true if API control is established. + + If false (which is default) then API calls would be ignored. After a successful call to `enableApiControl`, `isApiControlEnabled` should return true. + + Args: + vehicle_name (str, optional): Name of the vehicle + + Returns: + bool: If API control is enabled + """ + return self.client.call('isApiControlEnabled', vehicle_name) + + def armDisarm(self, arm, vehicle_name = ''): + """ + Arms or disarms vehicle + + Args: + arm (bool): True to arm, False to disarm the vehicle + vehicle_name (str, optional): Name of the vehicle to send this command to + + Returns: + bool: Success + """ + return self.client.call('armDisarm', arm, vehicle_name) + + def simPause(self, is_paused): + """ + Pauses simulation + + Args: + is_paused (bool): True to pause the simulation, False to release + """ + self.client.call('simPause', is_paused) + + def simIsPause(self): + """ + Returns true if the simulation is paused + + Returns: + bool: If the simulation is paused + """ + return self.client.call("simIsPaused") + + def simContinueForTime(self, seconds): + """ + Continue the simulation for the specified number of seconds + + Args: + seconds (float): Time to run the simulation for + """ + self.client.call('simContinueForTime', seconds) + + def getHomeGeoPoint(self, vehicle_name = ''): + """ + Get the Home location of the vehicle + + Args: + vehicle_name (str, optional): Name of vehicle to get home location of + + Returns: + GeoPoint: Home location of the vehicle + """ + return GeoPoint.from_msgpack(self.client.call('getHomeGeoPoint', vehicle_name)) + + def confirmConnection(self): + """ + Checks state of connection every 1 sec and reports it in Console so user can see the progress for connection. + """ + if self.ping(): + print("Connected!") + else: + print("Ping returned false!") + server_ver = self.getServerVersion() + client_ver = self.getClientVersion() + server_min_ver = self.getMinRequiredServerVersion() + client_min_ver = self.getMinRequiredClientVersion() + + ver_info = "Client Ver:" + str(client_ver) + " (Min Req: " + str(client_min_ver) + \ + "), Server Ver:" + str(server_ver) + " (Min Req: " + str(server_min_ver) + ")" + + if server_ver < server_min_ver: + print(ver_info, file=sys.stderr) + print("AirSim server is of older version and not supported by this client. Please upgrade!") + elif client_ver < client_min_ver: + print(ver_info, file=sys.stderr) + print("AirSim client is of older version and not supported by this server. Please upgrade!") + else: + print(ver_info) + print('') + + def simSwapTextures(self, tags, tex_id = 0, component_id = 0, material_id = 0): + """ + Runtime Swap Texture API + + See https://microsoft.github.io/AirSim/retexturing/ for details + + Args: + tags (str): string of "," or ", " delimited tags to identify on which actors to perform the swap + tex_id (int, optional): indexes the array of textures assigned to each actor undergoing a swap + + If out-of-bounds for some object's texture set, it will be taken modulo the number of textures that were available + component_id (int, optional): + material_id (int, optional): + + Returns: + list[str]: List of objects which matched the provided tags and had the texture swap perfomed + """ + return self.client.call("simSwapTextures", tags, tex_id, component_id, material_id) + + # time-of-day control + def simSetTimeOfDay(self, is_enabled, start_datetime = "", is_start_datetime_dst = False, celestial_clock_speed = 1, update_interval_secs = 60, move_sun = True): + """ + Control the position of Sun in the environment + + Sun's position is computed using the coordinates specified in `OriginGeopoint` in settings for the date-time specified in the argument, + else if the string is empty, current date & time is used + + Args: + is_enabled (bool): True to enable time-of-day effect, False to reset the position to original + start_datetime (str, optional): Date & Time in %Y-%m-%d %H:%M:%S format, e.g. `2018-02-12 15:20:00` + is_start_datetime_dst (bool, optional): True to adjust for Daylight Savings Time + celestial_clock_speed (float, optional): Run celestial clock faster or slower than simulation clock + E.g. Value 100 means for every 1 second of simulation clock, Sun's position is advanced by 100 seconds + so Sun will move in sky much faster + update_interval_secs (float, optional): Interval to update the Sun's position + move_sun (bool, optional): Whether or not to move the Sun + """ + self.client.call('simSetTimeOfDay', is_enabled, start_datetime, is_start_datetime_dst, celestial_clock_speed, update_interval_secs, move_sun) + + # weather + def simEnableWeather(self, enable): + """ + Enable Weather effects. Needs to be called before using `simSetWeatherParameter` API + + Args: + enable (bool): True to enable, False to disable + """ + self.client.call('simEnableWeather', enable) + + def simSetWeatherParameter(self, param, val): + """ + Enable various weather effects + + Args: + param (WeatherParameter): Weather effect to be enabled + val (float): Intensity of the effect, Range 0-1 + """ + self.client.call('simSetWeatherParameter', param, val) + + # camera control + # simGetImage returns compressed png in array of bytes + # image_type uses one of the ImageType members + def simGetImage(self, camera_name, image_type, vehicle_name = ''): + """ + Get a single image + + Returns bytes of png format image which can be dumped into abinary file to create .png image + `string_to_uint8_array()` can be used to convert into Numpy unit8 array + See https://microsoft.github.io/AirSim/image_apis/ for details + + Args: + camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. can also be used + image_type (ImageType): Type of image required + vehicle_name (str, optional): Name of the vehicle with the camera + + Returns: + Binary string literal of compressed png image + """ + # todo: in future remove below, it's only for compatibility to pre v1.2 + camera_name = str(camera_name) + + # because this method returns std::vector, msgpack decides to encode it as a string unfortunately. + result = self.client.call('simGetImage', camera_name, image_type, vehicle_name) + if (result == "" or result == "\0"): + return None + return result + + # camera control + # simGetImage returns compressed png in array of bytes + # image_type uses one of the ImageType members + def simGetImages(self, requests, vehicle_name = ''): + """ + Get multiple images + + See https://microsoft.github.io/AirSim/image_apis/ for details and examples + + Args: + requests (list[ImageRequest]): Images required + vehicle_name (str, optional): Name of vehicle associated with the camera + + Returns: + list[ImageResponse]: + """ + responses_raw = self.client.call('simGetImages', requests, vehicle_name) + return [ImageResponse.from_msgpack(response_raw) for response_raw in responses_raw] + + # gets the static meshes in the unreal scene + def simGetMeshPositionVertexBuffers(self): + """ + Returns the static meshes that make up the scene + + See https://microsoft.github.io/AirSim/meshes/ for details and how to use this + + Returns: + list[MeshPositionVertexBuffersResponse]: + """ + responses_raw = self.client.call('simGetMeshPositionVertexBuffers') + return [MeshPositionVertexBuffersResponse.from_msgpack(response_raw) for response_raw in responses_raw] + + def simGetCollisionInfo(self, vehicle_name = ''): + """ + Args: + vehicle_name (str, optional): Name of the Vehicle to get the info of + + Returns: + CollisionInfo: + """ + return CollisionInfo.from_msgpack(self.client.call('simGetCollisionInfo', vehicle_name)) + + def simSetVehiclePose(self, pose, ignore_collison, vehicle_name = ''): + """ + Set the pose of the vehicle + + If you don't want to change position (or orientation) then just set components of position (or orientation) to floating point nan values + + Args: + pose (Pose): Desired Pose pf the vehicle + ignore_collision (bool): Whether to ignore any collision or not + vehicle_name (str, optional): Name of the vehicle to move + """ + self.client.call('simSetVehiclePose', pose, ignore_collison, vehicle_name) + + def simGetVehiclePose(self, vehicle_name = ''): + """ + Args: + vehicle_name (str, optional): Name of the vehicle to get the Pose of + + Returns: + Pose: + """ + pose = self.client.call('simGetVehiclePose', vehicle_name) + return Pose.from_msgpack(pose) + + def simSetTraceLine(self, color_rgba, thickness=1.0, vehicle_name = ''): + """ + Modify the color and thickness of the line when Tracing is enabled + + Tracing can be enabled by pressing T in the Editor or setting `EnableTrace` to `True` in the Vehicle Settings + + Args: + color_rgba (list): desired RGBA values from 0.0 to 1.0 + thickness (float, optional): Thickness of the line + vehicle_name (string, optional): Name of the vehicle to set Trace line values for + """ + self.client.call('simSetTraceLine', color_rgba, thickness, vehicle_name) + + def simGetObjectPose(self, object_name): + """ + Args: + object_name (str): Object to get the Pose of + + Returns: + Pose: + """ + pose = self.client.call('simGetObjectPose', object_name) + return Pose.from_msgpack(pose) + + def simSetObjectPose(self, object_name, pose, teleport = True): + """ + Set the pose of the object(actor) in the environment + + The specified actor must have Mobility set to movable, otherwise there will be undefined behaviour. + See https://www.unrealengine.com/en-US/blog/moving-physical-objects for details on how to set Mobility and the effect of Teleport parameter + + Args: + object_name (str): Name of the object(actor) to move + pose (Pose): Desired Pose of the object + teleport (bool, optional): Whether to move the object immediately without affecting their velocity + + Returns: + bool: If the move was successful + """ + return self.client.call('simSetObjectPose', object_name, pose, teleport) + + def simListSceneObjects(self, name_regex = '.*'): + """ + Lists the objects present in the environment + + Default behaviour is to list all objects, regex can be used to return smaller list of matching objects or actors + + Args: + name_regex (str, optional): String to match actor names against, e.g. "Cylinder.*" + + Returns: + list[str]: List containing all the names + """ + return self.client.call('simListSceneObjects', name_regex) + + def simSetSegmentationObjectID(self, mesh_name, object_id, is_name_regex = False): + """ + Set segmentation ID for specific objects + + See https://microsoft.github.io/AirSim/image_apis/#segmentation for details + + Args: + mesh_name (str): Name of the mesh to set the ID of (supports regex) + object_id (int): Object ID to be set, range 0-255 + + RBG values for IDs can be seen at https://microsoft.github.io/AirSim/seg_rgbs.txt + is_name_regex (bool, optional): Whether the mesh name is a regex + + Returns: + bool: If the mesh was found + """ + return self.client.call('simSetSegmentationObjectID', mesh_name, object_id, is_name_regex) + + def simGetSegmentationObjectID(self, mesh_name): + """ + Returns Object ID for the given mesh name + + Mapping of Object IDs to RGB values can be seen at https://microsoft.github.io/AirSim/seg_rgbs.txt + + Args: + mesh_name (str): Name of the mesh to get the ID of + """ + return self.client.call('simGetSegmentationObjectID', mesh_name) + + def simPrintLogMessage(self, message, message_param = "", severity = 0): + """ + Prints the specified message in the simulator's window. + + If message_param is supplied, then it's printed next to the message and in that case if this API is called with same message value + but different message_param again then previous line is overwritten with new line (instead of API creating new line on display). + + For example, `simPrintLogMessage("Iteration: ", to_string(i))` keeps updating same line on display when API is called with different values of i. + The valid values of severity parameter is 0 to 3 inclusive that corresponds to different colors. + + Args: + message (str): Message to be printed + message_param (str, optional): Parameter to be printed next to the message + severity (int, optional): Range 0-3, inclusive, corresponding to the severity of the message + """ + self.client.call('simPrintLogMessage', message, message_param, severity) + + def simGetCameraInfo(self, camera_name, vehicle_name = ''): + """ + Get details about the camera + + Args: + camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. can also be used + vehicle_name (str, optional): Vehicle which the camera is associated with + + Returns: + CameraInfo: + """ + # TODO: below str() conversion is only needed for legacy reason and should be removed in future + return CameraInfo.from_msgpack(self.client.call('simGetCameraInfo', str(camera_name), vehicle_name)) + + def simSetCameraOrientation(self, camera_name, orientation, vehicle_name = ''): + """ + - Control the orientation of a selected camera + + Args: + camera_name (str): Name of the camera to be controlled + orientation (Quaternionr): Quaternion representing the desired orientation of the camera + vehicle_name (str, optional): Name of vehicle which the camera corresponds to + """ + # TODO: below str() conversion is only needed for legacy reason and should be removed in future + self.client.call('simSetCameraOrientation', str(camera_name), orientation, vehicle_name) + + def simSetCameraFov(self, camera_name, fov_degrees, vehicle_name = ''): + """ + - Control the field of view of a selected camera + + Args: + camera_name (str): Name of the camera to be controlled + fov_degrees (float): Value of field of view in degrees + vehicle_name (str, optional): Name of vehicle which the camera corresponds to + """ + # TODO: below str() conversion is only needed for legacy reason and should be removed in future + self.client.call('simSetCameraFov', str(camera_name), fov_degrees, vehicle_name) + + def simGetGroundTruthKinematics(self, vehicle_name = ''): + """ + Get Ground truth kinematics of the vehicle + + Args: + vehicle_name (str, optional): Name of the vehicle + + Returns: + KinematicsState: Ground truth of the vehicle + """ + kinematics_state = self.client.call('simGetGroundTruthKinematics', vehicle_name) + return KinematicsState.from_msgpack(kinematics_state) + simGetGroundTruthKinematics.__annotations__ = {'return': KinematicsState} + + def simGetGroundTruthEnvironment(self, vehicle_name = ''): + """ + Get ground truth environment state + + Args: + vehicle_name (str, optional): Name of the vehicle + + Returns: + EnvironmentState: Ground truth environment state + """ + env_state = self.client.call('simGetGroundTruthEnvironment', vehicle_name) + return EnvironmentState.from_msgpack(env_state) + simGetGroundTruthEnvironment.__annotations__ = {'return': EnvironmentState} + + # sensor APIs + def getImuData(self, imu_name = '', vehicle_name = ''): + """ + Args: + imu_name (str, optional): Name of IMU to get data from, specified in settings.json + vehicle_name (str, optional): Name of vehicle to which the sensor corresponds to + + Returns: + ImuData: + """ + return ImuData.from_msgpack(self.client.call('getImuData', imu_name, vehicle_name)) + + def getBarometerData(self, barometer_name = '', vehicle_name = ''): + """ + Args: + barometer_name (str, optional): Name of Barometer to get data from, specified in settings.json + vehicle_name (str, optional): Name of vehicle to which the sensor corresponds to + + Returns: + BarometerData: + """ + return BarometerData.from_msgpack(self.client.call('getBarometerData', barometer_name, vehicle_name)) + + def getMagnetometerData(self, magnetometer_name = '', vehicle_name = ''): + """ + Args: + magnetometer_name (str, optional): Name of Magnetometer to get data from, specified in settings.json + vehicle_name (str, optional): Name of vehicle to which the sensor corresponds to + + Returns: + MagnetometerData: + """ + return MagnetometerData.from_msgpack(self.client.call('getMagnetometerData', magnetometer_name, vehicle_name)) + + def getGpsData(self, gps_name = '', vehicle_name = ''): + """ + Args: + gps_name (str, optional): Name of GPS to get data from, specified in settings.json + vehicle_name (str, optional): Name of vehicle to which the sensor corresponds to + + Returns: + GpsData: + """ + return GpsData.from_msgpack(self.client.call('getGpsData', gps_name, vehicle_name)) + + def getDistanceSensorData(self, distance_sensor_name = '', vehicle_name = ''): + """ + Args: + distance_sensor_name (str, optional): Name of Distance Sensor to get data from, specified in settings.json + vehicle_name (str, optional): Name of vehicle to which the sensor corresponds to + + Returns: + DistanceSensorData: + """ + return DistanceSensorData.from_msgpack(self.client.call('getDistanceSensorData', distance_sensor_name, vehicle_name)) + + def getLidarData(self, lidar_name = '', vehicle_name = ''): + """ + Args: + lidar_name (str, optional): Name of Lidar to get data from, specified in settings.json + vehicle_name (str, optional): Name of vehicle to which the sensor corresponds to + + Returns: + LidarData: + """ + return LidarData.from_msgpack(self.client.call('getLidarData', lidar_name, vehicle_name)) + + def simGetLidarSegmentation(self, lidar_name = '', vehicle_name = ''): + """ + Returns Segmentation ID of each point's collided object in the last Lidar update + + Args: + lidar_name (str, optional): Name of Lidar sensor + vehicle_name (str, optional): Name of the vehicle wth the sensor + + Returns: + list[int]: Segmentation IDs of the objects + """ + return self.client.call('simGetLidarSegmentation', lidar_name, vehicle_name) + + # Plotting APIs + def simFlushPersistentMarkers(self): + """ + Clear any persistent markers - those plotted with setting `is_persistent=True` in the APIs below + """ + self.client.call('simFlushPersistentMarkers') + + def simPlotPoints(self, points, color_rgba=[1.0, 0.0, 0.0, 1.0], size = 10.0, duration = -1.0, is_persistent = False): + """ + Plot a list of 3D points in World NED frame + + Args: + points (list[Vector3r]): List of Vector3r objects + color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 + size (float, optional): Size of plotted point + duration (float, optional): Duration (seconds) to plot for + is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time. + """ + self.client.call('simPlotPoints', points, color_rgba, size, duration, is_persistent) + + def simPlotLineStrip(self, points, color_rgba=[1.0, 0.0, 0.0, 1.0], thickness = 5.0, duration = -1.0, is_persistent = False): + """ + Plots a line strip in World NED frame, defined from points[0] to points[1], points[1] to points[2], ... , points[n-2] to points[n-1] + + Args: + points (list[Vector3r]): List of 3D locations of line start and end points, specified as Vector3r objects + color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 + thickness (float, optional): Thickness of line + duration (float, optional): Duration (seconds) to plot for + is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time. + """ + self.client.call('simPlotLineStrip', points, color_rgba, thickness, duration, is_persistent) + + def simPlotLineList(self, points, color_rgba=[1.0, 0.0, 0.0, 1.0], thickness = 5.0, duration = -1.0, is_persistent = False): + """ + Plots a line strip in World NED frame, defined from points[0] to points[1], points[2] to points[3], ... , points[n-2] to points[n-1] + + Args: + points (list[Vector3r]): List of 3D locations of line start and end points, specified as Vector3r objects. Must be even + color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 + thickness (float, optional): Thickness of line + duration (float, optional): Duration (seconds) to plot for + is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time. + """ + self.client.call('simPlotLineList', points, color_rgba, thickness, duration, is_persistent) + + def simPlotArrows(self, points_start, points_end, color_rgba=[1.0, 0.0, 0.0, 1.0], thickness = 5.0, arrow_size = 2.0, duration = -1.0, is_persistent = False): + """ + Plots a list of arrows in World NED frame, defined from points_start[0] to points_end[0], points_start[1] to points_end[1], ... , points_start[n-1] to points_end[n-1] + + Args: + points_start (list[Vector3r]): List of 3D start positions of arrow start positions, specified as Vector3r objects + points_end (list[Vector3r]): List of 3D end positions of arrow start positions, specified as Vector3r objects + color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 + thickness (float, optional): Thickness of line + arrow_size (float, optional): Size of arrow head + duration (float, optional): Duration (seconds) to plot for + is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time. + """ + self.client.call('simPlotArrows', points_start, points_end, color_rgba, thickness, arrow_size, duration, is_persistent) + + + def simPlotStrings(self, strings, positions, scale = 5, color_rgba=[1.0, 0.0, 0.0, 1.0], duration = -1.0): + """ + Plots a list of strings at desired positions in World NED frame. + + Args: + strings (list[String], optional): List of strings to plot + positions (list[Vector3r]): List of positions where the strings should be plotted. Should be in one-to-one correspondence with the strings' list + scale (float, optional): Font scale of transform name + color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 + duration (float, optional): Duration (seconds) to plot for + """ + self.client.call('simPlotStrings', strings, positions, scale, color_rgba, duration) + + def simPlotTransforms(self, poses, scale = 5.0, thickness = 5.0, duration = -1.0, is_persistent = False): + """ + Plots a list of transforms in World NED frame. + + Args: + poses (list[Pose]): List of Pose objects representing the transforms to plot + scale (float, optional): Length of transforms' axes + thickness (float, optional): Thickness of transforms' axes + duration (float, optional): Duration (seconds) to plot for + is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time. + """ + self.client.call('simPlotTransforms', poses, scale, thickness, duration, is_persistent) + + def simPlotTransformsWithNames(self, poses, names, tf_scale = 5.0, tf_thickness = 5.0, text_scale = 10.0, text_color_rgba = [1.0, 0.0, 0.0, 1.0], duration = -1.0): + """ + Plots a list of transforms with their names in World NED frame. + + Args: + poses (list[Pose]): List of Pose objects representing the transforms to plot + names (list[string]): List of strings with one-to-one correspondence to list of poses + tf_scale (float, optional): Length of transforms' axes + tf_thickness (float, optional): Thickness of transforms' axes + text_scale (float, optional): Font scale of transform name + text_color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 for the transform name + duration (float, optional): Duration (seconds) to plot for + """ + self.client.call('simPlotTransformsWithNames', poses, names, tf_scale, tf_thickness, text_scale, text_color_rgba, duration) + + def cancelLastTask(self, vehicle_name = ''): + """ + Cancel previous Async task + + Args: + vehicle_name (str, optional): Name of the vehicle + """ + self.client.call('cancelLastTask', vehicle_name) + + def waitOnLastTask(self, timeout_sec = float('nan')): + """ + Wait for the last Async task to complete + + Args: + timeout_sec (float, optional): Time for the task to complete + + Returns: + bool: Result of the last task + + True if the task completed without cancellation or timeout + """ + return self.client.call('waitOnLastTask', timeout_sec) + +# ----------------------------------- Multirotor APIs --------------------------------------------- +class MultirotorClient(VehicleClient, object): + def __init__(self, ip = "", port = 41451, timeout_value = 3600): + super(MultirotorClient, self).__init__(ip, port, timeout_value) + + def takeoffAsync(self, timeout_sec = 20, vehicle_name = ''): + """ + Takeoff vehicle to 3m above ground. Vehicle should not be moving when this API is used + + Args: + timeout_sec (int, optional): Timeout for the vehicle to reach desired altitude + vehicle_name (str, optional): Name of the vehicle to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('takeoff', timeout_sec, vehicle_name) + + def landAsync(self, timeout_sec = 60, vehicle_name = ''): + """ + Land the vehicle + + Args: + timeout_sec (int, optional): Timeout for the vehicle to land + vehicle_name (str, optional): Name of the vehicle to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('land', timeout_sec, vehicle_name) + + def goHomeAsync(self, timeout_sec = 3e+38, vehicle_name = ''): + """ + Return vehicle to Home i.e. Launch location + + Args: + timeout_sec (int, optional): Timeout for the vehicle to reach desired altitude + vehicle_name (str, optional): Name of the vehicle to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('goHome', timeout_sec, vehicle_name) + + # APIs for control + def moveByAngleZAsync(self, pitch, roll, z, yaw, duration, vehicle_name = ''): + return self.client.call_async('moveByAngleZ', pitch, roll, z, yaw, duration, vehicle_name) + + def moveByAngleThrottleAsync(self, pitch, roll, throttle, yaw_rate, duration, vehicle_name = ''): + return self.client.call_async('moveByAngleThrottle', pitch, roll, throttle, yaw_rate, duration, vehicle_name) + + def moveByVelocityAsync(self, vx, vy, vz, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), vehicle_name = ''): + """ + Args: + vx (float): desired velocity in world (NED) X axis + vy (float): desired velocity in world (NED) Y axis + vz (float): desired velocity in world (NED) Z axis + duration (float): Desired amount of time (seconds), to send this command for + drivetrain (DrivetrainType, optional): + yaw_mode (YawMode, optional): + vehicle_name (str, optional): Name of the multirotor to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('moveByVelocity', vx, vy, vz, duration, drivetrain, yaw_mode, vehicle_name) + + def moveByVelocityZAsync(self, vx, vy, z, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), vehicle_name = ''): + return self.client.call_async('moveByVelocityZ', vx, vy, z, duration, drivetrain, yaw_mode, vehicle_name) + + def moveOnPathAsync(self, path, velocity, timeout_sec = 3e+38, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), + lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''): + return self.client.call_async('moveOnPath', path, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead, vehicle_name) + + def moveToPositionAsync(self, x, y, z, velocity, timeout_sec = 3e+38, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), + lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''): + return self.client.call_async('moveToPosition', x, y, z, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead, vehicle_name) + + def moveToZAsync(self, z, velocity, timeout_sec = 3e+38, yaw_mode = YawMode(), lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''): + return self.client.call_async('moveToZ', z, velocity, timeout_sec, yaw_mode, lookahead, adaptive_lookahead, vehicle_name) + + def moveByManualAsync(self, vx_max, vy_max, z_min, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), vehicle_name = ''): + """ + - Read current RC state and use it to control the vehicles. + + Parameters sets up the constraints on velocity and minimum altitude while flying. If RC state is detected to violate these constraints + then that RC state would be ignored. + + Args: + vx_max (float): max velocity allowed in x direction + vy_max (float): max velocity allowed in y direction + vz_max (float): max velocity allowed in z direction + z_min (float): min z allowed for vehicle position + duration (float): after this duration vehicle would switch back to non-manual mode + drivetrain (DrivetrainType): when ForwardOnly, vehicle rotates itself so that its front is always facing the direction of travel. If MaxDegreeOfFreedom then it doesn't do that (crab-like movement) + yaw_mode (YawMode): Specifies if vehicle should face at given angle (is_rate=False) or should be rotating around its axis at given rate (is_rate=True) + vehicle_name (str, optional): Name of the multirotor to send this command to + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('moveByManual', vx_max, vy_max, z_min, duration, drivetrain, yaw_mode, vehicle_name) + + def rotateToYawAsync(self, yaw, timeout_sec = 3e+38, margin = 5, vehicle_name = ''): + return self.client.call_async('rotateToYaw', yaw, timeout_sec, margin, vehicle_name) + + def rotateByYawRateAsync(self, yaw_rate, duration, vehicle_name = ''): + return self.client.call_async('rotateByYawRate', yaw_rate, duration, vehicle_name) + + def hoverAsync(self, vehicle_name = ''): + return self.client.call_async('hover', vehicle_name) + + def moveByRC(self, rcdata = RCData(), vehicle_name = ''): + return self.client.call('moveByRC', rcdata, vehicle_name) + + # low-level control API + def moveByMotorPWMsAsync(self, front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm, duration, vehicle_name = ''): + """ + - Directly control the motors using PWM values + + Args: + front_right_pwm (float): PWM value for the front right motor (between 0.0 to 1.0) + rear_left_pwm (float): PWM value for the rear left motor (between 0.0 to 1.0) + front_left_pwm (float): PWM value for the front left motor (between 0.0 to 1.0) + rear_right_pwm (float): PWM value for the rear right motor (between 0.0 to 1.0) + duration (float): Desired amount of time (seconds), to send this command for + vehicle_name (str, optional): Name of the multirotor to send this command to + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('moveByMotorPWMs', front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm, duration, vehicle_name) + + def moveByRollPitchYawZAsync(self, roll, pitch, yaw, z, duration, vehicle_name = ''): + """ + - z is given in local NED frame of the vehicle. + - Roll angle, pitch angle, and yaw angle set points are given in **radians**, in the body frame. + - The body frame follows the Front Left Up (FLU) convention, and right-handedness. + + - Frame Convention: + - X axis is along the **Front** direction of the quadrotor. + + | Clockwise rotation about this axis defines a positive **roll** angle. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + + - Y axis is along the **Left** direction of the quadrotor. + + | Clockwise rotation about this axis defines a positive **pitch** angle. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + + - Z axis is along the **Up** direction. + + | Clockwise rotation about this axis defines a positive **yaw** angle. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + + Args: + roll (float): Desired roll angle, in radians. + pitch (float): Desired pitch angle, in radians. + yaw (float): Desired yaw angle, in radians. + z (float): Desired Z value (in local NED frame of the vehicle) + duration (float): Desired amount of time (seconds), to send this command for + vehicle_name (str, optional): Name of the multirotor to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('moveByRollPitchYawZ', roll, -pitch, -yaw, z, duration, vehicle_name) + + def moveByRollPitchYawThrottleAsync(self, roll, pitch, yaw, throttle, duration, vehicle_name = ''): + """ + - Desired throttle is between 0.0 to 1.0 + - Roll angle, pitch angle, and yaw angle are given in **radians**, in the body frame. + - The body frame follows the Front Left Up (FLU) convention, and right-handedness. + + - Frame Convention: + - X axis is along the **Front** direction of the quadrotor. + + | Clockwise rotation about this axis defines a positive **roll** angle. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + + - Y axis is along the **Left** direction of the quadrotor. + + | Clockwise rotation about this axis defines a positive **pitch** angle. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + + - Z axis is along the **Up** direction. + + | Clockwise rotation about this axis defines a positive **yaw** angle. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + + Args: + roll (float): Desired roll angle, in radians. + pitch (float): Desired pitch angle, in radians. + yaw (float): Desired yaw angle, in radians. + throttle (float): Desired throttle (between 0.0 to 1.0) + duration (float): Desired amount of time (seconds), to send this command for + vehicle_name (str, optional): Name of the multirotor to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('moveByRollPitchYawThrottle', roll, -pitch, -yaw, throttle, duration, vehicle_name) + + def moveByRollPitchYawrateThrottleAsync(self, roll, pitch, yaw_rate, throttle, duration, vehicle_name = ''): + """ + - Desired throttle is between 0.0 to 1.0 + - Roll angle, pitch angle, and yaw rate set points are given in **radians**, in the body frame. + - The body frame follows the Front Left Up (FLU) convention, and right-handedness. + + - Frame Convention: + - X axis is along the **Front** direction of the quadrotor. + + | Clockwise rotation about this axis defines a positive **roll** angle. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + + - Y axis is along the **Left** direction of the quadrotor. + + | Clockwise rotation about this axis defines a positive **pitch** angle. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + + - Z axis is along the **Up** direction. + + | Clockwise rotation about this axis defines a positive **yaw** angle. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + + Args: + roll (float): Desired roll angle, in radians. + pitch (float): Desired pitch angle, in radians. + yaw_rate (float): Desired yaw rate, in radian per second. + throttle (float): Desired throttle (between 0.0 to 1.0) + duration (float): Desired amount of time (seconds), to send this command for + vehicle_name (str, optional): Name of the multirotor to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('moveByRollPitchYawrateThrottle', roll, -pitch, -yaw_rate, throttle, duration, vehicle_name) + + def moveByRollPitchYawrateZAsync(self, roll, pitch, yaw_rate, z, duration, vehicle_name = ''): + """ + - z is given in local NED frame of the vehicle. + - Roll angle, pitch angle, and yaw rate set points are given in **radians**, in the body frame. + - The body frame follows the Front Left Up (FLU) convention, and right-handedness. + + - Frame Convention: + - X axis is along the **Front** direction of the quadrotor. + + | Clockwise rotation about this axis defines a positive **roll** angle. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + + - Y axis is along the **Left** direction of the quadrotor. + + | Clockwise rotation about this axis defines a positive **pitch** angle. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + + - Z axis is along the **Up** direction. + + | Clockwise rotation about this axis defines a positive **yaw** angle. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + + Args: + roll (float): Desired roll angle, in radians. + pitch (float): Desired pitch angle, in radians. + yaw_rate (float): Desired yaw rate, in radian per second. + z (float): Desired Z value (in local NED frame of the vehicle) + duration (float): Desired amount of time (seconds), to send this command for + vehicle_name (str, optional): Name of the multirotor to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('moveByRollPitchYawrateZ', roll, -pitch, -yaw_rate, z, duration, vehicle_name) + + def moveByAngleRatesZAsync(self, roll_rate, pitch_rate, yaw_rate, z, duration, vehicle_name = ''): + """ + - z is given in local NED frame of the vehicle. + - Roll rate, pitch rate, and yaw rate set points are given in **radians**, in the body frame. + - The body frame follows the Front Left Up (FLU) convention, and right-handedness. + + - Frame Convention: + - X axis is along the **Front** direction of the quadrotor. + + | Clockwise rotation about this axis defines a positive **roll** angle. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + + - Y axis is along the **Left** direction of the quadrotor. + + | Clockwise rotation about this axis defines a positive **pitch** angle. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + + - Z axis is along the **Up** direction. + + | Clockwise rotation about this axis defines a positive **yaw** angle. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + + Args: + roll_rate (float): Desired roll rate, in radians / second + pitch_rate (float): Desired pitch rate, in radians / second + yaw_rate (float): Desired yaw rate, in radians / second + z (float): Desired Z value (in local NED frame of the vehicle) + duration (float): Desired amount of time (seconds), to send this command for + vehicle_name (str, optional): Name of the multirotor to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('moveByAngleRatesZ', roll_rate, -pitch_rate, -yaw_rate, z, duration, vehicle_name) + + def moveByAngleRatesThrottleAsync(self, roll_rate, pitch_rate, yaw_rate, throttle, duration, vehicle_name = ''): + """ + - Desired throttle is between 0.0 to 1.0 + - Roll rate, pitch rate, and yaw rate set points are given in **radians**, in the body frame. + - The body frame follows the Front Left Up (FLU) convention, and right-handedness. + + - Frame Convention: + - X axis is along the **Front** direction of the quadrotor. + + | Clockwise rotation about this axis defines a positive **roll** angle. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + + - Y axis is along the **Left** direction of the quadrotor. + + | Clockwise rotation about this axis defines a positive **pitch** angle. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + + - Z axis is along the **Up** direction. + + | Clockwise rotation about this axis defines a positive **yaw** angle. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + + Args: + roll_rate (float): Desired roll rate, in radians / second + pitch_rate (float): Desired pitch rate, in radians / second + yaw_rate (float): Desired yaw rate, in radians / second + throttle (float): Desired throttle (between 0.0 to 1.0) + duration (float): Desired amount of time (seconds), to send this command for + vehicle_name (str, optional): Name of the multirotor to send this command to + + Returns: + msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + """ + return self.client.call_async('moveByAngleRatesThrottle', roll_rate, -pitch_rate, -yaw_rate, throttle, duration, vehicle_name) + + def setAngleRateControllerGains(self, angle_rate_gains=AngleRateControllerGains(), vehicle_name = ''): + """ + - Modifying these gains will have an affect on *ALL* move*() APIs. + This is because any velocity setpoint is converted to an angle level setpoint which is tracked with an angle level controllers. + That angle level setpoint is itself tracked with and angle rate controller. + - This function should only be called if the default angle rate control PID gains need to be modified. + + Args: + angle_rate_gains (AngleRateControllerGains): + - Correspond to the roll, pitch, yaw axes, defined in the body frame. + - Pass AngleRateControllerGains() to reset gains to default recommended values. + vehicle_name (str, optional): Name of the multirotor to send this command to + """ + self.client.call('setAngleRateControllerGains', *(angle_rate_gains.to_lists()+(vehicle_name,))) + + def setAngleLevelControllerGains(self, angle_level_gains=AngleLevelControllerGains(), vehicle_name = ''): + """ + - Sets angle level controller gains (used by any API setting angle references - for ex: moveByRollPitchYawZAsync(), moveByRollPitchYawThrottleAsync(), etc) + - Modifying these gains will also affect the behaviour of moveByVelocityAsync() API. + This is because the AirSim flight controller will track velocity setpoints by converting them to angle set points. + - This function should only be called if the default angle level control PID gains need to be modified. + - Passing AngleLevelControllerGains() sets gains to default airsim values. + + Args: + angle_level_gains (AngleLevelControllerGains): + - Correspond to the roll, pitch, yaw axes, defined in the body frame. + - Pass AngleLevelControllerGains() to reset gains to default recommended values. + vehicle_name (str, optional): Name of the multirotor to send this command to + """ + self.client.call('setAngleLevelControllerGains', *(angle_level_gains.to_lists()+(vehicle_name,))) + + def setVelocityControllerGains(self, velocity_gains=VelocityControllerGains(), vehicle_name = ''): + """ + - Sets velocity controller gains for moveByVelocityAsync(). + - This function should only be called if the default velocity control PID gains need to be modified. + - Passing VelocityControllerGains() sets gains to default airsim values. + + Args: + velocity_gains (VelocityControllerGains): + - Correspond to the world X, Y, Z axes. + - Pass VelocityControllerGains() to reset gains to default recommended values. + - Modifying velocity controller gains will have an affect on the behaviour of moveOnSplineAsync() and moveOnSplineVelConstraintsAsync(), as they both use velocity control to track the trajectory. + vehicle_name (str, optional): Name of the multirotor to send this command to + """ + self.client.call('setVelocityControllerGains', *(velocity_gains.to_lists()+(vehicle_name,))) + + + def setPositionControllerGains(self, position_gains=PositionControllerGains(), vehicle_name = ''): + """ + Sets position controller gains for moveByPositionAsync. + This function should only be called if the default position control PID gains need to be modified. + + Args: + position_gains (PositionControllerGains): + - Correspond to the X, Y, Z axes. + - Pass PositionControllerGains() to reset gains to default recommended values. + vehicle_name (str, optional): Name of the multirotor to send this command to + """ + self.client.call('setPositionControllerGains', *(position_gains.to_lists()+(vehicle_name,))) + + # query vehicle state + def getMultirotorState(self, vehicle_name = ''): + """ + Args: + vehicle_name (str, optional): Vehicle to get the state of + + Returns: + MultirotorState: + """ + return MultirotorState.from_msgpack(self.client.call('getMultirotorState', vehicle_name)) + getMultirotorState.__annotations__ = {'return': MultirotorState} + + +# ----------------------------------- Car APIs --------------------------------------------- +class CarClient(VehicleClient, object): + def __init__(self, ip = "", port = 41451, timeout_value = 3600): + super(CarClient, self).__init__(ip, port, timeout_value) + + def setCarControls(self, controls, vehicle_name = ''): + """ + Control the car using throttle, steering, brake, etc. + + Args: + controls (CarControls): Struct containing control values + vehicle_name (str, optional): Name of vehicle to be controlled + """ + self.client.call('setCarControls', controls, vehicle_name) + + def getCarState(self, vehicle_name = ''): + """ + Args: + vehicle_name (str, optional): Name of vehicle + + Returns: + CarState: + """ + state_raw = self.client.call('getCarState', vehicle_name) + return CarState.from_msgpack(state_raw) + + def getCarControls(self, vehicle_name=''): + """ + Args: + vehicle_name (str, optional): Name of vehicle + + Returns: + CarControls: + """ + controls_raw = self.client.call('getCarControls', vehicle_name) + return CarControls.from_msgpack(controls_raw) diff --git a/auto/airsim/client.pyc b/auto/airsim/client.pyc new file mode 100644 index 0000000000..563d74ae68 Binary files /dev/null and b/auto/airsim/client.pyc differ diff --git a/auto/airsim/pfm.py b/auto/airsim/pfm.py new file mode 100644 index 0000000000..6f9f963a8a --- /dev/null +++ b/auto/airsim/pfm.py @@ -0,0 +1,85 @@ +import numpy as np +import matplotlib.pyplot as plt +import re +import sys +import pdb + + +def read_pfm(file): + """ Read a pfm file """ + file = open(file, 'rb') + + color = None + width = None + height = None + scale = None + endian = None + + header = file.readline().rstrip() + header = str(bytes.decode(header, encoding='utf-8')) + if header == 'PF': + color = True + elif header == 'Pf': + color = False + else: + raise Exception('Not a PFM file.') + + pattern = r'^(\d+)\s(\d+)\s$' + temp_str = str(bytes.decode(file.readline(), encoding='utf-8')) + dim_match = re.match(pattern, temp_str) + if dim_match: + width, height = map(int, dim_match.groups()) + else: + temp_str += str(bytes.decode(file.readline(), encoding='utf-8')) + dim_match = re.match(pattern, temp_str) + if dim_match: + width, height = map(int, dim_match.groups()) + else: + raise Exception('Malformed PFM header: width, height cannot be found') + + scale = float(file.readline().rstrip()) + if scale < 0: # little-endian + endian = '<' + scale = -scale + else: + endian = '>' # big-endian + + data = np.fromfile(file, endian + 'f') + shape = (height, width, 3) if color else (height, width) + + data = np.reshape(data, shape) + # DEY: I don't know why this was there. + file.close() + + return data, scale + + +def write_pfm(file, image, scale=1): + """ Write a pfm file """ + file = open(file, 'wb') + + color = None + + if image.dtype.name != 'float32': + raise Exception('Image dtype must be float32.') + + if len(image.shape) == 3 and image.shape[2] == 3: # color image + color = True + elif len(image.shape) == 2 or len(image.shape) == 3 and image.shape[2] == 1: # greyscale + color = False + else: + raise Exception('Image must have H x W x 3, H x W x 1 or H x W dimensions.') + + file.write(bytes('PF\n', 'UTF-8') if color else bytes('Pf\n', 'UTF-8')) + temp_str = '%d %d\n' % (image.shape[1], image.shape[0]) + file.write(bytes(temp_str, 'UTF-8')) + + endian = image.dtype.byteorder + + if endian == '<' or endian == '=' and sys.byteorder == 'little': + scale = -scale + + temp_str = '%f\n' % scale + file.write(bytes(temp_str, 'UTF-8')) + + image.tofile(file) diff --git a/auto/airsim/types.py b/auto/airsim/types.py new file mode 100644 index 0000000000..297fb4645c --- /dev/null +++ b/auto/airsim/types.py @@ -0,0 +1,507 @@ +from __future__ import print_function +import msgpackrpc #install as admin: pip install msgpack-rpc-python +import numpy as np #pip install numpy + +class MsgpackMixin: + def __repr__(self): + from pprint import pformat + return "<" + type(self).__name__ + "> " + pformat(vars(self), indent=4, width=1) + + def to_msgpack(self, *args, **kwargs): + return self.__dict__ + + @classmethod + def from_msgpack(cls, encoded): + obj = cls() + #obj.__dict__ = {k.decode('utf-8'): (from_msgpack(v.__class__, v) if hasattr(v, "__dict__") else v) for k, v in encoded.items()} + obj.__dict__ = { k : (v if not isinstance(v, dict) else getattr(getattr(obj, k).__class__, "from_msgpack")(v)) for k, v in encoded.items()} + #return cls(**msgpack.unpack(encoded)) + return obj + + +class ImageType: + Scene = 0 + DepthPlanner = 1 + DepthPerspective = 2 + DepthVis = 3 + DisparityNormalized = 4 + Segmentation = 5 + SurfaceNormals = 6 + Infrared = 7 + +class DrivetrainType: + MaxDegreeOfFreedom = 0 + ForwardOnly = 1 + +class LandedState: + Landed = 0 + Flying = 1 + +class WeatherParameter: + Rain = 0 + Roadwetness = 1 + Snow = 2 + RoadSnow = 3 + MapleLeaf = 4 + RoadLeaf = 5 + Dust = 6 + Fog = 7 + Enabled = 8 + +class Vector3r(MsgpackMixin): + x_val = 0.0 + y_val = 0.0 + z_val = 0.0 + + def __init__(self, x_val = 0.0, y_val = 0.0, z_val = 0.0): + self.x_val = x_val + self.y_val = y_val + self.z_val = z_val + + @staticmethod + def nanVector3r(): + return Vector3r(np.nan, np.nan, np.nan) + + def __add__(self, other): + return Vector3r(self.x_val + other.x_val, self.y_val + other.y_val, self.z_val + other.z_val) + + def __sub__(self, other): + return Vector3r(self.x_val - other.x_val, self.y_val - other.y_val, self.z_val - other.z_val) + + def __truediv__(self, other): + if type(other) in [int, float] + np.sctypes['int'] + np.sctypes['uint'] + np.sctypes['float']: + return Vector3r( self.x_val / other, self.y_val / other, self.z_val / other) + else: + raise TypeError('unsupported operand type(s) for /: %s and %s' % ( str(type(self)), str(type(other))) ) + + def __mul__(self, other): + if type(other) in [int, float] + np.sctypes['int'] + np.sctypes['uint'] + np.sctypes['float']: + return Vector3r(self.x_val*other, self.y_val*other, self.z_val*other) + else: + raise TypeError('unsupported operand type(s) for *: %s and %s' % ( str(type(self)), str(type(other))) ) + + def dot(self, other): + if type(self) == type(other): + return self.x_val*other.x_val + self.y_val*other.y_val + self.z_val*other.z_val + else: + raise TypeError('unsupported operand type(s) for \'dot\': %s and %s' % ( str(type(self)), str(type(other))) ) + + def cross(self, other): + if type(self) == type(other): + cross_product = np.cross(self.to_numpy_array(), other.to_numpy_array()) + return Vector3r(cross_product[0], cross_product[1], cross_product[2]) + else: + raise TypeError('unsupported operand type(s) for \'cross\': %s and %s' % ( str(type(self)), str(type(other))) ) + + def get_length(self): + return ( self.x_val**2 + self.y_val**2 + self.z_val**2 )**0.5 + + def distance_to(self, other): + return ( (self.x_val-other.x_val)**2 + (self.y_val-other.y_val)**2 + (self.z_val-other.z_val)**2 )**0.5 + + def to_Quaternionr(self): + return Quaternionr(self.x_val, self.y_val, self.z_val, 0) + + def to_numpy_array(self): + return np.array([self.x_val, self.y_val, self.z_val], dtype=np.float32) + + +class Quaternionr(MsgpackMixin): + w_val = 0.0 + x_val = 0.0 + y_val = 0.0 + z_val = 0.0 + + def __init__(self, x_val = 0.0, y_val = 0.0, z_val = 0.0, w_val = 1.0): + self.x_val = x_val + self.y_val = y_val + self.z_val = z_val + self.w_val = w_val + + @staticmethod + def nanQuaternionr(): + return Quaternionr(np.nan, np.nan, np.nan, np.nan) + + def __add__(self, other): + if type(self) == type(other): + return Quaternionr( self.x_val+other.x_val, self.y_val+other.y_val, self.z_val+other.z_val, self.w_val+other.w_val ) + else: + raise TypeError('unsupported operand type(s) for +: %s and %s' % ( str(type(self)), str(type(other))) ) + + def __mul__(self, other): + if type(self) == type(other): + t, x, y, z = self.w_val, self.x_val, self.y_val, self.z_val + a, b, c, d = other.w_val, other.x_val, other.y_val, other.z_val + return Quaternionr( w_val = a*t - b*x - c*y - d*z, + x_val = b*t + a*x + d*y - c*z, + y_val = c*t + a*y + b*z - d*x, + z_val = d*t + z*a + c*x - b*y) + else: + raise TypeError('unsupported operand type(s) for *: %s and %s' % ( str(type(self)), str(type(other))) ) + + def __truediv__(self, other): + if type(other) == type(self): + return self * other.inverse() + elif type(other) in [int, float] + np.sctypes['int'] + np.sctypes['uint'] + np.sctypes['float']: + return Quaternionr( self.x_val / other, self.y_val / other, self.z_val / other, self.w_val / other) + else: + raise TypeError('unsupported operand type(s) for /: %s and %s' % ( str(type(self)), str(type(other))) ) + + def dot(self, other): + if type(self) == type(other): + return self.x_val*other.x_val + self.y_val*other.y_val + self.z_val*other.z_val + self.w_val*other.w_val + else: + raise TypeError('unsupported operand type(s) for \'dot\': %s and %s' % ( str(type(self)), str(type(other))) ) + + def cross(self, other): + if type(self) == type(other): + return (self * other - other * self) / 2 + else: + raise TypeError('unsupported operand type(s) for \'cross\': %s and %s' % ( str(type(self)), str(type(other))) ) + + def outer_product(self, other): + if type(self) == type(other): + return ( self.inverse()*other - other.inverse()*self ) / 2 + else: + raise TypeError('unsupported operand type(s) for \'outer_product\': %s and %s' % ( str(type(self)), str(type(other))) ) + + def rotate(self, other): + if type(self) == type(other): + if other.get_length() == 1: + return other * self * other.inverse() + else: + raise ValueError('length of the other Quaternionr must be 1') + else: + raise TypeError('unsupported operand type(s) for \'rotate\': %s and %s' % ( str(type(self)), str(type(other))) ) + + def conjugate(self): + return Quaternionr(-self.x_val, -self.y_val, -self.z_val, self.w_val) + + def star(self): + return self.conjugate() + + def inverse(self): + return self.star() / self.dot(self) + + def sgn(self): + return self/self.get_length() + + def get_length(self): + return ( self.x_val**2 + self.y_val**2 + self.z_val**2 + self.w_val**2 )**0.5 + + def to_numpy_array(self): + return np.array([self.x_val, self.y_val, self.z_val, self.w_val], dtype=np.float32) + + +class Pose(MsgpackMixin): + position = Vector3r() + orientation = Quaternionr() + + def __init__(self, position_val = None, orientation_val = None): + position_val = position_val if position_val != None else Vector3r() + orientation_val = orientation_val if orientation_val != None else Quaternionr() + self.position = position_val + self.orientation = orientation_val + + @staticmethod + def nanPose(): + return Pose(Vector3r.nanVector3r(), Quaternionr.nanQuaternionr()) + + +class CollisionInfo(MsgpackMixin): + has_collided = False + normal = Vector3r() + impact_point = Vector3r() + position = Vector3r() + penetration_depth = 0.0 + time_stamp = 0.0 + object_name = "" + object_id = -1 + +class GeoPoint(MsgpackMixin): + latitude = 0.0 + longitude = 0.0 + altitude = 0.0 + +class YawMode(MsgpackMixin): + is_rate = True + yaw_or_rate = 0.0 + def __init__(self, is_rate = True, yaw_or_rate = 0.0): + self.is_rate = is_rate + self.yaw_or_rate = yaw_or_rate + +class RCData(MsgpackMixin): + timestamp = 0 + pitch, roll, throttle, yaw = (0.0,)*4 #init 4 variable to 0.0 + switch1, switch2, switch3, switch4 = (0,)*4 + switch5, switch6, switch7, switch8 = (0,)*4 + is_initialized = False + is_valid = False + def __init__(self, timestamp = 0, pitch = 0.0, roll = 0.0, throttle = 0.0, yaw = 0.0, switch1 = 0, + switch2 = 0, switch3 = 0, switch4 = 0, switch5 = 0, switch6 = 0, switch7 = 0, switch8 = 0, is_initialized = False, is_valid = False): + self.timestamp = timestamp + self.pitch = pitch + self.roll = roll + self.throttle = throttle + self.yaw = yaw + self.switch1 = switch1 + self.switch2 = switch2 + self.switch3 = switch3 + self.switch4 = switch4 + self.switch5 = switch5 + self.switch6 = switch6 + self.switch7 = switch7 + self.switch8 = switch8 + self.is_initialized = is_initialized + self.is_valid = is_valid + +class ImageRequest(MsgpackMixin): + camera_name = '0' + image_type = ImageType.Scene + pixels_as_float = False + compress = False + + def __init__(self, camera_name, image_type, pixels_as_float = False, compress = True): + # todo: in future remove str(), it's only for compatibility to pre v1.2 + self.camera_name = str(camera_name) + self.image_type = image_type + self.pixels_as_float = pixels_as_float + self.compress = compress + + +class ImageResponse(MsgpackMixin): + image_data_uint8 = np.uint8(0) + image_data_float = 0.0 + camera_position = Vector3r() + camera_orientation = Quaternionr() + time_stamp = np.uint64(0) + message = '' + pixels_as_float = 0.0 + compress = True + width = 0 + height = 0 + image_type = ImageType.Scene + +class CarControls(MsgpackMixin): + throttle = 0.0 + steering = 0.0 + brake = 0.0 + handbrake = False + is_manual_gear = False + manual_gear = 0 + gear_immediate = True + + def __init__(self, throttle = 0, steering = 0, brake = 0, + handbrake = False, is_manual_gear = False, manual_gear = 0, gear_immediate = True): + self.throttle = throttle + self.steering = steering + self.brake = brake + self.handbrake = handbrake + self.is_manual_gear = is_manual_gear + self.manual_gear = manual_gear + self.gear_immediate = gear_immediate + + + def set_throttle(self, throttle_val, forward): + if (forward): + self.is_manual_gear = False + self.manual_gear = 0 + self.throttle = abs(throttle_val) + else: + self.is_manual_gear = False + self.manual_gear = -1 + self.throttle = - abs(throttle_val) + +class KinematicsState(MsgpackMixin): + position = Vector3r() + orientation = Quaternionr() + linear_velocity = Vector3r() + angular_velocity = Vector3r() + linear_acceleration = Vector3r() + angular_acceleration = Vector3r() + +class EnvironmentState(MsgpackMixin): + position = Vector3r() + geo_point = GeoPoint() + gravity = Vector3r() + air_pressure = 0.0 + temperature = 0.0 + air_density = 0.0 + +class CarState(MsgpackMixin): + speed = 0.0 + gear = 0 + rpm = 0.0 + maxrpm = 0.0 + handbrake = False + collision = CollisionInfo() + kinematics_estimated = KinematicsState() + timestamp = np.uint64(0) + +class MultirotorState(MsgpackMixin): + collision = CollisionInfo() + kinematics_estimated = KinematicsState() + gps_location = GeoPoint() + timestamp = np.uint64(0) + landed_state = LandedState.Landed + rc_data = RCData() + ready = False + ready_message = "" + can_arm = False + +class ProjectionMatrix(MsgpackMixin): + matrix = [] + +class CameraInfo(MsgpackMixin): + pose = Pose() + fov = -1 + proj_mat = ProjectionMatrix() + +class LidarData(MsgpackMixin): + point_cloud = 0.0 + time_stamp = np.uint64(0) + pose = Pose() + +class ImuData(MsgpackMixin): + time_stamp = np.uint64(0) + orientation = Quaternionr() + angular_velocity = Vector3r() + linear_acceleration = Vector3r() + +class BarometerData(MsgpackMixin): + time_stamp = np.uint64(0) + altitude = Quaternionr() + pressure = Vector3r() + qnh = Vector3r() + +class MagnetometerData(MsgpackMixin): + time_stamp = np.uint64(0) + magnetic_field_body = Vector3r() + magnetic_field_covariance = 0.0 + +class GnssFixType(MsgpackMixin): + GNSS_FIX_NO_FIX = 0 + GNSS_FIX_TIME_ONLY = 1 + GNSS_FIX_2D_FIX = 2 + GNSS_FIX_3D_FIX = 3 + +class GnssReport(MsgpackMixin): + geo_point = GeoPoint() + eph = 0.0 + epv = 0.0 + velocity = Vector3r() + fix_type = GnssFixType() + time_utc = np.uint64(0) + +class GpsData(MsgpackMixin): + time_stamp = np.uint64(0) + gnss = GnssReport() + is_valid = False + +class DistanceSensorData(MsgpackMixin): + time_stamp = np.uint64(0) + distance = np.float32(0) + min_distance = np.float32(0) + max_distance = np.float32(0) + relative_pose = Pose() + +class PIDGains(): + """ + Struct to store values of PID gains. Used to transmit controller gain values while instantiating + AngleLevel/AngleRate/Velocity/PositionControllerGains objects. + + Attributes: + kP (float): Proportional gain + kI (float): Integrator gain + kD (float): Derivative gain + """ + def __init__(self, kp, ki, kd): + self.kp = kp + self.ki = ki + self.kd = kd + + def to_list(self): + return [self.kp, self.ki, self.kd] + +class AngleRateControllerGains(): + """ + Struct to contain controller gains used by angle level PID controller + + Attributes: + roll_gains (PIDGains): kP, kI, kD for roll axis + pitch_gains (PIDGains): kP, kI, kD for pitch axis + yaw_gains (PIDGains): kP, kI, kD for yaw axis + """ + def __init__(self, roll_gains = PIDGains(0.25, 0, 0), + pitch_gains = PIDGains(0.25, 0, 0), + yaw_gains = PIDGains(0.25, 0, 0)): + self.roll_gains = roll_gains + self.pitch_gains = pitch_gains + self.yaw_gains = yaw_gains + + def to_lists(self): + return [self.roll_gains.kp, self.pitch_gains.kp, self.yaw_gains.kp], [self.roll_gains.ki, self.pitch_gains.ki, self.yaw_gains.ki], [self.roll_gains.kd, self.pitch_gains.kd, self.yaw_gains.kd] + +class AngleLevelControllerGains(): + """ + Struct to contain controller gains used by angle rate PID controller + + Attributes: + roll_gains (PIDGains): kP, kI, kD for roll axis + pitch_gains (PIDGains): kP, kI, kD for pitch axis + yaw_gains (PIDGains): kP, kI, kD for yaw axis + """ + def __init__(self, roll_gains = PIDGains(2.5, 0, 0), + pitch_gains = PIDGains(2.5, 0, 0), + yaw_gains = PIDGains(2.5, 0, 0)): + self.roll_gains = roll_gains + self.pitch_gains = pitch_gains + self.yaw_gains = yaw_gains + + def to_lists(self): + return [self.roll_gains.kp, self.pitch_gains.kp, self.yaw_gains.kp], [self.roll_gains.ki, self.pitch_gains.ki, self.yaw_gains.ki], [self.roll_gains.kd, self.pitch_gains.kd, self.yaw_gains.kd] + +class VelocityControllerGains(): + """ + Struct to contain controller gains used by velocity PID controller + + Attributes: + x_gains (PIDGains): kP, kI, kD for X axis + y_gains (PIDGains): kP, kI, kD for Y axis + z_gains (PIDGains): kP, kI, kD for Z axis + """ + def __init__(self, x_gains = PIDGains(0.2, 0, 0), + y_gains = PIDGains(0.2, 0, 0), + z_gains = PIDGains(2.0, 2.0, 0)): + self.x_gains = x_gains + self.y_gains = y_gains + self.z_gains = z_gains + + def to_lists(self): + return [self.x_gains.kp, self.y_gains.kp, self.z_gains.kp], [self.x_gains.ki, self.y_gains.ki, self.z_gains.ki], [self.x_gains.kd, self.y_gains.kd, self.z_gains.kd] + +class PositionControllerGains(): + """ + Struct to contain controller gains used by position PID controller + + Attributes: + x_gains (PIDGains): kP, kI, kD for X axis + y_gains (PIDGains): kP, kI, kD for Y axis + z_gains (PIDGains): kP, kI, kD for Z axis + """ + def __init__(self, x_gains = PIDGains(0.25, 0, 0), + y_gains = PIDGains(0.25, 0, 0), + z_gains = PIDGains(0.25, 0, 0)): + self.x_gains = x_gains + self.y_gains = y_gains + self.z_gains = z_gains + + def to_lists(self): + return [self.x_gains.kp, self.y_gains.kp, self.z_gains.kp], [self.x_gains.ki, self.y_gains.ki, self.z_gains.ki], [self.x_gains.kd, self.y_gains.kd, self.z_gains.kd] + +class MeshPositionVertexBuffersResponse(MsgpackMixin): + position = Vector3r() + orientation = Quaternionr() + vertices = 0.0 + indices = 0.0 + name = '' diff --git a/auto/airsim/types.pyc b/auto/airsim/types.pyc new file mode 100644 index 0000000000..7ea22487d9 Binary files /dev/null and b/auto/airsim/types.pyc differ diff --git a/auto/airsim/utils.py b/auto/airsim/utils.py new file mode 100644 index 0000000000..cd56292417 --- /dev/null +++ b/auto/airsim/utils.py @@ -0,0 +1,222 @@ +import numpy as np #pip install numpy +import math +import time +import sys +import os +import inspect +import types +import re + +from .types import * + + +def string_to_uint8_array(bstr): + return np.fromstring(bstr, np.uint8) + +def string_to_float_array(bstr): + return np.fromstring(bstr, np.float32) + +def list_to_2d_float_array(flst, width, height): + return np.reshape(np.asarray(flst, np.float32), (height, width)) + +def get_pfm_array(response): + return list_to_2d_float_array(response.image_data_float, response.width, response.height) + + +def get_public_fields(obj): + return [attr for attr in dir(obj) + if not (attr.startswith("_") + or inspect.isbuiltin(attr) + or inspect.isfunction(attr) + or inspect.ismethod(attr))] + + + +def to_dict(obj): + return dict([attr, getattr(obj, attr)] for attr in get_public_fields(obj)) + + +def to_str(obj): + return str(to_dict(obj)) + + +def write_file(filename, bstr): + with open(filename, 'wb') as afile: + afile.write(bstr) + +# helper method for converting getOrientation to roll/pitch/yaw +# https:#en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles + +def to_eularian_angles(q): + z = q.z_val + y = q.y_val + x = q.x_val + w = q.w_val + ysqr = y * y + + # roll (x-axis rotation) + t0 = +2.0 * (w*x + y*z) + t1 = +1.0 - 2.0*(x*x + ysqr) + roll = math.atan2(t0, t1) + + # pitch (y-axis rotation) + t2 = +2.0 * (w*y - z*x) + if (t2 > 1.0): + t2 = 1 + if (t2 < -1.0): + t2 = -1.0 + pitch = math.asin(t2) + + # yaw (z-axis rotation) + t3 = +2.0 * (w*z + x*y) + t4 = +1.0 - 2.0 * (ysqr + z*z) + yaw = math.atan2(t3, t4) + + return (pitch, roll, yaw) + + +def to_quaternion(pitch, roll, yaw): + t0 = math.cos(yaw * 0.5) + t1 = math.sin(yaw * 0.5) + t2 = math.cos(roll * 0.5) + t3 = math.sin(roll * 0.5) + t4 = math.cos(pitch * 0.5) + t5 = math.sin(pitch * 0.5) + + q = Quaternionr() + q.w_val = t0 * t2 * t4 + t1 * t3 * t5 #w + q.x_val = t0 * t3 * t4 - t1 * t2 * t5 #x + q.y_val = t0 * t2 * t5 + t1 * t3 * t4 #y + q.z_val = t1 * t2 * t4 - t0 * t3 * t5 #z + return q + + +def wait_key(message = ''): + ''' Wait for a key press on the console and return it. ''' + if message != '': + print (message) + + result = None + if os.name == 'nt': + import msvcrt + result = msvcrt.getch() + else: + import termios + fd = sys.stdin.fileno() + + oldterm = termios.tcgetattr(fd) + newattr = termios.tcgetattr(fd) + newattr[3] = newattr[3] & ~termios.ICANON & ~termios.ECHO + termios.tcsetattr(fd, termios.TCSANOW, newattr) + + try: + result = sys.stdin.read(1) + except IOError: + pass + finally: + termios.tcsetattr(fd, termios.TCSAFLUSH, oldterm) + + return result + + +def read_pfm(file): + """ Read a pfm file """ + file = open(file, 'rb') + + color = None + width = None + height = None + scale = None + endian = None + + header = file.readline().rstrip() + header = str(bytes.decode(header, encoding='utf-8')) + if header == 'PF': + color = True + elif header == 'Pf': + color = False + else: + raise Exception('Not a PFM file.') + + temp_str = str(bytes.decode(file.readline(), encoding='utf-8')) + dim_match = re.match(r'^(\d+)\s(\d+)\s$', temp_str) + if dim_match: + width, height = map(int, dim_match.groups()) + else: + raise Exception('Malformed PFM header.') + + scale = float(file.readline().rstrip()) + if scale < 0: # little-endian + endian = '<' + scale = -scale + else: + endian = '>' # big-endian + + data = np.fromfile(file, endian + 'f') + shape = (height, width, 3) if color else (height, width) + + data = np.reshape(data, shape) + # DEY: I don't know why this was there. + file.close() + + return data, scale + + +def write_pfm(file, image, scale=1): + """ Write a pfm file """ + file = open(file, 'wb') + + color = None + + if image.dtype.name != 'float32': + raise Exception('Image dtype must be float32.') + + if len(image.shape) == 3 and image.shape[2] == 3: # color image + color = True + elif len(image.shape) == 2 or len(image.shape) == 3 and image.shape[2] == 1: # grayscale + color = False + else: + raise Exception('Image must have H x W x 3, H x W x 1 or H x W dimensions.') + + file.write('PF\n'.encode('utf-8') if color else 'Pf\n'.encode('utf-8')) + temp_str = '%d %d\n' % (image.shape[1], image.shape[0]) + file.write(temp_str.encode('utf-8')) + + endian = image.dtype.byteorder + + if endian == '<' or endian == '=' and sys.byteorder == 'little': + scale = -scale + + temp_str = '%f\n' % scale + file.write(temp_str.encode('utf-8')) + + image.tofile(file) + + +def write_png(filename, image): + """ image must be numpy array H X W X channels + """ + import zlib, struct + + buf = image.flatten().tobytes() + width = image.shape[1] + height = image.shape[0] + + # reverse the vertical line order and add null bytes at the start + width_byte_3 = width * 3 + raw_data = b''.join(b'\x00' + buf[span:span + width_byte_3] + for span in range((height - 1) * width_byte_3, -1, - width_byte_3)) + + def png_pack(png_tag, data): + chunk_head = png_tag + data + return (struct.pack("!I", len(data)) + + chunk_head + + struct.pack("!I", 0xFFFFFFFF & zlib.crc32(chunk_head))) + + png_bytes = b''.join([ + b'\x89PNG\r\n\x1a\n', + png_pack(b'IHDR', struct.pack("!2I5B", width, height, 8, 6, 0, 0, 0)), + png_pack(b'IDAT', zlib.compress(raw_data, 9)), + png_pack(b'IEND', b'')]) + + write_file(filename, png_bytes) diff --git a/auto/airsim/utils.pyc b/auto/airsim/utils.pyc new file mode 100644 index 0000000000..b306b9caa5 Binary files /dev/null and b/auto/airsim/utils.pyc differ diff --git a/auto/python/airsim_helper.py b/auto/python/airsim_helper.py new file mode 100644 index 0000000000..96243c7afe --- /dev/null +++ b/auto/python/airsim_helper.py @@ -0,0 +1,299 @@ +import setup_path +import airsim +import cv2 +import json +import platform +import rospy +import numpy as np +import math +import tf2_ros +import tf2_geometry_msgs +import atexit +import time +import threading +import subprocess +import os +import roslaunch +from os import environ +from os.path import expanduser +from sensor_msgs.msg import Image,CameraInfo +from tf2_msgs.msg import TFMessage +from geometry_msgs.msg import TransformStamped, Vector3, Quaternion, PoseStamped, Point +from cv_bridge import CvBridge +from tf.transformations import quaternion_from_matrix, quaternion_from_euler, euler_from_quaternion, quaternion_conjugate + +class AirSimHelper(threading.Thread): + + def severConnection(self,ASP): + for robot in ASP.robots: + ASP.client.cancelLastTask(robot.name) + ASP.client.reset() + if (not robot.sitl): + robot.px4.communicate('shutdown') + print('waiting for '+robot.name+' px4_sitl to shutdown') + print('Severed connection with '+robot.name) + print('waiting for all processes to finish...') + time.sleep(1) + + def __init__(self): + if (platform.system() == "Windows"): + home = environ['USERPROFILE'] + settings_path = home + "Documents\AirSim\settings.json" + else: + home = expanduser("~") + settings_path = home + "/Documents/AirSim/settings.json" + with open(settings_path) as f: + airsim_settings = json.load(f) + + threading.Thread.__init__(self) + + #initialize the airsim wrapper + ASP = AirSimPublisher() + atexit.register(self.severConnection, ASP=ASP) + ASP.client.confirmConnection() + + + launch = roslaunch.scriptapi.ROSLaunch() + launch.start() + uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) + roslaunch.configure_logging(uuid) + + #Init Rospy and TF + #rospy.init_node('airsim_publisher', anonymous=True) + static_br = tf2_ros.StaticTransformBroadcaster() + + vehicles = airsim_settings['Vehicles'].keys() + for vehicle in vehicles: + + robot = Robot(vehicle) + + cameras = airsim_settings['Vehicles'][vehicle]['Cameras'] + robot.addCameras(cameras) + try: + robot.setAddress(airsim_settings['Vehicles'][vehicle]['LocalHostIp']) + robot.setPort(airsim_settings['Vehicles'][vehicle]['TcpPort']) + robot.setSITL(airsim_settings['Vehicles'][vehicle]['UseSerial']) + except (RuntimeError, TypeError, NameError): + print('Error reading LocalHostIP, TcpPort, or UseSerial in settings.json for robot '+vehicle) + + if (not robot.sitl): + print('Starting px4_sitl for '+vehicle) + robot.setPX4(subprocess.Popen(["make","px4_sitl_default","none_iris"], stdout=subprocess.PIPE, stderr=subprocess.PIPE, cwd="../px4/Firmware")) + transforms = ASP.getTransforms(airsim_settings['Vehicles'][vehicle], robot) + + #add ned transforms + ned2enu = TransformStamped() + ned2enu.header.frame_id = robot.name + ned2enu.header.stamp = rospy.Time.now() + ned2enu.child_frame_id = robot.name + "_ned" + ned2enu.transform.translation.x = 0.0 + ned2enu.transform.translation.y = 0.0 + ned2enu.transform.translation.z = 0.0 + + q = quaternion_from_euler(180*(math.pi/180), 0, 90*(math.pi/180)) + ned2enu.transform.rotation.x = q[0] + ned2enu.transform.rotation.y = q[1] + ned2enu.transform.rotation.z = q[2] + ned2enu.transform.rotation.w = q[3] + transforms.append(ned2enu) + #add orb transforms + orb2enu = TransformStamped() + orb2enu.header.frame_id = robot.name + orb2enu.header.stamp = rospy.Time.now() + orb2enu.child_frame_id = robot.name + "_orb" + orb2enu.transform.translation.x = 0.0 + orb2enu.transform.translation.y = 0.0 + orb2enu.transform.translation.z = 0.0 + + q = quaternion_from_euler(0, 0, 90*(math.pi/180)) + orb2enu.transform.rotation.x = q[0] + orb2enu.transform.rotation.y = q[1] + orb2enu.transform.rotation.z = q[2] + orb2enu.transform.rotation.w = q[3] + transforms.append(orb2enu) + + static_br.sendTransform(transforms) + ASP.addRobot(robot) + + #Begin Loop + rate = rospy.Rate(30) # 30hz + while not rospy.is_shutdown(): + #TODO: multithreading + ASP.updateTime() + for robot in ASP.robots: + transforms = ASP.updateData(robot) + rate.sleep() + +class Robot: + def __init__(self, name): + self.name = name + self.address = '0.0.0.0' + self.port = '0' + self.sitl = False + self.px4 = [] + self.MAVROS = [] + self.sensors = {} + self.cameras = {} + self.publishers = {} + self.publishers['truepose'] = rospy.Publisher('airsim/' + self.name + "/truepose", PoseStamped, queue_size=10) + + def setAddress(self, address): + self.address = address + + def setPort(self, port): + self.port = port + + def setSITL(self, enable): + if (enable == 'True' or enable == 'true'): + self.sitl = True + + def setPX4(self, subprocess): + self.px4 = subprocess + + def setMAVROS(self, ros_process): + self.MAVROS = ros_process + + def addCameras(self, cameras): + camera_dict = {} + for camera in cameras: + camera_params = cameras[camera]['CaptureSettings'][0] + if ('PublishToRos' in camera_params and camera_params['PublishToRos'] == 1): + camera_dict[camera] = cameras[camera] + self.publishers[camera] = rospy.Publisher('airsim/' + self.name + "/" + camera + "/image_raw", Image, queue_size=30) + self.publishers[camera + "_info"] = rospy.Publisher('airsim/' + self.name + "/" + camera, CameraInfo, queue_size=30) + self.cameras = camera_dict + +class AirSimPublisher: + def __init__(self): + self.robots = [] + self.client = airsim.MultirotorClient() + self.time = rospy.Time.now() + self.bridge = CvBridge() + self.tfBuffer = tf2_ros.Buffer() + listener = tf2_ros.TransformListener(self.tfBuffer) + + def updateTime(self): + self.time = rospy.Time.now() + + def addRobot(self, robot): + self.robots.append(robot) + + def getCameraInfoMsg(self,camera,robot): + cam_params = robot.cameras[camera]['CaptureSettings'][0] + height = cam_params['Height'] + width = cam_params['Width'] + fx = abs((width/2)/math.tan(cam_params['FOV_Degrees']/2*math.pi/180)) + fy = fx + #fy = abs((height/2)/math.tan(cam_params['FOV_Degrees']/2*math.pi/180)) + cx = width/2 + cy = height/2 + cam_msg = CameraInfo() + cam_msg.header.stamp = self.time + cam_msg.header.frame_id = str(camera) + cam_msg.height = height + cam_msg.width = width + cam_msg.distortion_model = "plumb_bob" + cam_msg.D = (0, 0, 0, 0, 0) + cam_msg.K = (fx, 0, cx, 0, fy, cy, 0, 0, 1) + return cam_msg + + def getImageMsg(self,image,camera,robot): + cam_params = robot.cameras[camera]['CaptureSettings'][0] + height = cam_params['Height'] + width = cam_params['Width'] + + img_msg = Image() + img_msg.header.stamp = self.time + img_msg.header.frame_id = str(camera) + img_msg.encoding = "bgr8" + img_msg.height = height + img_msg.width = width + img_msg.data = image + img_msg.is_bigendian = 0 + img_msg.step = img_msg.width * 3 + return img_msg + + def getPoseMsg(self,robot): + pose_data = self.client.simGetVehiclePose(robot.name) + + ned_pose = PoseStamped() + ned_pose.header.stamp = self.time + ned_pose.header.frame_id = robot.name + "_ned" + ned_pose.pose.position = Point(pose_data.position.x_val, pose_data.position.y_val, pose_data.position.z_val) + ned_pose.pose.orientation = Quaternion(pose_data.orientation.x_val, pose_data.orientation.y_val, pose_data.orientation.z_val, pose_data.orientation.w_val) + + #pose_msg = PoseStamped() + #pose_msg = tf2_geometry_msgs.do_transform_pose(ned_pose, t) + + return ned_pose + + def getTransforms(self, settings, robot): + #https://github.com/methylDragon/ros-sensor-fusion-tutorial/blob/master/01%20-%20ROS%20and%20Sensor%20Fusion%20Tutorial.md#3.1 + cameras = settings['Cameras'] + transforms = [] + for camera in cameras: + #gimbal cameras are not going to be static transforms + if not ('Gimbal' in cameras[camera]): + transforms.append(self.staticTransform(cameras[camera], str(camera), robot.name)) + robot_2_map = TransformStamped() + robot_2_map.header.stamp = self.time + robot_2_map.header.frame_id = 'odom' + robot_2_map.child_frame_id = robot.name + robot_2_map.transform.rotation = Quaternion(0,0,0,1) + transforms.append(robot_2_map) + map_2_odom = TransformStamped() + map_2_odom.header.stamp = self.time + map_2_odom.header.frame_id = 'map' + map_2_odom.child_frame_id = 'odom' + map_2_odom.transform.rotation = Quaternion(0,0,0,1) + transforms.append(map_2_odom) + return transforms + + def staticTransform(self, sensor, child, parent): + x = sensor['X'] if ('X' in sensor) else 0 + y = sensor['Y'] if ('Y' in sensor) else 0 + z = sensor['Z'] if ('Z' in sensor) else 0 + roll = -sensor['Roll']*(math.pi/180) if ('Roll' in sensor) else 0 + pitch = -sensor['Pitch']*(math.pi/180) if ('Pitch' in sensor) else 0 + yaw = -sensor['Yaw']*(math.pi/180) if ('Yaw' in sensor) else 0 + + transform = TransformStamped() + transform.header.stamp = self.time + transform.header.frame_id = 'base_link_orb' + transform.child_frame_id = 'camera_link' + print(parent,child) + q = quaternion_from_euler(roll, pitch, yaw) + transform.transform.translation = Point(x,y,z) + transform.transform.rotation.x = q[0] + transform.transform.rotation.y = q[1] + transform.transform.rotation.z = q[2] + transform.transform.rotation.w = q[3] + return transform + + def updateData(self, robot): + img_pub = [] + cam_pub = [] + img_requests = [] + transforms = [] + for camera in robot.cameras: + img_pub.append(robot.publishers[camera]) + cam_pub = robot.publishers[camera + "_info"] + cam_pub.publish(self.getCameraInfoMsg(camera,robot)) + img_type = robot.cameras[camera]['CaptureSettings'][0]['ImageType'] + img_requests.append(airsim.ImageRequest(camera, img_type, False, False)) + #TODO publish gimbal transform + #if ('Gimbal' in robot.cameras[camera]): + + #get images + responses = self.client.simGetImages(img_requests, robot.name) + if len(responses) != len(robot.cameras): + print("Error receiving images from AirSim") + else: + for i in range(len(responses)): + img_pub[i].publish(self.getImageMsg(responses[i].image_data_uint8,robot.cameras.keys()[i],robot)) + #update true pose + pub = robot.publishers['truepose'] + pub.publish(self.getPoseMsg(robot)) + + + diff --git a/auto/python/airsim_helper.pyc b/auto/python/airsim_helper.pyc new file mode 100644 index 0000000000..fc2360d730 Binary files /dev/null and b/auto/python/airsim_helper.pyc differ diff --git a/auto/python/autonomy.py b/auto/python/autonomy.py new file mode 100644 index 0000000000..9d2e7d4db0 --- /dev/null +++ b/auto/python/autonomy.py @@ -0,0 +1,49 @@ +import rospy +import time +from sensor_msgs.msg import Image,CameraInfo +from std_msgs.msg import Float32 +from tf2_msgs.msg import TFMessage +from geometry_msgs.msg import TransformStamped, Vector3, Quaternion, PoseWithCovarianceStamped, PoseStamped +from sensor_msgs.msg import PointCloud2 +from cv_bridge import CvBridge +from tf.transformations import quaternion_from_euler +from airsim2ros.msg import Barometer, Distance +from nav_msgs.msg import Odometry +from multiprocessing import Process + +from tf.transformations import quaternion_from_matrix, rotation_matrix, quaternion_matrix + +from pad_detection import PadDetector +from airsim_helper import AirSimHelper + +import setup_path +import airsim +import cv2 +import time +import numpy as np +import roslaunch +import atexit +import sys +import tf2_ros +import tf2_geometry_msgs +from threading import Thread + +class Autonomy: + + def __init__(self): + + rospy.init_node('autonomy', anonymous=True) + + #Helper function to setup 'True' Drone pose and Video ROS Topics + #Not intended for actual drone use + AirSim = AirSimHelper() + AirSim.daemon = True + AirSim.start() + + rospy.spin() + + +if __name__ == "__main__": + Autonomy() + + diff --git a/auto/python/backup b/auto/python/backup new file mode 100644 index 0000000000..18054a6f64 --- /dev/null +++ b/auto/python/backup @@ -0,0 +1,152 @@ +import rospy +import glob +import json +import math +import os +import px4tools +import sys +import time +import actionlib +import roslaunch +import numpy as np +import tf2_ros +import tf2_geometry_msgs + +from mavros import mavlink +from mavros_msgs.msg import Mavlink, Waypoint, WaypointReached, GlobalPositionTarget, State, TakeoffAction, TakeoffGoal, LandAction, LandGoal, WaypointsAction, WaypointsGoal, HomePosition +from mavros_msgs.srv import CommandBool, SetMode, CommandTOL, WaypointPush, WaypointClear, CommandHome +from sensor_msgs.msg import NavSatFix +from nav_msgs.msg import Odometry +from geometry_msgs.msg import Pose, PoseWithCovariance, PoseStamped, PoseWithCovarianceStamped +from mavros_test_common import MavrosTestCommon +from pymavlink import mavutil + +class companion(): + + def __init__(self): + + #Initialize ROSLaunch + launch = roslaunch.scriptapi.ROSLaunch() + launch.start() + uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) + roslaunch.configure_logging(uuid) + + #Launch MAVROS + cli_args = ('mavros', 'px4.launch') + roslaunch_file = [(roslaunch.rlutil.resolve_launch_arguments(cli_args)[0])] + mavros = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_file) + mavros.start() + + #Launch ORBSLAM + cli_args = ('orb_slam2_ros', 'orb_slam2_mono.launch') + roslaunch_file = [(roslaunch.rlutil.resolve_launch_arguments(cli_args)[0])] + orbslam = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_file) + orbslam.start() + + rospy.init_node('companion_node', anonymous=True) + + self.local_pose = Pose() + self.slam_pose = PoseStamped() + self.raw_slam_pose = PoseStamped() + self.calib_init_slam = Pose() + self.calib_init_local = Pose() + self.slam_calibrated = False + self.slam_offset_x = 0 + self.slam_offset_y = 0 + self.slam_offset_z = 0 + self.slam_scale = 1 + + self.timer = 0 + self.calibration_check_interval = 128 #in px4 freq + + rospy.Subscriber('/mavros/global_position/local', Odometry, self.local_pose_callback) + rospy.Subscriber('/orb_slam2_mono/pose', PoseStamped, self.slamCallback) + self.slam_pub = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=50) + + self.tfBuffer = tf2_ros.Buffer() + listener = tf2_ros.TransformListener(self.tfBuffer) + + rospy.spin() + + #Code to scale and publish orbslam pose to px4 + + def local_pose_callback(self,data): + self.local_pose = data.pose.pose + self.timer + if (self.timer == self.calibration_check_interval): + self.timer = 0 + self.runCalibration() + else: + self.timer += 1 + + def slamCallback(self,data): + pose_converted = self.convertFRD(data) + + if (self.raw_slam_pose.header.stamp.secs < pose_converted.header.stamp.secs - 1) or (self.calib_init_slam.position.x == 0) : + self.calib_init_slam = pose_converted.pose + self.calib_init_local = self.local_pose + #print(self.calib_init_slam.position,self.calib_init_local.position) + self.slam_calibrated = False + + self.raw_slam_pose = pose_converted + if (self.slam_calibrated): + + #print('bfore: ', self.raw_slam_pose.pose.position.x,self.raw_slam_pose.pose.position.y,self.raw_slam_pose.pose.position.z) + #transform SLAM pose + self.slam_pose.pose.position.x = self.raw_slam_pose.pose.position.x * self.slam_scale + self.slam_offset_x + self.slam_pose.pose.position.y = self.raw_slam_pose.pose.position.y * self.slam_scale + self.slam_offset_y + self.slam_pose.pose.position.z = self.raw_slam_pose.pose.position.z * self.slam_scale + self.slam_offset_z + + self.slam_pose.pose.orientation = self.raw_slam_pose.pose.orientation + #print('after: ', self.slam_pose.pose.position.x,self.slam_pose.pose.position.y,self.slam_pose.pose.position.z) + + self.slam_pose.header.stamp = self.raw_slam_pose.header.stamp + self.slam_pose.header.frame_id = 'Drone1' + + self.slam_pub.publish(self.slam_pose) + else: + self.runCalibration() + + def runCalibration(self): + #check for sufficient calibration movement + #TODO Check data timeliness + x = self.local_pose.position.x - self.calib_init_local.position.x + y = self.local_pose.position.y - self.calib_init_local.position.y + x_slam = self.raw_slam_pose.pose.position.x - self.calib_init_slam.position.x + y_slam = self.raw_slam_pose.pose.position.y - self.calib_init_slam.position.y + if (math.sqrt(x*x+y*y) > 5.0): + self.slam_scale = math.sqrt(x*x+y*y)/math.sqrt(x_slam*x_slam+y_slam*y_slam) + #print('scale', self.slam_scale) + self.slam_offset_x = (self.calib_init_local.position.x-self.slam_scale*self.calib_init_slam.position.x) + self.slam_offset_y = (self.calib_init_local.position.y-self.slam_scale*self.calib_init_slam.position.y) + self.slam_offset_z = (self.calib_init_local.position.z-self.slam_scale*self.calib_init_slam.position.z) + self.slam_calibrated = True + + def convertFRD(self, data): + + #apply gimbal rotation (this is (mostly) working Dec 15th) + try: + transform = self.tfBuffer.lookup_transform('main','Drone1',rospy.Time(0),rospy.Duration(1.0)) + data = tf2_geometry_msgs.do_transform_pose(data, transform) + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): + print('Error loading gimbal transform') + + #convert to px4 coord system (this is working Dec 15th) + converted = PoseStamped() + converted.header = data.header + converted.pose.position.x = -data.pose.position.y + converted.pose.position.y = data.pose.position.x + converted.pose.position.z = data.pose.position.z + + converted.pose.orientation.x = -data.pose.orientation.y + converted.pose.orientation.y = data.pose.orientation.x + converted.pose.orientation.z = data.pose.orientation.z + converted.pose.orientation.w = data.pose.orientation.w + #print(converted.pose.orientation) + + return converted + + +if __name__ == '__main__': + companion() + diff --git a/auto/python/companion.py b/auto/python/companion.py new file mode 100644 index 0000000000..0975908828 --- /dev/null +++ b/auto/python/companion.py @@ -0,0 +1,231 @@ +import rospy +import glob +import json +import math +import os +import px4tools +import sys +import time +import actionlib +import roslaunch +import numpy as np +import tf2_ros +import tf2_geometry_msgs +import cv2 +import shutil + +from cv_bridge import CvBridge, CvBridgeError + +from mavros import mavlink +from mavros_msgs.msg import Mavlink, Waypoint, WaypointReached, GlobalPositionTarget, State, TakeoffAction, TakeoffGoal, LandAction, LandGoal, WaypointsAction, WaypointsGoal, HomePosition +from mavros_msgs.srv import CommandBool, SetMode, CommandTOL, WaypointPush, WaypointClear, CommandHome +from orb_slam2_ros.msg import KeyFrames, Observations +from sensor_msgs.msg import NavSatFix, Image, PointCloud2 +from std_msgs.msg import Header +from nav_msgs.msg import Odometry +from geometry_msgs.msg import Pose, PoseWithCovariance, PoseStamped, PoseWithCovarianceStamped +from mavros_test_common import MavrosTestCommon +from pymavlink import mavutil + +class KeyFrame(object): + empty = [] + + def __init__(self): + self.image_id = "" + self.latitude = 0.0 + self.longitude = 0.0 + self.altitude = 0.0 + self.orientation = [0.0,0.0,0.0,0.0] + self.image = "" + +class companion(): + + def __init__(self): + + #Initialize ROSLaunch + launch = roslaunch.scriptapi.ROSLaunch() + launch.start() + uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) + roslaunch.configure_logging(uuid) + + #Launch MAVROS + cli_args = ('mavros', 'px4.launch') + roslaunch_file = [(roslaunch.rlutil.resolve_launch_arguments(cli_args)[0])] + mavros = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_file) + mavros.start() + + #Launch ORBSLAM + cli_args = ('orb_slam2_ros', 'orb_slam2_mono.launch') + roslaunch_file = [(roslaunch.rlutil.resolve_launch_arguments(cli_args)[0])] + orbslam = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_file) + orbslam.start() + + rospy.init_node('companion_node', anonymous=True) + + #clear old images + shutil.rmtree('img') + os.mkdir('img') + + self.local_pose = Pose() + self.slam_pose = PoseStamped() + self.raw_slam_pose = PoseStamped() + self.calib_init_slam = Pose() + self.calib_init_local = Pose() + self.slam_calibrated = False + self.slam_offset_x = 0 + self.slam_offset_y = 0 + self.slam_offset_z = 0 + self.slam_scale = 1 + self.state = "SYSTEM_NOT_READY" + self.last_state = "SYSTEM_NOT_READY" + + #States: + #"SYSTEM_NOT_READY" + #"NO_IMAGES_YET" + #"NOT_INITIALIZED" + #"OK" + #"LOST" + + self.keyframes = [] + self.image_buffer = [] + self.bridge = CvBridge() + + rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.local_pose_callback) + rospy.Subscriber('/orb_slam2_mono/pose', PoseStamped, self.slamCallback) + rospy.Subscriber('/orb_slam2_mono/state', Header, self.stateCallback) + rospy.Subscriber('/orb_slam2_mono/map_points', PointCloud2, self.pointcloudCallback) + rospy.Subscriber('/orb_slam2_mono/observations', Observations, self.observationsCallback) + rospy.Subscriber('/orb_slam2_mono/keyframes', KeyFrames, self.keyframesCallback) + rospy.Subscriber('/airsim/base_link/camera/image_raw', Image, self.imageBuffer) + #rospy.Subscriber('/orb_slam2_mono/keyframes', KeyFrames, self.keyframesCallback) + self.slam_pub = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=50) + + rospy.spin() + + #Code to scale and publish orbslam pose to px4 + + def local_pose_callback(self,data): + #TODO Ensure this data is up to date!! + self.local_pose = data.pose + + def stateCallback(self,data): + #Check for state change + self.state = data.frame_id + if (self.last_state != self.state): + self.slam_calibrated = False + if (self.state == "OK"): + self.calib_init_slam = self.raw_slam_pose.pose + self.calib_init_local = self.local_pose + self.last_state = self.state + + def slamCallback(self,data): + + self.raw_slam_pose = self.convertFRD(data) + self.runCalibration() + if (self.slam_calibrated): + #transform SLAM pose + self.slam_pose.pose.position.x = self.raw_slam_pose.pose.position.x * self.slam_scale + self.slam_offset_x + self.slam_pose.pose.position.y = self.raw_slam_pose.pose.position.y * self.slam_scale + self.slam_offset_y + self.slam_pose.pose.position.z = self.raw_slam_pose.pose.position.z * self.slam_scale + self.slam_offset_z + self.slam_pose.pose.orientation = self.raw_slam_pose.pose.orientation + self.slam_pose.header = self.raw_slam_pose.header + + self.slam_pub.publish(self.slam_pose) + #else: + #if (self.state == "OK"): + + + + def runCalibration(self): + #check for sufficient calibration movement + x = self.local_pose.position.x - self.calib_init_local.position.x + y = self.local_pose.position.y - self.calib_init_local.position.y + x_slam = self.raw_slam_pose.pose.position.x - self.calib_init_slam.position.x + y_slam = self.raw_slam_pose.pose.position.y - self.calib_init_slam.position.y + if (math.sqrt(x*x+y*y) > 5.0 and x_slam != 0): + self.slam_scale = math.sqrt(x*x+y*y)/math.sqrt(x_slam*x_slam+y_slam*y_slam) + self.slam_offset_x = (self.calib_init_local.position.x-self.slam_scale*self.calib_init_slam.position.x) + self.slam_offset_y = (self.calib_init_local.position.y-self.slam_scale*self.calib_init_slam.position.y) + self.slam_offset_z = (self.calib_init_local.position.z-self.slam_scale*self.calib_init_slam.position.z) + #print('offset', self.slam_offset_x, self.slam_offset_y, self.slam_offset_z) + self.slam_calibrated = True + + def convertFRD(self, data): + + #convert to px4 coord system (this is working Dec 15th) + converted = PoseStamped() + converted.header.frame_id = 'map' + converted.pose.position.x = -data.pose.position.y + converted.pose.position.y = data.pose.position.z + converted.pose.position.z = -data.pose.position.x + + converted.pose.orientation.x = -data.pose.orientation.y + converted.pose.orientation.y = data.pose.orientation.z + converted.pose.orientation.z = -data.pose.orientation.x + converted.pose.orientation.w = data.pose.orientation.w + + return data + + def imageBuffer(self, data): + buffer_size = 50 + if (len(self.image_buffer) > buffer_size): + self.image_buffer.pop() + self.image_buffer.insert(0,data) + + def keyframesCallback(self,data): + #TODO not currently capturing all keyframes + #print(len(data.keyframes),len(self.keyframes)) + #check for new keyframes + data = list(data.keyframes) + if (len(self.keyframes) == 0): + for kf in data: + self.addKeyFrame(kf) + else: + #check for removed keyframes + for old_kf in self.keyframes: + found = False + for new_kf in data: + if (old_kf.image_id == new_kf.header.stamp.to_sec()): + found = True + break + #print(found) + if (not found): + self.removeKeyFrame(old_kf) + #Add new kfs + latest = self.keyframes[-1].image_id + for new_kf in data: + if (new_kf.header.stamp.to_sec() > latest): + self.addKeyFrame(new_kf) + + def removeKeyFrame(self, data): + #print('remove frame '+data.image) + self.keyframes.remove(data) + if os.path.exists(data.image): + os.remove(data.image) + + def addKeyFrame(self, data): + kf = KeyFrame() + found = False + for img in self.image_buffer: + #print(img.header.stamp, data.header.stamp) + if (img.header.stamp.to_sec() == data.header.stamp.to_sec()): + #print('add frame') + kf.image_id = data.header.stamp.to_sec() + cv_img = self.bridge.imgmsg_to_cv2(img, "passthrough") + kf.image = 'img/'+str(kf.image_id)+'.png' + cv2.imwrite(kf.image, cv_img) + self.keyframes.append(kf) + found = True + return + print('Image not found, consider increasing image buffer size') + + def pointcloudCallback(self, data): + self.pointcloud = data + + def observationsCallback(self, data): + data = list(data.observations) + #print(self.pointcloud.width, len(data)) + + +if __name__ == '__main__': + companion() diff --git a/auto/python/companion.pyc b/auto/python/companion.pyc new file mode 100644 index 0000000000..a4bd2f35d8 Binary files /dev/null and b/auto/python/companion.pyc differ diff --git a/auto/python/companion2.py b/auto/python/companion2.py new file mode 100644 index 0000000000..8b5b39e6a2 --- /dev/null +++ b/auto/python/companion2.py @@ -0,0 +1,158 @@ +import rospy +import glob +import json +import math +import os +import px4tools +import sys +import time +import actionlib +import roslaunch +import numpy as np +import tf2_ros +import tf2_geometry_msgs + +from mavros import mavlink +from mavros_msgs.msg import Mavlink, Waypoint, WaypointReached, GlobalPositionTarget, State, TakeoffAction, TakeoffGoal, LandAction, LandGoal, WaypointsAction, WaypointsGoal, HomePosition +from mavros_msgs.srv import CommandBool, SetMode, CommandTOL, WaypointPush, WaypointClear, CommandHome +from sensor_msgs.msg import NavSatFix +from nav_msgs.msg import Odometry +from geometry_msgs.msg import Pose, PoseWithCovariance, PoseStamped, PoseWithCovarianceStamped +from mavros_test_common import MavrosTestCommon +from pymavlink import mavutil + +class companion(): + + def __init__(self): + + #Initialize ROSLaunch + launch = roslaunch.scriptapi.ROSLaunch() + launch.start() + uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) + roslaunch.configure_logging(uuid) + + #Launch MAVROS + cli_args = ('mavros', 'px4.launch') + roslaunch_file = [(roslaunch.rlutil.resolve_launch_arguments(cli_args)[0])] + mavros = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_file) + mavros.start() + + #Launch ORBSLAM + cli_args = ('orb_slam2_ros', 'orb_slam2_mono.launch') + roslaunch_file = [(roslaunch.rlutil.resolve_launch_arguments(cli_args)[0])] + orbslam = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_file) + orbslam.start() + + rospy.init_node('companion_node', anonymous=True) + + self.local_pose = Pose() + self.slam_pose = PoseStamped() + self.raw_slam_pose = PoseStamped() + self.calib_init_slam = Pose() + self.calib_init_local = Pose() + self.slam_calibrated = False + self.slam_offset_x = 0 + self.slam_offset_y = 0 + self.slam_offset_z = 0 + self.slam_scale = 1 + + self.timer = 0 + self.calibration_check_interval = 128 #in px4 freq + + rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.local_pose_callback) + rospy.Subscriber('/orb_slam2_mono/pose', PoseStamped, self.slamCallback) + self.slam_pub = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=50) + + self.tfBuffer = tf2_ros.Buffer() + listener = tf2_ros.TransformListener(self.tfBuffer) + + self.cam_t = self.tfBuffer.lookup_transform('base_link','camera_pose',rospy.Time(0),rospy.Duration(1.0)) + + rospy.spin() + + #Code to scale and publish orbslam pose to px4 + + def local_pose_callback(self,data): + self.local_pose = data.pose + self.timer + if (self.timer == self.calibration_check_interval): + self.timer = 0 + self.runCalibration() + else: + self.timer += 1 + + def slamCallback(self,data): + pose_converted = self.convertFRD(data) + + if (self.raw_slam_pose.header.stamp.secs < pose_converted.header.stamp.secs - 1) or (self.calib_init_slam.position.x == 0) : + self.calib_init_slam = pose_converted.pose + self.calib_init_local = self.local_pose + print(self.calib_init_slam.position,self.calib_init_local.position) + self.slam_calibrated = False + + self.raw_slam_pose = pose_converted + if (self.slam_calibrated): + + #print('bfore: ', self.raw_slam_pose.pose.position.x,self.raw_slam_pose.pose.position.y,self.raw_slam_pose.pose.position.z) + #transform SLAM pose + self.slam_pose.pose.position.x = self.raw_slam_pose.pose.position.x * self.slam_scale + self.slam_offset_x + self.cam_t.transform.translation.x + self.slam_pose.pose.position.y = self.raw_slam_pose.pose.position.y * self.slam_scale + self.slam_offset_y + self.cam_t.transform.translation.y + self.slam_pose.pose.position.z = self.raw_slam_pose.pose.position.z * self.slam_scale + self.slam_offset_z + self.cam_t.transform.translation.z + + self.slam_pose.pose.orientation = self.raw_slam_pose.pose.orientation + + self.slam_pose.header = self.raw_slam_pose.header + + self.slam_pub.publish(self.slam_pose) + else: + self.runCalibration() + + def runCalibration(self): + #check for sufficient calibration movement + #TODO Check data timeliness + x = self.local_pose.position.x - self.calib_init_local.position.x + y = self.local_pose.position.y - self.calib_init_local.position.y + x_slam = self.raw_slam_pose.pose.position.x - self.calib_init_slam.position.x + y_slam = self.raw_slam_pose.pose.position.y - self.calib_init_slam.position.y + if (math.sqrt(x*x+y*y) > 5.0): + self.slam_scale = math.sqrt(x*x+y*y)/math.sqrt(x_slam*x_slam+y_slam*y_slam) + print('scale', self.slam_scale) + self.slam_offset_x = (self.calib_init_local.position.x-self.slam_scale*self.calib_init_slam.position.x) + self.slam_offset_y = (self.calib_init_local.position.y-self.slam_scale*self.calib_init_slam.position.y) + self.slam_offset_z = (self.calib_init_local.position.z-self.slam_scale*self.calib_init_slam.position.z) + self.slam_calibrated = True + + def convertFRD(self, data): + + temp = data + #data.pose.position.x = temp.pose.position.z + #data.pose.position.y = temp.pose.position.x + #data.pose.position.z = temp.pose.position.y + + #apply transform from camera pose to robot frame + transform = self.cam_t + transform.transform.translation.x = 0.0 + transform.transform.translation.y = 0.0 + transform.transform.translation.z = 0.0 + data = tf2_geometry_msgs.do_transform_pose(data, transform) + + #convert to px4 coord system (this is working Dec 15th) + converted = PoseStamped() + converted.header.frame_id = 'base_link' + converted.pose.position.x = -data.pose.position.y + converted.pose.position.y = data.pose.position.x + converted.pose.position.z = data.pose.position.z + + converted.pose.orientation.x = -data.pose.orientation.y + converted.pose.orientation.y = data.pose.orientation.x + converted.pose.orientation.z = data.pose.orientation.z + converted.pose.orientation.w = data.pose.orientation.w + #print(converted.pose.orientation) + + #return data + return converted + + +if __name__ == '__main__': + companion() + diff --git a/auto/python/disable_gps.py b/auto/python/disable_gps.py new file mode 100644 index 0000000000..6597602360 --- /dev/null +++ b/auto/python/disable_gps.py @@ -0,0 +1,21 @@ +import rospy +import px4tools + +from mavros import mavlink +from mavros_msgs.msg import Mavlink, Waypoint, WaypointReached, GlobalPositionTarget, State, TakeoffAction, TakeoffGoal, LandAction, LandGoal, WaypointsAction, WaypointsGoal, HomePosition, ParamValue +from mavros_msgs.srv import CommandBool, SetMode, CommandTOL, WaypointPush, WaypointClear, CommandHome, ParamSet + + +rospy.init_node('disable_gps', anonymous=True) + +rospy.wait_for_service('/mavros/param/set') +print('connected to ParamSet Service') +param_set = rospy.ServiceProxy('/mavros/param/set', ParamSet) + +param = ParamValue() +param_set + + +rospy.spin() + + diff --git a/auto/python/frames.gv b/auto/python/frames.gv new file mode 100644 index 0000000000..43a84fa2c5 --- /dev/null +++ b/auto/python/frames.gv @@ -0,0 +1,21 @@ +digraph G { +"map" -> "map_ned"[label="Broadcaster: /mavros\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1608106089.618 sec old)\nBuffer length: 0.000 sec\n"]; +"odom" -> "odom_ned"[label="Broadcaster: /mavros\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1608106089.618 sec old)\nBuffer length: 0.000 sec\n"]; +"base_link" -> "base_link_frd"[label="Broadcaster: /mavros\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1608106089.618 sec old)\nBuffer length: 0.000 sec\n"]; +"map" -> "base_link"[label="Broadcaster: /autonomy_11715_1608104044344\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1608106089.618 sec old)\nBuffer length: 0.000 sec\n"]; +"Drone1" -> "color"[label="Broadcaster: /autonomy_11715_1608104044344\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1608106089.618 sec old)\nBuffer length: 0.000 sec\n"]; +"Drone1" -> "depth"[label="Broadcaster: /autonomy_11715_1608104044344\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1608106089.618 sec old)\nBuffer length: 0.000 sec\n"]; +"base_link" -> "main"[label="Broadcaster: /autonomy_11715_1608104044344\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1608106089.618 sec old)\nBuffer length: 0.000 sec\n"]; +edge [style=invis]; + subgraph cluster_legend { style=bold; color=black; label ="view_frames Result"; +"Recorded at time: 1608106089.618"[ shape=plaintext ] ; + }->"map"; +edge [style=invis]; + subgraph cluster_legend { style=bold; color=black; label ="view_frames Result"; +"Recorded at time: 1608106089.618"[ shape=plaintext ] ; + }->"odom"; +edge [style=invis]; + subgraph cluster_legend { style=bold; color=black; label ="view_frames Result"; +"Recorded at time: 1608106089.618"[ shape=plaintext ] ; + }->"Drone1"; +} \ No newline at end of file diff --git a/auto/python/frames.pdf b/auto/python/frames.pdf new file mode 100644 index 0000000000..5b41792d0d Binary files /dev/null and b/auto/python/frames.pdf differ diff --git a/auto/python/ground.py b/auto/python/ground.py new file mode 100644 index 0000000000..5b59d6e78b --- /dev/null +++ b/auto/python/ground.py @@ -0,0 +1,112 @@ +import rospy +import glob +import json +import math +import os +import px4tools +import sys +import time +import actionlib +import roslaunch +from mavros import mavlink +from mavros import action_server2 +from mavros_msgs.msg import Mavlink, Waypoint, WaypointReached, GlobalPositionTarget, State, TakeoffAction, TakeoffGoal, LandAction, LandGoal, WaypointsAction, WaypointsGoal, HomePosition +from mavros_msgs.srv import CommandBool, SetMode, CommandTOL, WaypointPush, WaypointClear, CommandHome +from sensor_msgs.msg import NavSatFix +from geometry_msgs.msg import PoseStamped +from mavros_test_common import MavrosTestCommon +from pymavlink import mavutil +from threading import Thread + +# Brings in the SimpleActionClient +import actionlib + +class ground_station(): + + state_sub = rospy.Subscriber('mavros/state', State, self.state_callback) + self.state = State + #PUBLISHERS + local_pos_pub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10) + #global_pos_pub = rospy.Publisher('mavros/setpoint_position/global', GlobalPositionTarget, queue_size=10) + local_pos_sub = rospy.Subscriber('/mavros/global_position/global', NavSatFix, self.global_pos_callback) + home_pos_sub = rospy.Subscriber('/mavros/home_position/home', HomePosition, self.home_pos_callback) + + #ACTIONS + #init actionlib servers + server = Thread(target=action_server2.ActionServer) + server.setDaemon(True) + server.start() + takeoff_client = actionlib.SimpleActionClient('takeoff', TakeoffAction) + land_client = actionlib.SimpleActionClient('land', LandAction) + waypoints_client = actionlib.SimpleActionClient('waypoints', WaypointsAction) + + # need to simulate heartbeat to prevent datalink loss detection + hb_mav_msg = mavutil.mavlink.MAVLink_heartbeat_message(mavutil.mavlink.MAV_TYPE_GCS, 0, 0, 0, 0, 0) + hb_mav_msg.pack(mavutil.mavlink.MAVLink('', 2, 1)) + hb_ros_msg = mavlink.convert_to_rosmsg(hb_mav_msg) + hb_thread = Thread(target=self.send_heartbeat, args=(hb_ros_msg)) + hb_thread.setDaemon(True) + + #PREFLIGHT CHECK + + rate = rospy.Rate(30) + while (not self.state.connected): + print('Waiting on Connection') + rate.sleep() + print('Connected') + + time.sleep(5) + + goal = TakeoffGoal() + goal.height = 10 + + print('Actionlib started') + + takeoff_client.send_goal(goal) + takeoff_client.wait_for_result() + + wps = [] + wp1 = Waypoint() + wp2 = Waypoint() + wp1.command = 16 + wp1.x_lat = self.home_pos.geo.latitude - 0.00050 + wp1.y_long = self.home_pos.geo.longitude + wp1.z_alt = 10 + wp1.autocontinue = True + + wp2.command = 16 + wp2.x_lat = self.home_pos.geo.latitude + wp2.y_long = self.home_pos.geo.longitude + wp2.z_alt = 10 + wp2.autocontinue = True + + goal = WaypointsGoal() + goal.waypoints.append(wp1) + goal.waypoints.append(wp2) + print(goal) + waypoints_client.send_goal(goal) + waypoints_client.wait_for_result(rospy.Duration.from_sec(45.0)) + + time.sleep(5) + + goal = LandGoal() + goal.x_lat = self.home_pos.geo.latitude + goal.y_long = self.home_pos.geo.longitude + goal.z_alt = 0.0 + + print('Actionlib started') + + land_client.send_goal(goal) + land_client.wait_for_result(rospy.Duration.from_sec(30.0)) + sys.exit() + + + # Heartbeat must be sent to px4 at 2Hz or else auto disconnect + def send_heartbeat(self, hb_ros_msg): + rate = rospy.Rate(2) # Hz + while not rospy.is_shutdown(): + self.mavlink_pub.publish(hb_ros_msg) + try: # prevent garbage in console output when thread is killed + rate.sleep() + except rospy.ROSInterruptException: + pass diff --git a/auto/python/mapping.py b/auto/python/mapping.py new file mode 100644 index 0000000000..aafa515302 --- /dev/null +++ b/auto/python/mapping.py @@ -0,0 +1,227 @@ +import rospy +import tf2_ros +import tf2_geometry_msgs +import cv2 +import numpy as np +import math +import struct + +from cv_bridge import CvBridge +from sensor_msgs.msg import Image, CameraInfo, PointCloud2, PointField +from sensor_msgs import point_cloud2 +from tf2_msgs.msg import TFMessage +from geometry_msgs.msg import PoseStamped, Quaternion, TransformStamped +from nav_msgs.msg import Odometry + +from sfm import getColors +from sfm import triangulate +from sfm import triangulate_int +from sfm import drawTracks +from sfm import getTrackLength +from sfm import getObjectPointsEssential +from sfm import eliminateDuplicateObjects +from sfm import baFun +from sfm import bundle_adjustment_sparsity + +from scipy.sparse import lil_matrix +from scipy.optimize import least_squares + +from tf.transformations import quaternion_matrix, euler_from_quaternion, quaternion_multiply + +class mapping(): + + def __init__(self): + + rospy.init_node('mapping', anonymous=True) + + self.bridge = CvBridge() + self.tfBuffer = tf2_ros.Buffer() + listener = tf2_ros.TransformListener(self.tfBuffer) + + self.image = [] + self.pose = PoseStamped() + self.K = [] + self.d = [] + self.cam_width = [] + self.cam_height = [] + self.rotation = [] + self.translation = [] + self.tracking = False + + self.img_curr = [] + self.img_prev = [] + + self.features_orig = [] + self.features_prev = [] + + self.obj_mask = [] + + self.reset = True + + #INITIALIZE FEATURE MATCHING PARAMETERS# + self.maxCorners = 500 #Maximum number of corners to return. If there are more corners than are found, the strongest of them is returned + self.qualityLevel = 0.01 #For example, if best corner has measure = 1500, and qualityLevel=0.01 , then corners with quality<15 are rejected. + self.minDistance = 10 #Minimum possible Euclidean distance between the returned corners. + self.lk_params = dict(winSize = (15,15), maxLevel = 2, criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03)) + + #rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.poseCallback) + rospy.Subscriber('orb_slam2_mono/pose', PoseStamped, self.poseCallback) + rospy.Subscriber('/airsim/base_link/camera/image_raw', Image, self.imageCallback) + rospy.Subscriber('/airsim/base_link/camera', CameraInfo, self.camInfoCallback) + self.cloud_pub = rospy.Publisher("cloud", PointCloud2, queue_size=10) + self.test_pose = rospy.Publisher("test_pose", PoseStamped, queue_size=10) + + print('waiting on topics...') + rospy.wait_for_message('/airsim/base_link/camera', Image) + #self.cam_width = self.img_curr.shape[0] + #self.cam_height = self.img_curr.shape[1] + print('K: ', self.K) + print('D: ', self.D) + rospy.wait_for_message('/mavros/local_position/pose', PoseStamped) + print('connected') + + rospy.spin() + + def poseCallback(self, data): + pose_stamped = PoseStamped() + pose_stamped.pose = data.pose + pose_stamped.header = data.header + self.pose.header = pose_stamped.header + self.pose = data + try: + t = self.tfBuffer.lookup_transform('camera_pose','base_link',rospy.Time(0),rospy.Duration(1.0)) + #self.pose = tf2_geometry_msgs.do_transform_pose(self.pose, t) + #self.pose.pose.position.x = pose_stamped.pose.position.x + t.transform.translation.x + #self.pose.pose.position.y = pose_stamped.pose.position.y + t.transform.translation.y + #self.pose.pose.position.z = pose_stamped.pose.position.z + t.transform.translation.z + #q0 = (pose_stamped.pose.orientation.x,pose_stamped.pose.orientation.y,pose_stamped.pose.orientation.z,pose_stamped.pose.orientation.w) + #q1 = (self.pose.pose.orientation.x,self.pose.pose.orientation.y,self.pose.pose.orientation.z,self.pose.pose.orientation.w) + #q = quaternion_multiply(q1,q0) + #self.pose.pose.orientation.x = q[0] + #self.pose.pose.orientation.y = q[1] + #self.pose.pose.orientation.z = q[2] + #self.pose.pose.orientation.w = q[3] + self.test_pose.publish(self.pose) + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): + print('Error loading gimbal transform') + q = (self.pose.pose.orientation.x,self.pose.pose.orientation.y,self.pose.pose.orientation.z,self.pose.pose.orientation.w) + r = quaternion_matrix(q) + self.rotation = r[0:3,0:3] + self.translation = np.array(([self.pose.pose.position.x],[self.pose.pose.position.y],[self.pose.pose.position.z])) + #print(self.translation) + + def imageCallback(self, data): + self.header = data.header + self.img_curr = self.bridge.imgmsg_to_cv2(data, desired_encoding='passthrough') + + P1 = np.hstack((self.rotation,self.translation)) + + grey_curr = cv2.cvtColor(self.img_curr, cv2.COLOR_RGB2GRAY) + + #check if we are currently tracking flow, if no, we need to initialize and wait for next image + if (self.reset or ): + #Get first set of features to track + self.features_orig = cv2.goodFeaturesToTrack(grey_curr, self.maxCorners, self.qualityLevel, self.minDistance) + self.features_prev = self.features_orig + self.P0 = P1 + self.obj_mask = np.full((len(self.features_orig), 4),-1) + self.reset = False + self.img_prev = self.img_curr + return + + grey_prev = cv2.cvtColor(self.img_prev, cv2.COLOR_RGB2GRAY) + + #GET NEW FEATURE LOCATIONS# + next, status, error = cv2.calcOpticalFlowPyrLK(grey_prev, grey_curr , self.features_prev, None, **self.lk_params) + if (not next.any()): + self.reset = True + return + + #Filter out the points with high error + error = error[status[:,0] == 1] + new = next[status[:,0] == 1] + new = new[error[:,0] < 10] + + #Update the original list of features to reflect pixels that have been lost in the flow + self.features_orig = self.features_orig[status[:,0] == 1] + self.features_orig = self.features_orig[error[:,0] < 10] + + # Updates previous good feature points + self.features_prev = new.reshape(-1, 1, 2) + + #Optional visualization + output = drawTracks(self.features_orig.astype(int), new, self.img_curr, (0, 255, 0)) + cv2.imshow('Tracks',output) + self.img_prev = self.img_curr + key = cv2.waitKey(1) & 0xFF; + + #Check sufficient parallax# + avg_track_len = getTrackLength(self.features_orig,new) + if (avg_track_len < 30): + #not enough parrallax + return + + #Attempt to calculate SFM + orig_pts = self.features_orig.reshape(len(self.features_orig),2) + + new_pts = new.reshape(len(new),2) + mask_inf = np.zeros(len(new_pts)) + + #TODO filter out points with low parallax + + print(len(orig_pts)) + #Check quality of pts + if (len(orig_pts) < 10): + #Not enough quality points, reset + self.reset = True + return + + #SolvePNP + #Get 3d world points and associated pixel values for the second image + print(np.reshape(self.P0[:,3],(1,3)),np.reshape(P1[:,2],(1,3))) + print(P1,(self.pose.pose.position.x,self.pose.pose.position.y,self.pose.pose.position.z)) + obj_pts, prev_pts, new_pts, P = getObjectPointsEssential(orig_pts,new_pts,self.P0,self.K, self.D) + + #obj_pts, mask_tri, error = triangulate(orig_pts, self.P0, new_pts, P1, self.K, self.D) + + #Get colors from detected pixels for coloring pointcloud + + #pixel_pts = new_pts[mask_tri[:,0]==1] + colors = getColors(new_pts,self.img_curr.copy()) + + self.publishPoints(obj_pts, colors) + self.reset = True + + return + + def camInfoCallback(self,data): + self.K = np.reshape(data.K,(3,3)) + self.D = np.reshape(data.D,(1,5)) + + def publishPoints(self, obj_pts, colors): + points = [] + for i in range(len(obj_pts)): + x = obj_pts[i,0] + y = obj_pts[i,1] + z = obj_pts[i,2] + #print(obj_pts[i]) + r = colors[i,0] + g = colors[i,1] + b = colors[i,2] + a = 255 + rgb = struct.unpack('I', struct.pack('BBBB', b, g, r, a))[0] + pt = [x, y, z, rgb] + points.append(pt) + fields = [PointField('x', 0, PointField.FLOAT32, 1), + PointField('y', 4, PointField.FLOAT32, 1), + PointField('z', 8, PointField.FLOAT32, 1), + PointField('rgba', 12, PointField.UINT32, 1), + ] + #Raytracing (clearing of free space) is always done from the origin of this frame_id at the time of the point cloud + header = self.header + header.frame_id = 'map' + cloud = point_cloud2.create_cloud(header, fields, points) + self.cloud_pub.publish(cloud) + +if __name__ == '__main__': + mapping() diff --git a/auto/python/mapping2.py b/auto/python/mapping2.py new file mode 100644 index 0000000000..7d1adb64f9 --- /dev/null +++ b/auto/python/mapping2.py @@ -0,0 +1,229 @@ +import rospy +import tf2_ros +import tf2_geometry_msgs +import cv2 +import numpy as np +import math +import struct + +from cv_bridge import CvBridge +from sensor_msgs.msg import Image, CameraInfo, PointCloud2, PointField +from sensor_msgs import point_cloud2 +from tf2_msgs.msg import TFMessage +from geometry_msgs.msg import PoseStamped, Quaternion, TransformStamped +from nav_msgs.msg import Odometry + +from sfm import getColors +from sfm import triangulate +from sfm import triangulate_int +from sfm import drawTracks +from sfm import getTrackLength +from sfm import getObjectPointsEssential +from sfm import eliminateDuplicateObjects +from sfm import baFun +from sfm import bundle_adjustment_sparsity + +from scipy.sparse import lil_matrix +from scipy.optimize import least_squares + +from tf.transformations import quaternion_matrix, euler_from_quaternion, quaternion_multiply, quaternion_from_matrix + +class mapping(): + + def __init__(self): + + rospy.init_node('mapping', anonymous=True) + + self.bridge = CvBridge() + self.tfBuffer = tf2_ros.Buffer() + listener = tf2_ros.TransformListener(self.tfBuffer) + + self.image = [] + self.pose = PoseStamped() + self.K = [] + self.d = [] + self.cam_width = [] + self.cam_height = [] + self.rotation = [] + self.translation = [] + self.tracking = False + + self.img_curr = [] + self.img_prev = [] + + self.features_orig = [] + self.features_prev = [] + + self.obj_mask = [] + + self.reset = True + + #INITIALIZE FEATURE MATCHING PARAMETERS# + self.maxCorners = 500 #Maximum number of corners to return. If there are more corners than are found, the strongest of them is returned + self.qualityLevel = 0.01 #For example, if best corner has measure = 1500, and qualityLevel=0.01 , then corners with quality<15 are rejected. + self.minDistance = 10 #Minimum possible Euclidean distance between the returned corners. + self.lk_params = dict(winSize = (15,15), maxLevel = 2, criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03)) + + #rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.poseCallback) + rospy.Subscriber('orb_slam2_mono/pose', PoseStamped, self.poseCallback) + rospy.Subscriber('/airsim/base_link/camera/image_raw', Image, self.imageCallback) + rospy.Subscriber('/airsim/base_link/camera', CameraInfo, self.camInfoCallback) + self.cloud_pub = rospy.Publisher("cloud", PointCloud2, queue_size=10) + self.test_pose = rospy.Publisher("test_pose", PoseStamped, queue_size=10) + + print('waiting on topics...') + rospy.wait_for_message('/airsim/base_link/camera/image_raw', Image) + + #self.cam_width = self.img_curr.shape[0] + #self.cam_height = self.img_curr.shape[1] + print('K: ', self.K) + print('D: ', self.D) + rospy.wait_for_message('/mavros/local_position/pose', PoseStamped) + print('connected') + print(float(self.pose.header.stamp.secs)+float(self.pose.header.stamp.nsecs)/1000000000.0,float(self.header.stamp.secs)+float(self.header.stamp.nsecs)/1000000000.0) + rospy.spin() + + def poseCallback(self, data): + self.pose = data + #self.test_pose.publish(self.pose) + q = (self.pose.pose.orientation.x,self.pose.pose.orientation.y,self.pose.pose.orientation.z,self.pose.pose.orientation.w) + r = quaternion_matrix(q) + #print(r) + self.rotation = r[0:3,0:3] + self.translation = np.array(([self.pose.pose.position.x],[self.pose.pose.position.y],[self.pose.pose.position.z])) + #print(self.translation) + + def imageCallback(self, data): + self.header = data.header + self.img_curr = self.bridge.imgmsg_to_cv2(data, desired_encoding='passthrough') + + P1 = np.hstack((self.rotation,self.translation)) + + + grey_curr = cv2.cvtColor(self.img_curr, cv2.COLOR_RGB2GRAY) + + #check if we are currently tracking flow, if no, we need to initialize and wait for next image + if (self.reset and P1 != []): + #Get first set of features to track + self.features_orig = cv2.goodFeaturesToTrack(grey_curr, self.maxCorners, self.qualityLevel, self.minDistance) + self.features_prev = self.features_orig + self.P0 = P1 + self.obj_mask = np.full((len(self.features_orig), 4),-1) + self.reset = False + self.img_prev = self.img_curr + return + + grey_prev = cv2.cvtColor(self.img_prev, cv2.COLOR_RGB2GRAY) + + #GET NEW FEATURE LOCATIONS# + next, status, error = cv2.calcOpticalFlowPyrLK(grey_prev, grey_curr , self.features_prev, None, **self.lk_params) + if (not next.any()): + self.reset = True + return + + #Filter out the points with high error + error = error[status[:,0] == 1] + new = next[status[:,0] == 1] + new = new[error[:,0] < 10] + + #Update the original list of features to reflect pixels that have been lost in the flow + self.features_orig = self.features_orig[status[:,0] == 1] + self.features_orig = self.features_orig[error[:,0] < 10] + + # Updates previous good feature points + self.features_prev = new.reshape(-1, 1, 2) + + #Check sufficient parallax# + avg_track_len = getTrackLength(self.features_orig,new) + if (avg_track_len < 30): + #not enough parrallax + return + + #Attempt to calculate SFM + orig_pts = self.features_orig.reshape(len(self.features_orig),2) + + new_pts = new.reshape(len(new),2) + mask_inf = np.zeros(len(new_pts)) + + #TODO filter out points with low parallax + + print(len(orig_pts)) + #Check quality of pts + if (len(orig_pts) < 10): + #Not enough quality points, reset + self.reset = True + return + + #Optional visualization + output = drawTracks(orig_pts.astype(int), new_pts.astype(int, self.img_curr, (0, 255, 0)) + cv2.imshow('Tracks',output) + self.img_prev = self.img_curr + key = cv2.waitKey(1) & 0xFF; + + #SolvePNP + #Get 3d world points and associated pixel values for the second image + print('1', ) + #obj_pts, prev_pts, new_pts, P = getObjectPointsEssential(orig_pts,new_pts,self.P0,self.K, self.D) + + obj_pts, mask_tri, error = triangulate(orig_pts, self.P0, new_pts, P1, self.K, self.D) + + test = PoseStamped() + test.header.stamp = data.header.stamp + test.header.frame_id = self.pose.header.frame_id + test.pose.position.x = P1[0,3] + test.pose.position.y = P1[1,3] + test.pose.position.z = P1[2,3] + + orient = np.hstack((P1[0:3,0:3],[[0],[0],[0]])) + print(orient) + orient = np.vstack((orient,[0,0,0,1])) + orient = quaternion_from_matrix(orient) + + test.pose.orientation.x = orient[0] + test.pose.orientation.y = orient[1] + test.pose.orientation.z = orient[2] + test.pose.orientation.w = orient[3] + + self.test_pose.publish(test) + + #Get colors from detected pixels for coloring pointcloud + + new_pts = new_pts[mask_tri[:,0]==1] + colors = getColors(new_pts,self.img_curr.copy()) + + self.publishPoints(obj_pts, colors) + self.reset = True + + return + + def camInfoCallback(self,data): + self.K = np.reshape(data.K,(3,3)) + self.D = np.reshape(data.D,(1,5)) + + def publishPoints(self, obj_pts, colors): + points = [] + for i in range(len(obj_pts)): + x = obj_pts[i,0] + y = obj_pts[i,1] + z = obj_pts[i,2] + #print(obj_pts[i]) + r = colors[i,0] + g = colors[i,1] + b = colors[i,2] + a = 255 + rgb = struct.unpack('I', struct.pack('BBBB', b, g, r, a))[0] + pt = [x, y, z, rgb] + points.append(pt) + fields = [PointField('x', 0, PointField.FLOAT32, 1), + PointField('y', 4, PointField.FLOAT32, 1), + PointField('z', 8, PointField.FLOAT32, 1), + PointField('rgba', 12, PointField.UINT32, 1), + ] + #Raytracing (clearing of free space) is always done from the origin of this frame_id at the time of the point cloud + header = self.header + header.frame_id = 'map' + cloud = point_cloud2.create_cloud(header, fields, points) + self.cloud_pub.publish(cloud) + +if __name__ == '__main__': + mapping() diff --git a/auto/python/mapping3.py b/auto/python/mapping3.py new file mode 100644 index 0000000000..eaca36799a --- /dev/null +++ b/auto/python/mapping3.py @@ -0,0 +1,164 @@ +import rospy +import tf2_ros +import tf2_geometry_msgs +import cv2 +import numpy as np +import math +import struct + +from cv_bridge import CvBridge +from sensor_msgs.msg import Image, CameraInfo, PointCloud2, PointField, NavSatFix +from sensor_msgs import point_cloud2 +from tf2_msgs.msg import TFMessage +from geometry_msgs.msg import PoseStamped, Quaternion, TransformStamped, Point +from nav_msgs.msg import Odometry + +from sfm import getColors +from sfm import triangulate +from sfm import triangulate_int +from sfm import drawTracks +from sfm import getTrackLength +from sfm import getObjectPointsEssential +from sfm import eliminateDuplicateObjects +from sfm import baFun +from sfm import bundle_adjustment_sparsity + +from scipy.sparse import lil_matrix +from scipy.optimize import least_squares + +from tf.transformations import quaternion_matrix, euler_from_quaternion, quaternion_multiply, quaternion_from_matrix + +bowExtractor = cv2.BOWImgDescriptorExtractor() +orb = cv2.ORB_create() + +class frame(): + + def __init__(self, image): + self.image = image + kp = orb.detect(self.image, None) + kp, desc = orb.compute(self.image,kp) + + def computeBOW(self): + self.BoW = bowExtractor.compute(self.image, kps, desc) + +class mapping(): + + def gpsCallback(self, data): + if (not self.gpsInitialized): + self.initialGPS = data + self.gpsInitialized = True + else: + self.curr_pose.header = data.header + self.curr_pose.frame_id = "map" + self.curr_pose.position = self.ecef_to_enu(self.gps_to_ecef(data),self.gps_to_ecef(self.initialGPS)) + self.test_pose.publish(self.curr_pose) + + def gps_to_ecef(self, gps): + lat = data.latitude + lon = data.longitude + alt = data.altitude + rad_lat = lat * (math.pi / 180.0) + rad_lon = lon * (math.pi / 180.0) + + a = 6378137.0 + finv = 298.257223563 + f = 1 / finv + e2 = 1 - (1 - f) * (1 - f) + v = a / math.sqrt(1 - e2 * math.sin(rad_lat) * math.sin(rad_lat)) + + x = (v + alt) * math.cos(rad_lat) * math.cos(rad_lon) + y = (v + alt) * math.cos(rad_lat) * math.sin(rad_lon) + z = (v * (1 - e2) + alt) * math.sin(rad_lat) + + position = Point() + position.x = x + position.y = y + position.z = z + + return position + + def ecef_to_enu(self, point, origin): + enu = Point() + enu.x = point.x - origin.x + enu.y = point.y - origin.y + enu.z = point.z - origin.z + return enu + + def imageCallback(self, data): + self.header = data.header + self.img_curr = self.bridge.imgmsg_to_cv2(data, desired_encoding='passthrough') + + def camInfoCallback(self,data): + self.K = np.reshape(data.K,(3,3)) + self.D = np.reshape(data.D,(1,5)) + + def publishPoints(self, obj_pts, colors): + points = [] + for i in range(len(obj_pts)): + x = obj_pts[i,0] + y = obj_pts[i,1] + z = obj_pts[i,2] + #print(obj_pts[i]) + r = colors[i,0] + g = colors[i,1] + b = colors[i,2] + a = 255 + rgb = struct.unpack('I', struct.pack('BBBB', b, g, r, a))[0] + pt = [x, y, z, rgb] + points.append(pt) + fields = [PointField('x', 0, PointField.FLOAT32, 1), + PointField('y', 4, PointField.FLOAT32, 1), + PointField('z', 8, PointField.FLOAT32, 1), + PointField('rgba', 12, PointField.UINT32, 1), + ] + #Raytracing (clearing of free space) is always done from the origin of this frame_id at the time of the point cloud + header = self.header + header.frame_id = 'map' + cloud = point_cloud2.create_cloud(header, fields, points) + self.cloud_pub.publish(cloud) + + def __init__(self): + + rospy.init_node('mapping', anonymous=True) + + self.bridge = CvBridge() + self.tfBuffer = tf2_ros.Buffer() + listener = tf2_ros.TransformListener(self.tfBuffer) + + self.img_curr = [] + self.curr_pose = PoseStamped() + self.K = [] + self.d = [] + + self.gpsInitialized = False + self.initialGPS = NavSatFix() + + rospy.Subscriber('/mavros/global_position/global', NavSatFix , self.gpsCallback) + rospy.Subscriber('/airsim/base_link/camera/image_raw', Image, self.imageCallback) + rospy.Subscriber('/airsim/base_link/camera', CameraInfo, self.camInfoCallback) + self.cloud_pub = rospy.Publisher("cloud", PointCloud2, queue_size=10) + self.test_pose = rospy.Publisher("test_pose", PoseStamped, queue_size=10) + + print('waiting on topics...') + rospy.wait_for_message('/airsim/base_link/camera/image_raw', Image) + print('K: ', self.K) + print('D: ', self.D) + rospy.wait_for_message('/mavros/global_position/global', PoseStamped) + + self.run() + + def run(self): + + + if (self.state != "initialized"): + self.initialize() + + def initialize(self): + + + + +if __name__ == '__main__': + mapping() + + diff --git a/auto/python/mav.py b/auto/python/mav.py new file mode 100644 index 0000000000..9c3b913927 --- /dev/null +++ b/auto/python/mav.py @@ -0,0 +1,91 @@ +import rospy +import glob +import json +import math +import os +import px4tools +import sys +import time +import actionlib +import roslaunch +import numpy as np +import tf2_ros +import tf2_geometry_msgs +import cv2 +import shutil + +from cv_bridge import CvBridge, CvBridgeError +from mavros import mavlink +from mavros_msgs.msg import Mavlink, Waypoint, WaypointReached, GlobalPositionTarget, State, TakeoffAction, TakeoffGoal, LandAction, LandGoal, WaypointsAction, WaypointsGoal, HomePosition +from mavros_msgs.srv import CommandBool, SetMode, CommandTOL, WaypointPush, WaypointClear, CommandHome +from orb_slam2_ros.msg import KeyFrames, Observations +from sensor_msgs.msg import NavSatFix, Image, PointCloud2 +from std_msgs.msg import Header +from nav_msgs.msg import Odometry +from geometry_msgs.msg import Pose, PoseWithCovariance, PoseStamped, PoseWithCovarianceStamped +from mavros_test_common import MavrosTestCommon +from pymavlink import mavutil + +class KeyFrame(object): + empty = [] + + def __init__(self): + self.image_id = "" + self.latitude = 0.0 + self.longitude = 0.0 + self.altitude = 0.0 + self.orientation = [0.0,0.0,0.0,0.0] + self.image = "" + +class companion(): + + def __init__(self): + + #Initialize ROSLaunch + launch = roslaunch.scriptapi.ROSLaunch() + launch.start() + uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) + roslaunch.configure_logging(uuid) + + #Launch MAVROS + cli_args = ('mavros', 'px4.launch') + roslaunch_file = [(roslaunch.rlutil.resolve_launch_arguments(cli_args)[0])] + mavros = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_file) + mavros.start() + + rospy.init_node('companion_node', anonymous=True) + + #clear old images + shutil.rmtree('img') + os.mkdir('img') + + self.local_pose = Pose() + self.slam_pose = PoseStamped() + self.raw_slam_pose = PoseStamped() + self.calib_init_slam = Pose() + self.calib_init_local = Pose() + self.slam_calibrated = False + self.slam_offset_x = 0 + self.slam_offset_y = 0 + self.slam_offset_z = 0 + self.slam_scale = 1 + self.state = "SYSTEM_NOT_READY" + self.last_state = "SYSTEM_NOT_READY" + + #States: + #"SYSTEM_NOT_READY" + #"NO_IMAGES_YET" + #"NOT_INITIALIZED" + #"OK" + #"LOST" + + self.keyframes = [] + self.image_buffer = [] + self.bridge = CvBridge() + + rospy.spin() + + #Code to scale and publish orbslam pose to px + +if __name__ == '__main__': + companion() diff --git a/auto/python/mavros_test_common.py b/auto/python/mavros_test_common.py new file mode 100644 index 0000000000..b9d9c9ffa7 --- /dev/null +++ b/auto/python/mavros_test_common.py @@ -0,0 +1,417 @@ +#!/usr/bin/env python2 +from __future__ import division + +import unittest +import rospy +import math +from geometry_msgs.msg import PoseStamped +from mavros_msgs.msg import Altitude, ExtendedState, HomePosition, State, \ + WaypointList +from mavros_msgs.srv import CommandBool, ParamGet, SetMode, WaypointClear, \ + WaypointPush +from pymavlink import mavutil +from sensor_msgs.msg import NavSatFix + + +class MavrosTestCommon(unittest.TestCase): + def __init__(self, *args): + super(MavrosTestCommon, self).__init__(*args) + + def setUp(self): + self.altitude = Altitude() + self.extended_state = ExtendedState() + self.global_position = NavSatFix() + self.home_position = HomePosition() + self.local_position = PoseStamped() + self.mission_wp = WaypointList() + self.state = State() + self.mav_type = None + + self.sub_topics_ready = { + key: False + for key in [ + 'alt', 'ext_state', 'global_pos', 'home_pos', 'local_pos', + 'mission_wp', 'state' + ] + } + + # ROS services + service_timeout = 30 + rospy.loginfo("waiting for ROS services") + try: + rospy.wait_for_service('mavros/param/get', service_timeout) + rospy.wait_for_service('mavros/cmd/arming', service_timeout) + rospy.wait_for_service('mavros/mission/push', service_timeout) + rospy.wait_for_service('mavros/mission/clear', service_timeout) + rospy.wait_for_service('mavros/set_mode', service_timeout) + rospy.loginfo("ROS services are up") + except rospy.ROSException: + self.fail("failed to connect to services") + self.get_param_srv = rospy.ServiceProxy('mavros/param/get', ParamGet) + self.set_arming_srv = rospy.ServiceProxy('mavros/cmd/arming', + CommandBool) + self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode) + self.wp_clear_srv = rospy.ServiceProxy('mavros/mission/clear', + WaypointClear) + self.wp_push_srv = rospy.ServiceProxy('mavros/mission/push', + WaypointPush) + + # ROS subscribers + self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude, + self.altitude_callback) + self.ext_state_sub = rospy.Subscriber('mavros/extended_state', + ExtendedState, + self.extended_state_callback) + self.global_pos_sub = rospy.Subscriber('mavros/global_position/global', + NavSatFix, + self.global_position_callback) + self.home_pos_sub = rospy.Subscriber('mavros/home_position/home', + HomePosition, + self.home_position_callback) + self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose', + PoseStamped, + self.local_position_callback) + self.mission_wp_sub = rospy.Subscriber( + 'mavros/mission/waypoints', WaypointList, self.mission_wp_callback) + self.state_sub = rospy.Subscriber('mavros/state', State, + self.state_callback) + + def tearDown(self): + self.log_topic_vars() + + # + # Callback functions + # + def altitude_callback(self, data): + self.altitude = data + + # amsl has been observed to be nan while other fields are valid + if not self.sub_topics_ready['alt'] and not math.isnan(data.amsl): + self.sub_topics_ready['alt'] = True + + def extended_state_callback(self, data): + if self.extended_state.vtol_state != data.vtol_state: + rospy.loginfo("VTOL state changed from {0} to {1}".format( + mavutil.mavlink.enums['MAV_VTOL_STATE'] + [self.extended_state.vtol_state].name, mavutil.mavlink.enums[ + 'MAV_VTOL_STATE'][data.vtol_state].name)) + + if self.extended_state.landed_state != data.landed_state: + rospy.loginfo("landed state changed from {0} to {1}".format( + mavutil.mavlink.enums['MAV_LANDED_STATE'] + [self.extended_state.landed_state].name, mavutil.mavlink.enums[ + 'MAV_LANDED_STATE'][data.landed_state].name)) + + self.extended_state = data + + if not self.sub_topics_ready['ext_state']: + self.sub_topics_ready['ext_state'] = True + + def global_position_callback(self, data): + self.global_position = data + + if not self.sub_topics_ready['global_pos']: + self.sub_topics_ready['global_pos'] = True + + def home_position_callback(self, data): + self.home_position = data + + if not self.sub_topics_ready['home_pos']: + self.sub_topics_ready['home_pos'] = True + + def local_position_callback(self, data): + self.local_position = data + + if not self.sub_topics_ready['local_pos']: + self.sub_topics_ready['local_pos'] = True + + def mission_wp_callback(self, data): + if self.mission_wp.current_seq != data.current_seq: + rospy.loginfo("current mission waypoint sequence updated: {0}". + format(data.current_seq)) + + self.mission_wp = data + + if not self.sub_topics_ready['mission_wp']: + self.sub_topics_ready['mission_wp'] = True + + def state_callback(self, data): + if self.state.armed != data.armed: + rospy.loginfo("armed state changed from {0} to {1}".format( + self.state.armed, data.armed)) + + if self.state.connected != data.connected: + rospy.loginfo("connected changed from {0} to {1}".format( + self.state.connected, data.connected)) + + if self.state.mode != data.mode: + rospy.loginfo("mode changed from {0} to {1}".format( + self.state.mode, data.mode)) + + if self.state.system_status != data.system_status: + rospy.loginfo("system_status changed from {0} to {1}".format( + mavutil.mavlink.enums['MAV_STATE'][ + self.state.system_status].name, mavutil.mavlink.enums[ + 'MAV_STATE'][data.system_status].name)) + + self.state = data + + # mavros publishes a disconnected state message on init + if not self.sub_topics_ready['state'] and data.connected: + self.sub_topics_ready['state'] = True + + # + # Helper methods + # + def set_arm(self, arm, timeout): + """arm: True to arm or False to disarm, timeout(int): seconds""" + rospy.loginfo("setting FCU arm: {0}".format(arm)) + old_arm = self.state.armed + loop_freq = 1 # Hz + rate = rospy.Rate(loop_freq) + arm_set = False + for i in xrange(timeout * loop_freq): + if self.state.armed == arm: + arm_set = True + rospy.loginfo("set arm success | seconds: {0} of {1}".format( + i / loop_freq, timeout)) + break + else: + try: + res = self.set_arming_srv(arm) + if not res.success: + rospy.logerr("failed to send arm command") + except rospy.ServiceException as e: + rospy.logerr(e) + + try: + rate.sleep() + except rospy.ROSException as e: + self.fail(e) + + self.assertTrue(arm_set, ( + "failed to set arm | new arm: {0}, old arm: {1} | timeout(seconds): {2}". + format(arm, old_arm, timeout))) + + def set_mode(self, mode, timeout): + """mode: PX4 mode string, timeout(int): seconds""" + rospy.loginfo("setting FCU mode: {0}".format(mode)) + old_mode = self.state.mode + loop_freq = 1 # Hz + rate = rospy.Rate(loop_freq) + mode_set = False + for i in xrange(timeout * loop_freq): + if self.state.mode == mode: + mode_set = True + rospy.loginfo("set mode success | seconds: {0} of {1}".format( + i / loop_freq, timeout)) + break + else: + try: + res = self.set_mode_srv(0, mode) # 0 is custom mode + if not res.mode_sent: + rospy.logerr("failed to send mode command") + except rospy.ServiceException as e: + rospy.logerr(e) + + try: + rate.sleep() + except rospy.ROSException as e: + self.fail(e) + + self.assertTrue(mode_set, ( + "failed to set mode | new mode: {0}, old mode: {1} | timeout(seconds): {2}". + format(mode, old_mode, timeout))) + + def wait_for_topics(self, timeout): + """wait for simulation to be ready, make sure we're getting topic info + from all topics by checking dictionary of flag values set in callbacks, + timeout(int): seconds""" + rospy.loginfo("waiting for subscribed topics to be ready") + loop_freq = 1 # Hz + rate = rospy.Rate(loop_freq) + simulation_ready = False + for i in xrange(timeout * loop_freq): + if all(value for value in self.sub_topics_ready.values()): + simulation_ready = True + rospy.loginfo("simulation topics ready | seconds: {0} of {1}". + format(i / loop_freq, timeout)) + break + + try: + rate.sleep() + except rospy.ROSException as e: + self.fail(e) + + self.assertTrue(simulation_ready, ( + "failed to hear from all subscribed simulation topics | topic ready flags: {0} | timeout(seconds): {1}". + format(self.sub_topics_ready, timeout))) + + def wait_for_landed_state(self, desired_landed_state, timeout, index): + rospy.loginfo("waiting for landed state | state: {0}, index: {1}". + format(mavutil.mavlink.enums['MAV_LANDED_STATE'][ + desired_landed_state].name, index)) + loop_freq = 10 # Hz + rate = rospy.Rate(loop_freq) + landed_state_confirmed = False + for i in xrange(timeout * loop_freq): + if self.extended_state.landed_state == desired_landed_state: + landed_state_confirmed = True + rospy.loginfo("landed state confirmed | seconds: {0} of {1}". + format(i / loop_freq, timeout)) + break + + try: + rate.sleep() + except rospy.ROSException as e: + self.fail(e) + + self.assertTrue(landed_state_confirmed, ( + "landed state not detected | desired: {0}, current: {1} | index: {2}, timeout(seconds): {3}". + format(mavutil.mavlink.enums['MAV_LANDED_STATE'][ + desired_landed_state].name, mavutil.mavlink.enums[ + 'MAV_LANDED_STATE'][self.extended_state.landed_state].name, + index, timeout))) + + def wait_for_vtol_state(self, transition, timeout, index): + """Wait for VTOL transition, timeout(int): seconds""" + rospy.loginfo( + "waiting for VTOL transition | transition: {0}, index: {1}".format( + mavutil.mavlink.enums['MAV_VTOL_STATE'][ + transition].name, index)) + loop_freq = 10 # Hz + rate = rospy.Rate(loop_freq) + transitioned = False + for i in xrange(timeout * loop_freq): + if transition == self.extended_state.vtol_state: + rospy.loginfo("transitioned | seconds: {0} of {1}".format( + i / loop_freq, timeout)) + transitioned = True + break + + try: + rate.sleep() + except rospy.ROSException as e: + self.fail(e) + + self.assertTrue(transitioned, ( + "transition not detected | desired: {0}, current: {1} | index: {2} timeout(seconds): {3}". + format(mavutil.mavlink.enums['MAV_VTOL_STATE'][transition].name, + mavutil.mavlink.enums['MAV_VTOL_STATE'][ + self.extended_state.vtol_state].name, index, timeout))) + + def clear_wps(self, timeout): + """timeout(int): seconds""" + loop_freq = 1 # Hz + rate = rospy.Rate(loop_freq) + wps_cleared = False + for i in xrange(timeout * loop_freq): + if not self.mission_wp.waypoints: + wps_cleared = True + rospy.loginfo("clear waypoints success | seconds: {0} of {1}". + format(i / loop_freq, timeout)) + break + else: + try: + res = self.wp_clear_srv() + if not res.success: + rospy.logerr("failed to send waypoint clear command") + except rospy.ServiceException as e: + rospy.logerr(e) + + try: + rate.sleep() + except rospy.ROSException as e: + self.fail(e) + + self.assertTrue(wps_cleared, ( + "failed to clear waypoints | timeout(seconds): {0}".format(timeout) + )) + + def send_wps(self, waypoints, timeout): + """waypoints, timeout(int): seconds""" + rospy.loginfo("sending mission waypoints") + if self.mission_wp.waypoints: + rospy.loginfo("FCU already has mission waypoints") + + loop_freq = 1 # Hz + rate = rospy.Rate(loop_freq) + wps_sent = False + wps_verified = False + for i in xrange(timeout * loop_freq): + if not wps_sent: + try: + res = self.wp_push_srv(start_index=0, waypoints=waypoints) + wps_sent = res.success + if wps_sent: + rospy.loginfo("waypoints successfully transferred") + except rospy.ServiceException as e: + rospy.logerr(e) + else: + if len(waypoints) == len(self.mission_wp.waypoints): + rospy.loginfo("number of waypoints transferred: {0}". + format(len(waypoints))) + wps_verified = True + + if wps_sent and wps_verified: + rospy.loginfo("send waypoints success | seconds: {0} of {1}". + format(i / loop_freq, timeout)) + break + + try: + rate.sleep() + except rospy.ROSException as e: + self.fail(e) + + self.assertTrue(( + wps_sent and wps_verified + ), "mission could not be transferred and verified | timeout(seconds): {0}". + format(timeout)) + + def wait_for_mav_type(self, timeout): + """Wait for MAV_TYPE parameter, timeout(int): seconds""" + rospy.loginfo("waiting for MAV_TYPE") + loop_freq = 1 # Hz + rate = rospy.Rate(loop_freq) + res = False + for i in xrange(timeout * loop_freq): + try: + res = self.get_param_srv('MAV_TYPE') + if res.success: + self.mav_type = res.value.integer + rospy.loginfo( + "MAV_TYPE received | type: {0} | seconds: {1} of {2}". + format(mavutil.mavlink.enums['MAV_TYPE'][self.mav_type] + .name, i / loop_freq, timeout)) + break + except rospy.ServiceException as e: + rospy.logerr(e) + + try: + rate.sleep() + except rospy.ROSException as e: + self.fail(e) + + self.assertTrue(res.success, ( + "MAV_TYPE param get failed | timeout(seconds): {0}".format(timeout) + )) + + def log_topic_vars(self): + """log the state of topic variables""" + rospy.loginfo("========================") + rospy.loginfo("===== topic values =====") + rospy.loginfo("========================") + rospy.loginfo("altitude:\n{}".format(self.altitude)) + rospy.loginfo("========================") + rospy.loginfo("extended_state:\n{}".format(self.extended_state)) + rospy.loginfo("========================") + rospy.loginfo("global_position:\n{}".format(self.global_position)) + rospy.loginfo("========================") + rospy.loginfo("home_position:\n{}".format(self.home_position)) + rospy.loginfo("========================") + rospy.loginfo("local_position:\n{}".format(self.local_position)) + rospy.loginfo("========================") + rospy.loginfo("mission_wp:\n{}".format(self.mission_wp)) + rospy.loginfo("========================") + rospy.loginfo("state:\n{}".format(self.state)) + rospy.loginfo("========================") diff --git a/auto/python/mavros_test_common.pyc b/auto/python/mavros_test_common.pyc new file mode 100644 index 0000000000..4fef7677ec Binary files /dev/null and b/auto/python/mavros_test_common.pyc differ diff --git a/auto/python/mavtest.py b/auto/python/mavtest.py new file mode 100644 index 0000000000..a13a9a4a25 --- /dev/null +++ b/auto/python/mavtest.py @@ -0,0 +1,204 @@ +import rospy +import glob +import json +import math +import os +import px4tools +import sys +from mavros import mavlink +from mavros_msgs.msg import Mavlink, Waypoint, WaypointReached, GlobalPositionTarget, State +from mavros_msgs.srv import CommandBool, SetMode, CommandTOL, WaypointPush, WaypointClear +from sensor_msgs.msg import NavSatFix +from geometry_msgs.msg import PoseStamped +from mavros_test_common import MavrosTestCommon +from pymavlink import mavutil +from threading import Thread + +class mavrostest(): + + def state_callback(self, data): + self.state = data + + def wp_reached_callback(self, data): + self.wp_reached = data + + def global_pos_callback(self, data): + self.global_pos = data + + def __init__(self): + + rospy.init_node('test_node', anonymous=True) + self.state = State() + self.wp_reached = 0 + self.global_pos = NavSatFix() + + #SUBSCRIBERS + state_sub = rospy.Subscriber('mavros/state', State, self.state_callback) + #global_pos_sub = rospy.Subscriber('/mavros/global_position/global', State, self.state_callback) + local_pos_sub = rospy.Subscriber('/mavros/global_position/global', NavSatFix, self.global_pos_callback) + wp_reached_sub = rospy.Subscriber('/mavros/mission/reached', WaypointReached, self.state_callback) + + #PUBLISHERS + local_pos_pub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10) + #global_pos_pub = rospy.Publisher('mavros/setpoint_position/global', GlobalPositionTarget, queue_size=10) + + #SERVICES + arm_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) + takeoff_client = rospy.ServiceProxy('mavros/cmd/takeoff', CommandTOL) + land_client = rospy.ServiceProxy('mavros/cmd/land', CommandTOL) + mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode) + mission_push_client = rospy.ServiceProxy('mavros/mission/push', WaypointPush) + mission_clear_client = rospy.ServiceProxy('mavros/mission/clear', WaypointClear) + + rate = rospy.Rate(20) + + while (not self.state.connected): + print('Waiting on Connection') + rate.sleep() + print('Connected') + + # need to simulate heartbeat to prevent datalink loss detection + hb_mav_msg = mavutil.mavlink.MAVLink_heartbeat_message(mavutil.mavlink.MAV_TYPE_GCS, 0, 0, 0, 0, 0) + hb_mav_msg.pack(mavutil.mavlink.MAVLink('', 2, 1)) + hb_ros_msg = mavlink.convert_to_rosmsg(hb_mav_msg) + hb_thread = Thread(target=self.send_heartbeat, args=(hb_ros_msg)) + + last_request = rospy.Time.now() + # Disarm + ack = False + while (not ack): + try: + ack = arm_client(False).success + except rospy.ServiceException as e: + print("Disarming Failed: %s" %e) + rate.sleep() + print('Disarmed') + + # Set Mode + mode = "AUTO.LOITER" + ack = False + while (not ack): + try: + ack = mode_client(0, "AUTO.LOITER").mode_sent # 0 is custom mode + except rospy.ServiceException as e: + print("Mode Change Failed: %s" %e) + rate.sleep() + print('Mode set to ', mode) + + # Arm + ack = False + while (not ack): + try: + ack = arm_client(True).success + except rospy.ServiceException as e: + print("Arming Failed: %s" %e) + rate.sleep() + print('Armed') + + #Clear any old missions + ack = False + while (not ack): + try: + ack = mission_clear_client().success + except rospy.ServiceException as e: + print("Mission Clear Failed: %s" %e) + rate.sleep() + print('old missions cleared') + + #Create and execute Mission + + home_lat = self.global_pos.latitude + home_long = self.global_pos.longitude + + waypoints = [] + takeoff = Waypoint() + takeoff.frame = 3 + takeoff.command = mavutil.mavlink.MAV_CMD_NAV_TAKEOFF + takeoff.is_current = True + takeoff.autocontinue = True + takeoff.param1 = 0.0 + takeoff.param2 = 0.0 + takeoff.param3 = 0.3 + takeoff.param4 = 0.0 + takeoff.x_lat = home_lat + takeoff.y_long = home_long + takeoff.z_alt = 8.0 + + wp1 = Waypoint() + wp1.frame = 3 + wp1.command = mavutil.mavlink.MAV_CMD_NAV_WAYPOINT + wp1.is_current = True + wp1.autocontinue = True + wp1.param1 = 0.0 + wp1.param2 = 0.0 + wp1.param3 = 0.3 + wp1.param4 = 0.0 + wp1.x_lat = home_lat + 0.00005 + wp1.y_long = home_long + wp1.z_alt = 8.0 + + waypoints.append(wp1) + + rtl = Waypoint() + rtl.frame = 3 + rtl.command = 20 + rtl.is_current = True + rtl.autocontinue = True + rtl.param1 = 0.0 + rtl.param2 = 0.0 + rtl.param3 = 0.0 + rtl.param4 = 0.0 + rtl.x_lat = 0.0 + rtl.y_long = 0.0 + rtl.z_alt = 0.0 + waypoints.append(rtl) + + + ack = False + while (not ack): + try: + ack = mission_push_client(start_index=0, waypoints=waypoints).success + except rospy.ServiceException as e: + print("Mission Push Failed: %s" %e) + rate.sleep() + + print('Mission Loaded') + + # Set Mode + mode = "AUTO.MISSION" + ack = False + while (not ack): + try: + ack = mode_client(5, mode).mode_sent # 0 is custom mode + except rospy.ServiceException as e: + print("Mode Change Failed: %s" %e) + rate.sleep() + + print('Beginning Mission') + + while (self.wp_reached != 3): + rate.sleep() + +# print "\nTaking off" +# try: +# response = takeoff_client(altitude=10, latitude=0, longitude=0, min_pitch=0, yaw=0) +# rospy.loginfo(response) +# except rospy.ServiceException as e: +# print("Takeoff failed: %s" %e) + + + # + # Helper methods + # + def send_heartbeat(self, hb_ros_msg): + rate = rospy.Rate(2) # Hz + while not rospy.is_shutdown(): + self.mavlink_pub.publish(hb_ros_msg) + try: # prevent garbage in console output when thread is killed + rate.sleep() + except rospy.ROSInterruptException: + pass + +if __name__ == '__main__': + mavrostest() + diff --git a/auto/python/navigation.py b/auto/python/navigation.py new file mode 100644 index 0000000000..5158b68c31 --- /dev/null +++ b/auto/python/navigation.py @@ -0,0 +1,240 @@ +import rospy +import glob +import json +import math +import os +import px4tools +import sys +import time +import actionlib +import roslaunch +import numpy as np +from mavros import mavlink +from mavros import action_server +from mavros_msgs.msg import Mavlink, Waypoint, WaypointReached, GlobalPositionTarget, State, TakeoffAction, TakeoffGoal, LandAction, LandGoal, WaypointsAction, WaypointsGoal, HomePosition +from mavros_msgs.srv import CommandBool, SetMode, CommandTOL, WaypointPush, WaypointClear, CommandHome +from sensor_msgs.msg import NavSatFix +from geometry_msgs.msg import PoseStamped +from mavros_test_common import MavrosTestCommon +from pymavlink import mavutil +from threading import Thread + +# Brings in the SimpleActionClient +import actionlib + +class offboard(): + + def state_callback(self, data): + self.state = data + + def wp_reached_callback(self, data): + self.wp_reached = data + + def home_pos_callback(self, data): + self.home_pos = data + #print(self.home_pos.geo) + + def global_pos_callback(self, data): + self.global_pos = data + + def __init__(self): + + rospy.init_node('guidance_node', anonymous=True) + + state_sub = rospy.Subscriber('mavros/state', State, self.state_callback) + self.state = State + #PUBLISHERS + local_pos_pub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10) + #global_pos_pub = rospy.Publisher('mavros/setpoint_position/global', GlobalPositionTarget, queue_size=10) + local_pos_sub = rospy.Subscriber('/mavros/global_position/global', NavSatFix, self.global_pos_callback) + home_pos_sub = rospy.Subscriber('/mavros/home_position/home', HomePosition, self.home_pos_callback) + + #ACTIONS + #init actionlib servers + server = Thread(target=action_server.ActionServer) + server.setDaemon(True) + server.start() + takeoff_client = actionlib.SimpleActionClient('takeoff', TakeoffAction) + land_client = actionlib.SimpleActionClient('land', LandAction) + waypoints_client = actionlib.SimpleActionClient('waypoints', WaypointsAction) + + # need to simulate heartbeat to prevent datalink loss detection + hb_mav_msg = mavutil.mavlink.MAVLink_heartbeat_message(mavutil.mavlink.MAV_TYPE_GCS, 0, 0, 0, 0, 0) + hb_mav_msg.pack(mavutil.mavlink.MAVLink('', 2, 1)) + hb_ros_msg = mavlink.convert_to_rosmsg(hb_mav_msg) + hb_thread = Thread(target=self.send_heartbeat, args=(hb_ros_msg)) + hb_thread.setDaemon(True) + + #PREFLIGHT CHECK + + rate = rospy.Rate(30) + while (not self.state.connected): + print('Waiting on Connection') + rate.sleep() + print('Connected') + + time.sleep(5) + + goal = TakeoffGoal() + goal.height = 22 + + print('Actionlib started') + + takeoff_client.send_goal(goal) + takeoff_client.wait_for_result() + +#1: Hold Hold time. (ignored by fixed wing, time to stay at waypoint for rotary wing) min:0 s +#2: Accept Radius Acceptance radius (if the sphere with this radius is hit, the waypoint counts as reached) min:0 m +#3: Pass Radius 0 to pass through the WP, if > 0 radius to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. +#4: Yaw Desired yaw angle (deg) at waypoint (rotary wing). NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.). +#5: Latitude Latitude +#6: Longitude Longitude +#7: Altitude Altitude + + wps = [] + wp1 = Waypoint() + wp2 = Waypoint() + wp3 = Waypoint() + wp4 = Waypoint() + wp5 = Waypoint() + wp6 = Waypoint() + wp7 = Waypoint() + wp8 = Waypoint() + wp9 = Waypoint() + wp10 = Waypoint() + wp11 = Waypoint() + wp12 = Waypoint() + + wp1.command = 16 + #wp1.param4 = np.nan + wp1.x_lat = 40.00482126 + wp1.y_long = 100.0100724 + wp1.z_alt = 22 + wp1.autocontinue = True + + wp2.command = 16 + #wp3.param4 = np.nan + wp2.x_lat = 40.00422467 + wp2.y_long = 100.0122255 + wp2.z_alt = 22 + wp2.autocontinue = True + + + wp3.command = 16 + #wp3.param4 = np.nan + wp3.x_lat = 40.00443838 + wp3.y_long = 100.0123931 + wp3.z_alt = 22 + wp3.autocontinue = True + + wp4.command = 16 + #wp4.param4 = np.nan + wp4.x_lat = 40.00515832 + wp4.y_long = 100.0103007 + wp4.z_alt = 22 + wp4.autocontinue = True + + wp5.command = 16 + #wp5.param4 = np.nan + wp5.x_lat = 40.00549537 + wp5.y_long = 100.010529 + wp5.z_alt = 22 + wp5.autocontinue = True + + wp6.command = 16 + #wp6.param4 = np.nan + wp6.x_lat = 40.0046521 + wp6.y_long = 100.0125607 + wp6.z_alt = 22 + wp6.autocontinue = True + + wp7.command = 16 + #wp7.param4 = np.nan + wp7.x_lat = 40.00489078 + wp7.y_long = 100.0127564 + wp7.z_alt = 22 + wp7.autocontinue = True + + wp8.command = 16 + #wp8.param4 = np.nan + wp8.x_lat = 40.00573685 + wp8.y_long = 100.0106538 + wp8.z_alt = 22 + wp8.autocontinue = True + + wp9.command = 16 + #wp9.param4 = np.nan + wp9.x_lat = 40.00597832 + wp9.y_long = 100.0107787 + wp9.z_alt = 22 + wp9.autocontinue = True + + wp10.command = 16 + #wp10.param4 = np.nan + wp10.x_lat = 40.00512946 + wp10.y_long = 100.0129521 + wp10.z_alt = 22 + wp10.autocontinue = True + + wp11.command = 16 + #wp11.param4 = np.nan + wp11.x_lat = 40.00536815 + wp11.y_long = 100.0131477 + wp11.z_alt = 22 + wp11.autocontinue = True + + wp12.command = 16 + #wp12.param4 = np.nan + wp12.x_lat = 40.00621979 + wp12.y_long = 100.0109035 + wp12.z_alt = 22 + wp12.autocontinue = True + + goal = WaypointsGoal() + goal.waypoints.append(wp1) + goal.waypoints.append(wp2) + goal.waypoints.append(wp3) + goal.waypoints.append(wp4) + goal.waypoints.append(wp5) + goal.waypoints.append(wp6) + goal.waypoints.append(wp7) + goal.waypoints.append(wp8) + goal.waypoints.append(wp9) + goal.waypoints.append(wp10) + goal.waypoints.append(wp11) + goal.waypoints.append(wp12) + print(goal) + waypoints_client.send_goal(goal) + waypoints_client.wait_for_result(rospy.Duration.from_sec(800.0)) + + time.sleep(5) + + goal = LandGoal() + goal.x_lat = self.home_pos.geo.latitude + goal.y_long = self.home_pos.geo.longitude + goal.z_alt = 0.0 + + print('Actionlib started') + + land_client.send_goal(goal) + land_client.wait_for_result(rospy.Duration.from_sec(30.0)) + sys.exit() + + + # Heartbeat must be sent to px4 at 2Hz or else auto disconnect + def send_heartbeat(self, hb_ros_msg): + rate = rospy.Rate(2) # Hz + while not rospy.is_shutdown(): + self.mavlink_pub.publish(hb_ros_msg) + try: # prevent garbage in console output when thread is killed + rate.sleep() + except rospy.ROSInterruptException: + pass + + +if __name__ == '__main__': + offboard() + + + + diff --git a/auto/python/ned2gps.py b/auto/python/ned2gps.py new file mode 100644 index 0000000000..b219209fc8 --- /dev/null +++ b/auto/python/ned2gps.py @@ -0,0 +1,36 @@ +import math + +EARTH_RADIUS = 6378137.0 + +home_lat = 40 +home_lon = 100 + +home_alt = -21396.533203 +#player_start +ps_z = 0 + +#destination +dest_x = 59766.203125 +dest_y = 112109.476562 +z = 20720.0/100 + +x = (dest_x)/100 +y = (dest_y)/100 + + +x_rad = x/EARTH_RADIUS +y_rad = y/EARTH_RADIUS + +c = math.sqrt(x_rad*x_rad + y_rad*y_rad) +sin_c = math.sin(c) +cos_c = math.cos(c) + +home_lat_rad = home_lat * math.pi / 180 +home_lon_rad = home_lon * math.pi / 180 +home_cos_lat = math.cos(home_lat_rad); +home_sin_lat = math.sin(home_lat_rad); + +lat_rad = math.asin(cos_c * home_sin_lat + (x_rad * sin_c * home_cos_lat) / c) +lon_rad = (home_lon_rad + math.atan2(y_rad * sin_c, c * home_cos_lat * cos_c - x_rad * home_sin_lat * sin_c)) + +print(lat_rad*180/math.pi,lon_rad*180/math.pi, ps_z-z) diff --git a/auto/python/offboard.py b/auto/python/offboard.py new file mode 100644 index 0000000000..32d9887c85 --- /dev/null +++ b/auto/python/offboard.py @@ -0,0 +1,165 @@ +import rospy +import glob +import json +import math +import os +import px4tools +import sys +import time +import actionlib +import roslaunch +from mavros import mavlink +from mavros import action_server +from mavros_msgs.msg import Mavlink, Waypoint, WaypointReached, GlobalPositionTarget, State, TakeoffAction, TakeoffGoal, LandAction, LandGoal, WaypointsAction, WaypointsGoal +from mavros_msgs.srv import CommandBool, SetMode, CommandTOL, WaypointPush, WaypointClear +from sensor_msgs.msg import NavSatFix +from geometry_msgs.msg import PoseStamped +from mavros_test_common import MavrosTestCommon +from pymavlink import mavutil +from threading import Thread + +# Brings in the SimpleActionClient +import actionlib + +class offboard(MavrosTestCommon): + + def state_callback(self, data): + self.state = data + + def wp_reached_callback(self, data): + self.wp_reached = data + + def global_pos_callback(self, data): + self.global_pos = data + + def __init__(self): + + #Initialize ROSLaunch + launch = roslaunch.scriptapi.ROSLaunch() + launch.start() + uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) + roslaunch.configure_logging(uuid) + + #Launch MAVROS + cli_args = ('mavros', 'px4.launch') + roslaunch_file = [(roslaunch.rlutil.resolve_launch_arguments(cli_args)[0])] + mavros = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_file) + mavros.start() + + rospy.init_node('test_node', anonymous=True) + + print('got here') + + #SUBSCRIBERS + state_sub = rospy.Subscriber('mavros/state', State, self.state_callback) + #global_pos_sub = rospy.Subscriber('/mavros/global_position/global', State, self.state_callback) + local_pos_sub = rospy.Subscriber('/mavros/global_position/global', NavSatFix, self.global_pos_callback) + #wp_reached_sub = rospy.Subscriber('/mavros/mission/reached', WaypointReached, self.wp_reached_callback) + + #PUBLISHERS + local_pos_pub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10) + #global_pos_pub = rospy.Publisher('mavros/setpoint_position/global', GlobalPositionTarget, queue_size=10) + + #SERVICES + arm_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) + #takeoff_client = rospy.ServiceProxy('mavros/cmd/takeoff', CommandTOL) + #land_client = rospy.ServiceProxy('mavros/cmd/land', CommandTOL) + mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode) + #mission_push_client = rospy.ServiceProxy('mavros/mission/push', WaypointPush) + mission_clear_client = rospy.ServiceProxy('mavros/mission/clear', WaypointClear) + + #ACTIONS + #init actionlib servers + server = Thread(target=action_server.ActionServer) + server.setDaemon(True) + server.start() + takeoff_client = actionlib.SimpleActionClient('takeoff', TakeoffAction) + land_client = actionlib.SimpleActionClient('land', LandAction) + waypoints_client = actionlib.SimpleActionClient('waypoints', WaypointsAction) + + self.state = State() + self.wp_reached = 0 + self.global_pos = NavSatFix() + + rate = rospy.Rate(30) + while (not self.state.connected): + print('Waiting on Connection') + rate.sleep() + print('Connected') + + # need to simulate heartbeat to prevent datalink loss detection + hb_mav_msg = mavutil.mavlink.MAVLink_heartbeat_message(mavutil.mavlink.MAV_TYPE_GCS, 0, 0, 0, 0, 0) + hb_mav_msg.pack(mavutil.mavlink.MAVLink('', 2, 1)) + hb_ros_msg = mavlink.convert_to_rosmsg(hb_mav_msg) + hb_thread = Thread(target=self.send_heartbeat, args=(hb_ros_msg)) + hb_thread.setDaemon(True) + + #PREFLIGHT CHECK + + time.sleep(5) + + #Clear any old missions + ack = False + while (not ack): + try: + ack = mission_clear_client().success + except rospy.ServiceException as e: + print("Mission Clear Failed: %s" %e) + rate.sleep() + print('old missions cleared') + + time.sleep(5) + + # Arm + self.set_arm(True, 5) + + goal = TakeoffGoal() + goal.height = 10 + + print('Actionlib started') + + takeoff_client.send_goal(goal) + takeoff_client.wait_for_result() + + wp1 = Waypoint() + wp2 = Waypoint() + wp1.x_lat = self.global_pos.latitude + 0.00110 + wp1.y_long = self.global_pos.longitude + 0.00110 + wp1.z_alt = 10 + + wp2.x_lat = self.global_pos.latitude + wp2.y_long = self.global_pos.longitude + wp2.z_alt = 10 + + goal = WaypointsGoal() + goal.waypoints = [wp1, wp2] + #waypoints_client.send_goal(goal) + #waypoints_client.wait_for_result(rospy.Duration.from_sec(45.0)) + + goal = LandGoal() + goal.height = 0.0 + + print('Actionlib started') + + land_client.send_goal(goal) + land_client.wait_for_result(rospy.Duration.from_sec(30.0)) + sys.exit() + + + # Heartbeat must be sent to px4 at 2Hz or else auto disconnect + def send_heartbeat(self, hb_ros_msg): + rate = rospy.Rate(2) # Hz + while not rospy.is_shutdown(): + self.mavlink_pub.publish(hb_ros_msg) + try: # prevent garbage in console output when thread is killed + rate.sleep() + except rospy.ROSInterruptException: + pass + + +if __name__ == '__main__': + offboard() + + + + diff --git a/auto/python/offboard2.py b/auto/python/offboard2.py new file mode 100644 index 0000000000..20a810a414 --- /dev/null +++ b/auto/python/offboard2.py @@ -0,0 +1,151 @@ +import rospy +import glob +import json +import math +import os +import px4tools +import sys +import time +import actionlib +import roslaunch +from mavros import mavlink +from mavros import action_server2 +from mavros_msgs.msg import Mavlink, Waypoint, WaypointReached, GlobalPositionTarget, State, TakeoffAction, TakeoffGoal, LandAction, LandGoal, WaypointsAction, WaypointsGoal, HomePosition +from mavros_msgs.srv import CommandBool, SetMode, CommandTOL, WaypointPush, WaypointClear, CommandHome +from sensor_msgs.msg import NavSatFix +from geometry_msgs.msg import PoseStamped +from mavros_test_common import MavrosTestCommon +from pymavlink import mavutil +from threading import Thread + +# Brings in the SimpleActionClient +import actionlib + +class offboard(): + + def state_callback(self, data): + self.state = data + + def wp_reached_callback(self, data): + self.wp_reached = data + + def home_pos_callback(self, data): + self.home_pos = data + #print(self.home_pos.geo) + + def global_pos_callback(self, data): + self.global_pos = data + + def __init__(self): + + #Initialize ROSLaunch + launch = roslaunch.scriptapi.ROSLaunch() + launch.start() + uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) + roslaunch.configure_logging(uuid) + + #Launch MAVROS + cli_args = ('mavros', 'px4.launch') + roslaunch_file = [(roslaunch.rlutil.resolve_launch_arguments(cli_args)[0])] + mavros = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_file) + mavros.start() + + rospy.init_node('test_node', anonymous=True) + + print('got here') + + state_sub = rospy.Subscriber('mavros/state', State, self.state_callback) + self.state = State + #PUBLISHERS + local_pos_pub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10) + #global_pos_pub = rospy.Publisher('mavros/setpoint_position/global', GlobalPositionTarget, queue_size=10) + local_pos_sub = rospy.Subscriber('/mavros/global_position/global', NavSatFix, self.global_pos_callback) + home_pos_sub = rospy.Subscriber('/mavros/home_position/home', HomePosition, self.home_pos_callback) + + #ACTIONS + #init actionlib servers + server = Thread(target=action_server2.ActionServer) + server.setDaemon(True) + server.start() + takeoff_client = actionlib.SimpleActionClient('takeoff', TakeoffAction) + land_client = actionlib.SimpleActionClient('land', LandAction) + waypoints_client = actionlib.SimpleActionClient('waypoints', WaypointsAction) + + # need to simulate heartbeat to prevent datalink loss detection + hb_mav_msg = mavutil.mavlink.MAVLink_heartbeat_message(mavutil.mavlink.MAV_TYPE_GCS, 0, 0, 0, 0, 0) + hb_mav_msg.pack(mavutil.mavlink.MAVLink('', 2, 1)) + hb_ros_msg = mavlink.convert_to_rosmsg(hb_mav_msg) + hb_thread = Thread(target=self.send_heartbeat, args=(hb_ros_msg)) + hb_thread.setDaemon(True) + + #PREFLIGHT CHECK + + rate = rospy.Rate(30) + while (not self.state.connected): + print('Waiting on Connection') + rate.sleep() + print('Connected') + + time.sleep(5) + + goal = TakeoffGoal() + goal.height = 10 + + print('Actionlib started') + + takeoff_client.send_goal(goal) + takeoff_client.wait_for_result() + + wps = [] + wp1 = Waypoint() + wp2 = Waypoint() + wp1.command = 16 + wp1.x_lat = self.home_pos.geo.latitude - 0.00050 + wp1.y_long = self.home_pos.geo.longitude + wp1.z_alt = 10 + wp1.autocontinue = True + + wp2.command = 16 + wp2.x_lat = self.home_pos.geo.latitude + wp2.y_long = self.home_pos.geo.longitude + wp2.z_alt = 10 + wp2.autocontinue = True + + goal = WaypointsGoal() + goal.waypoints.append(wp1) + goal.waypoints.append(wp2) + print(goal) + waypoints_client.send_goal(goal) + waypoints_client.wait_for_result(rospy.Duration.from_sec(45.0)) + + time.sleep(5) + + goal = LandGoal() + goal.x_lat = self.home_pos.geo.latitude + goal.y_long = self.home_pos.geo.longitude + goal.z_alt = 0.0 + + print('Actionlib started') + + land_client.send_goal(goal) + land_client.wait_for_result(rospy.Duration.from_sec(30.0)) + sys.exit() + + + # Heartbeat must be sent to px4 at 2Hz or else auto disconnect + def send_heartbeat(self, hb_ros_msg): + rate = rospy.Rate(2) # Hz + while not rospy.is_shutdown(): + self.mavlink_pub.publish(hb_ros_msg) + try: # prevent garbage in console output when thread is killed + rate.sleep() + except rospy.ROSInterruptException: + pass + + +if __name__ == '__main__': + offboard() + + + + diff --git a/auto/python/offboard3.py b/auto/python/offboard3.py new file mode 100644 index 0000000000..eef9eb1d3b --- /dev/null +++ b/auto/python/offboard3.py @@ -0,0 +1,146 @@ +import rospy +import glob +import json +import math +import os +import px4tools +import sys +import time +import actionlib +import roslaunch +from mavros import mavlink +from mavros import action_server2 +from mavros_msgs.msg import Mavlink, Waypoint, WaypointReached, GlobalPositionTarget, State, TakeoffAction, TakeoffGoal, LandAction, LandGoal, WaypointsAction, WaypointsGoal, HomePosition +from mavros_msgs.srv import CommandBool, SetMode, CommandTOL, WaypointPush, WaypointClear, CommandHome +from sensor_msgs.msg import NavSatFix +from geometry_msgs.msg import PoseStamped +from mavros_test_common import MavrosTestCommon +from pymavlink import mavutil +from threading import Thread + +# Brings in the SimpleActionClient +import actionlib + +class offboard(): + + def state_callback(self, data): + self.state = data + + def wp_reached_callback(self, data): + self.wp_reached = data + + def home_pos_callback(self, data): + self.home_pos = data + #print(self.home_pos.geo) + + def global_pos_callback(self, data): + self.global_pos = data + + def __init__(self): + + rospy.init_node('guidance_node', anonymous=True) + + state_sub = rospy.Subscriber('mavros/state', State, self.state_callback) + self.state = State + #PUBLISHERS + local_pos_pub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10) + #global_pos_pub = rospy.Publisher('mavros/setpoint_position/global', GlobalPositionTarget, queue_size=10) + local_pos_sub = rospy.Subscriber('/mavros/global_position/global', NavSatFix, self.global_pos_callback) + home_pos_sub = rospy.Subscriber('/mavros/home_position/home', HomePosition, self.home_pos_callback) + + #ACTIONS + #init actionlib servers + server = Thread(target=action_server2.ActionServer) + server.setDaemon(True) + server.start() + takeoff_client = actionlib.SimpleActionClient('takeoff', TakeoffAction) + land_client = actionlib.SimpleActionClient('land', LandAction) + waypoints_client = actionlib.SimpleActionClient('waypoints', WaypointsAction) + + # need to simulate heartbeat to prevent datalink loss detection + hb_mav_msg = mavutil.mavlink.MAVLink_heartbeat_message(mavutil.mavlink.MAV_TYPE_GCS, 0, 0, 0, 0, 0) + hb_mav_msg.pack(mavutil.mavlink.MAVLink('', 2, 1)) + hb_ros_msg = mavlink.convert_to_rosmsg(hb_mav_msg) + hb_thread = Thread(target=self.send_heartbeat, args=(hb_ros_msg)) + hb_thread.setDaemon(True) + + #PREFLIGHT CHECK + + rate = rospy.Rate(30) + while (not self.state.connected): + print('Waiting on Connection') + rate.sleep() + print('Connected') + + time.sleep(5) + + goal = TakeoffGoal() + goal.height = 10 + + print('Actionlib started') + + takeoff_client.send_goal(goal) + takeoff_client.wait_for_result() + + wps = [] + wp1 = Waypoint() + wp2 = Waypoint() + wp3 = Waypoint() + wp1.command = 16 + wp1.x_lat = self.home_pos.geo.latitude - 0.00050 + wp1.y_long = self.home_pos.geo.longitude + wp1.z_alt = 10 + wp1.autocontinue = True + + wp2.command = 16 + wp2.x_lat = self.home_pos.geo.latitude - 0.00050 + wp2.y_long = self.home_pos.geo.longitude - 0.00050 + wp2.z_alt = 10 + wp2.autocontinue = True + + + wp3.command = 16 + wp3.x_lat = self.home_pos.geo.latitude + wp3.y_long = self.home_pos.geo.longitude + wp3.z_alt = 10 + wp3.autocontinue = True + + goal = WaypointsGoal() + goal.waypoints.append(wp1) + goal.waypoints.append(wp2) + goal.waypoints.append(wp3) + print(goal) + waypoints_client.send_goal(goal) + waypoints_client.wait_for_result(rospy.Duration.from_sec(45.0)) + + time.sleep(5) + + goal = LandGoal() + goal.x_lat = self.home_pos.geo.latitude + goal.y_long = self.home_pos.geo.longitude + goal.z_alt = 0.0 + + print('Actionlib started') + + land_client.send_goal(goal) + land_client.wait_for_result(rospy.Duration.from_sec(30.0)) + sys.exit() + + + # Heartbeat must be sent to px4 at 2Hz or else auto disconnect + def send_heartbeat(self, hb_ros_msg): + rate = rospy.Rate(2) # Hz + while not rospy.is_shutdown(): + self.mavlink_pub.publish(hb_ros_msg) + try: # prevent garbage in console output when thread is killed + rate.sleep() + except rospy.ROSInterruptException: + pass + + +if __name__ == '__main__': + offboard() + + + + diff --git a/auto/python/out.ply b/auto/python/out.ply new file mode 100644 index 0000000000..fea14b6142 --- /dev/null +++ b/auto/python/out.ply @@ -0,0 +1,5692 @@ +ply +format ascii 1.0 +element vertex 9977 +property float x +property float y +property float z +property uchar red +property uchar green +property uchar blue +end_header +-0.876712 3.945205 -5.885192 140 170 202 +-0.821918 3.945205 -5.885192 139 170 202 +-0.767123 3.945205 -5.885192 140 170 202 +-0.709898 3.931741 -5.865106 140 170 202 +-0.653061 3.918367 -5.845157 140 170 202 +-0.586667 3.840000 -5.728253 140 170 202 +-0.533333 3.840000 -5.728253 140 170 202 +-0.469055 3.752443 -5.597642 140 170 203 +-0.402516 3.622642 -5.404013 139 170 202 +-0.350000 3.600000 -5.370238 139 170 202 +-0.299065 3.588785 -5.353508 139 170 202 +-0.249221 3.588785 -5.353508 140 170 203 +-0.200000 3.600000 -5.370238 139 170 202 +-0.150000 3.600000 -5.370238 140 170 203 +-0.100000 3.600000 -5.370238 139 170 202 +-0.050000 3.600000 -5.370238 140 170 203 +0.000000 3.600000 -5.370238 140 170 203 +0.050000 3.600000 -5.370238 139 170 202 +0.100000 3.600000 -5.370238 140 170 202 +0.150000 3.600000 -5.370238 139 170 202 +0.200000 3.600000 -5.370238 140 170 202 +0.248447 3.577640 -5.336882 140 170 202 +0.298137 3.577640 -5.336882 140 170 202 +0.348910 3.588785 -5.353508 140 170 202 +0.398754 3.588785 -5.353508 140 170 202 +0.448598 3.588785 -5.353508 140 170 202 +0.498442 3.588785 -5.353508 140 170 202 +0.550000 3.600000 -5.370238 141 170 202 +0.600000 3.600000 -5.370238 140 170 202 +0.652038 3.611285 -5.387073 139 170 202 +0.702194 3.611285 -5.387073 138 169 202 +0.752351 3.611285 -5.387073 139 170 202 +0.802508 3.611285 -5.387073 138 169 201 +0.852665 3.611285 -5.387073 138 169 201 +0.902821 3.611285 -5.387073 138 169 202 +1.937500 4.500000 -6.712797 136 166 198 +2.000000 4.500000 -6.712797 135 165 198 +2.062500 4.500000 -6.712797 134 164 199 +2.125000 4.500000 -6.712797 132 163 198 +2.187500 4.500000 -6.712797 132 164 199 +2.250000 4.500000 -6.712797 131 163 198 +2.312500 4.500000 -6.712797 131 163 198 +2.375000 4.500000 -6.712797 131 163 198 +2.437500 4.500000 -6.712797 130 163 197 +2.500000 4.500000 -6.712797 130 162 197 +2.562500 4.500000 -6.712797 130 162 197 +2.625000 4.500000 -6.712797 129 161 197 +2.687500 4.500000 -6.712797 128 161 196 +2.750000 4.500000 -6.712797 128 161 196 +2.812500 4.500000 -6.712797 128 160 196 +2.875000 4.500000 -6.712797 128 160 196 +2.937500 4.500000 -6.712797 127 160 195 +3.000000 4.500000 -6.712797 126 159 194 +3.062500 4.500000 -6.712797 126 158 194 +3.125000 4.500000 -6.712797 126 158 194 +3.187500 4.500000 -6.712797 126 158 194 +3.250000 4.500000 -6.712797 126 158 193 +3.312500 4.500000 -6.712797 125 157 192 +3.375000 4.500000 -6.712797 124 156 192 +3.437500 4.500000 -6.712797 124 155 192 +3.500000 4.500000 -6.712797 122 155 191 +3.562500 4.500000 -6.712797 122 155 191 +3.625000 4.500000 -6.712797 122 155 190 +3.687500 4.500000 -6.712797 121 154 189 +3.750000 4.500000 -6.712797 121 153 189 +3.812500 4.500000 -6.712797 121 153 188 +3.875000 4.500000 -6.712797 120 152 187 +3.937500 4.500000 -6.712797 120 152 188 +4.000000 4.500000 -6.712797 119 151 187 +5.437500 4.500000 -6.712797 109 137 171 +5.500000 4.500000 -6.712797 108 137 171 +5.562500 4.500000 -6.712797 106 135 170 +5.625000 4.500000 -6.712797 105 134 170 +5.687500 4.500000 -6.712797 105 134 169 +5.750000 4.500000 -6.712797 105 134 169 +5.812500 4.500000 -6.712797 105 133 167 +5.875000 4.500000 -6.712797 104 132 167 +5.937500 4.500000 -6.712797 104 132 166 +6.000000 4.500000 -6.712797 104 131 165 +6.062500 4.500000 -6.712797 104 131 164 +6.125000 4.500000 -6.712797 102 129 163 +6.750000 4.500000 -6.712797 93 122 155 +6.812500 4.500000 -6.712797 93 122 155 +6.875000 4.500000 -6.712797 91 121 155 +6.937500 4.500000 -6.712797 90 120 153 +7.000000 4.500000 -6.712797 91 120 153 +7.062500 4.500000 -6.712797 90 119 152 +7.125000 4.500000 -6.712797 90 118 151 +7.187500 4.500000 -6.712797 88 118 150 +7.250000 4.500000 -6.712797 88 117 150 +7.312500 4.500000 -6.712797 86 117 149 +7.375000 4.500000 -6.712797 86 115 148 +7.437500 4.500000 -6.712797 86 114 148 +7.500000 4.500000 -6.712797 85 113 146 +7.562500 4.500000 -6.712797 85 112 145 +7.625000 4.500000 -6.712797 85 112 144 +-0.882759 3.917241 -5.925779 141 170 202 +-0.824742 3.903780 -5.905416 141 170 202 +-0.769759 3.903780 -5.905416 141 170 202 +-0.714777 3.903780 -5.905416 141 171 202 +-0.655290 3.877133 -5.865106 141 170 202 +-0.598639 3.863945 -5.845157 141 171 202 +-0.533333 3.786667 -5.728253 141 171 203 +-0.455696 3.594937 -5.438215 141 171 203 +-0.402516 3.572327 -5.404013 141 171 203 +-0.350000 3.550000 -5.370238 141 171 203 +-0.300000 3.550000 -5.370238 141 171 203 +-0.250000 3.550000 -5.370238 141 171 203 +-0.200000 3.550000 -5.370238 141 171 203 +-0.150000 3.550000 -5.370238 141 171 203 +-0.100000 3.550000 -5.370238 141 171 203 +-0.050000 3.550000 -5.370238 140 170 203 +0.000000 3.550000 -5.370238 141 171 203 +0.050000 3.550000 -5.370238 141 171 203 +0.100000 3.550000 -5.370238 141 171 203 +0.150000 3.550000 -5.370238 142 171 203 +0.200000 3.550000 -5.370238 141 171 203 +0.248447 3.527950 -5.336882 141 171 203 +0.298137 3.527950 -5.336882 142 171 203 +0.348910 3.538941 -5.353508 142 171 203 +0.398754 3.538941 -5.353508 142 171 203 +0.448598 3.538941 -5.353508 142 171 202 +0.498442 3.538941 -5.353508 142 171 202 +0.550000 3.550000 -5.370238 142 170 202 +0.601881 3.561129 -5.387073 141 170 202 +0.652038 3.561129 -5.387073 141 170 202 +0.702194 3.561129 -5.387073 140 170 202 +0.752351 3.561129 -5.387073 140 170 202 +0.802508 3.561129 -5.387073 140 170 202 +0.852665 3.561129 -5.387073 140 170 202 +0.902821 3.561129 -5.387073 139 170 202 +1.875000 4.437500 -6.712797 137 167 199 +1.937500 4.437500 -6.712797 137 167 199 +2.000000 4.437500 -6.712797 136 166 199 +2.062500 4.437500 -6.712797 135 166 199 +2.125000 4.437500 -6.712797 133 165 199 +2.187500 4.437500 -6.712797 133 165 199 +2.250000 4.437500 -6.712797 132 164 199 +2.312500 4.437500 -6.712797 132 163 199 +2.375000 4.437500 -6.712797 132 164 199 +2.437500 4.437500 -6.712797 131 163 198 +2.500000 4.437500 -6.712797 131 163 198 +2.562500 4.437500 -6.712797 131 163 197 +2.625000 4.437500 -6.712797 131 163 197 +2.687500 4.437500 -6.712797 130 162 197 +2.750000 4.437500 -6.712797 130 162 197 +2.812500 4.437500 -6.712797 129 161 196 +2.875000 4.437500 -6.712797 129 160 196 +2.937500 4.437500 -6.712797 129 161 196 +3.000000 4.437500 -6.712797 128 160 195 +3.062500 4.437500 -6.712797 127 160 194 +3.125000 4.437500 -6.712797 127 159 194 +3.187500 4.437500 -6.712797 127 159 194 +3.250000 4.437500 -6.712797 126 158 194 +3.312500 4.437500 -6.712797 126 158 193 +3.375000 4.437500 -6.712797 126 158 193 +3.437500 4.437500 -6.712797 126 158 193 +3.500000 4.437500 -6.712797 125 157 192 +3.562500 4.437500 -6.712797 125 156 192 +3.625000 4.437500 -6.712797 125 156 191 +3.687500 4.437500 -6.712797 124 155 190 +3.750000 4.437500 -6.712797 124 155 189 +3.812500 4.437500 -6.712797 124 154 189 +3.875000 4.437500 -6.712797 121 153 188 +3.937500 4.437500 -6.712797 121 154 189 +4.000000 4.437500 -6.712797 121 153 188 +4.062500 4.437500 -6.712797 120 152 187 +5.375000 4.437500 -6.712797 112 139 173 +5.437500 4.437500 -6.712797 109 138 172 +5.500000 4.437500 -6.712797 109 138 172 +5.562500 4.437500 -6.712797 108 136 171 +5.625000 4.437500 -6.712797 106 135 170 +5.687500 4.437500 -6.712797 106 135 170 +5.750000 4.437500 -6.712797 106 135 169 +5.812500 4.437500 -6.712797 105 134 168 +5.875000 4.437500 -6.712797 105 133 167 +5.937500 4.437500 -6.712797 105 132 166 +6.000000 4.437500 -6.712797 104 132 165 +6.062500 4.437500 -6.712797 104 131 164 +6.125000 4.437500 -6.712797 104 130 163 +6.187500 4.437500 -6.712797 102 130 163 +6.687500 4.437500 -6.712797 96 124 157 +6.750000 4.437500 -6.712797 95 124 156 +6.812500 4.437500 -6.712797 93 122 155 +6.875000 4.437500 -6.712797 93 122 155 +6.937500 4.437500 -6.712797 93 121 154 +7.000000 4.437500 -6.712797 93 121 154 +7.062500 4.437500 -6.712797 91 120 153 +7.125000 4.437500 -6.712797 90 119 152 +7.187500 4.437500 -6.712797 90 119 151 +7.250000 4.437500 -6.712797 88 118 150 +7.312500 4.437500 -6.712797 88 117 149 +7.375000 4.437500 -6.712797 88 115 148 +7.437500 4.437500 -6.712797 88 115 148 +7.500000 4.437500 -6.712797 88 115 147 +7.562500 4.437500 -6.712797 86 114 146 +-0.888889 3.888889 -5.966931 143 171 202 +-0.827586 3.862069 -5.925779 143 172 203 +-0.777778 3.888889 -5.966931 143 172 203 +-0.719723 3.875432 -5.946284 143 172 203 +-0.662069 3.862069 -5.925779 143 172 203 +-0.606897 3.862069 -5.925779 143 172 203 +-0.536913 3.758389 -5.766698 143 172 203 +-0.455696 3.544304 -5.438215 142 172 203 +-0.402516 3.522012 -5.404013 142 172 203 +-0.350000 3.500000 -5.370238 142 172 203 +-0.300000 3.500000 -5.370238 143 173 204 +-0.250000 3.500000 -5.370238 143 173 204 +-0.200000 3.500000 -5.370238 143 173 204 +-0.150000 3.500000 -5.370238 143 173 203 +-0.100000 3.500000 -5.370238 142 172 203 +-0.050000 3.500000 -5.370238 143 173 204 +0.000000 3.500000 -5.370238 142 173 203 +0.050000 3.500000 -5.370238 143 173 204 +0.100000 3.500000 -5.370238 143 173 204 +0.150000 3.500000 -5.370238 143 172 203 +0.200000 3.500000 -5.370238 143 172 203 +0.249221 3.489097 -5.353508 142 172 203 +0.299065 3.489097 -5.353508 143 172 203 +0.348910 3.489097 -5.353508 143 172 203 +0.398754 3.489097 -5.353508 142 172 203 +0.448598 3.489097 -5.353508 143 172 203 +0.500000 3.500000 -5.370238 144 173 203 +0.550000 3.500000 -5.370238 143 172 203 +0.601881 3.510972 -5.387073 143 172 203 +0.652038 3.510972 -5.387073 142 172 203 +0.702194 3.510972 -5.387073 141 171 203 +0.752351 3.510972 -5.387073 141 171 203 +0.802508 3.510972 -5.387073 142 171 203 +0.850000 3.500000 -5.370238 141 170 202 +0.900000 3.500000 -5.370238 141 171 202 +1.375000 4.375000 -6.712797 140 170 202 +1.437500 4.375000 -6.712797 139 170 202 +1.812500 4.375000 -6.712797 140 169 200 +1.875000 4.375000 -6.712797 139 168 200 +1.937500 4.375000 -6.712797 138 167 199 +2.000000 4.375000 -6.712797 138 167 200 +2.062500 4.375000 -6.712797 137 167 199 +2.125000 4.375000 -6.712797 136 167 200 +2.187500 4.375000 -6.712797 135 167 200 +2.250000 4.375000 -6.712797 134 166 199 +2.312500 4.375000 -6.712797 134 166 200 +2.375000 4.375000 -6.712797 133 165 199 +2.437500 4.375000 -6.712797 132 164 199 +2.500000 4.375000 -6.712797 132 164 199 +2.562500 4.375000 -6.712797 132 164 198 +2.625000 4.375000 -6.712797 132 163 198 +2.687500 4.375000 -6.712797 131 163 197 +2.750000 4.375000 -6.712797 131 163 197 +2.812500 4.375000 -6.712797 131 163 197 +2.875000 4.375000 -6.712797 130 162 196 +2.937500 4.375000 -6.712797 130 162 196 +3.000000 4.375000 -6.712797 129 161 196 +3.062500 4.375000 -6.712797 129 160 195 +3.125000 4.375000 -6.712797 129 160 195 +3.187500 4.375000 -6.712797 128 160 195 +3.250000 4.375000 -6.712797 128 160 195 +3.312500 4.375000 -6.712797 127 160 194 +3.375000 4.375000 -6.712797 127 159 194 +3.437500 4.375000 -6.712797 127 159 193 +3.500000 4.375000 -6.712797 126 158 193 +3.562500 4.375000 -6.712797 125 157 192 +3.625000 4.375000 -6.712797 125 156 192 +3.687500 4.375000 -6.712797 125 156 190 +3.750000 4.375000 -6.712797 125 155 190 +3.812500 4.375000 -6.712797 125 155 189 +3.875000 4.375000 -6.712797 124 155 189 +3.937500 4.375000 -6.712797 122 154 189 +4.000000 4.375000 -6.712797 122 154 189 +4.062500 4.375000 -6.712797 121 153 188 +4.125000 4.375000 -6.712797 121 153 188 +5.312500 4.375000 -6.712797 113 141 173 +5.375000 4.375000 -6.712797 112 140 173 +5.437500 4.375000 -6.712797 112 140 173 +5.500000 4.375000 -6.712797 110 138 172 +5.562500 4.375000 -6.712797 110 138 172 +5.625000 4.375000 -6.712797 109 137 170 +5.687500 4.375000 -6.712797 108 136 170 +5.750000 4.375000 -6.712797 108 136 170 +5.812500 4.375000 -6.712797 106 135 169 +5.875000 4.375000 -6.712797 105 133 167 +5.937500 4.375000 -6.712797 105 133 167 +6.000000 4.375000 -6.712797 105 133 166 +6.062500 4.375000 -6.712797 104 132 165 +6.125000 4.375000 -6.712797 104 132 165 +6.187500 4.375000 -6.712797 104 131 164 +6.625000 4.375000 -6.712797 98 126 159 +6.687500 4.375000 -6.712797 96 125 158 +6.750000 4.375000 -6.712797 96 125 157 +6.812500 4.375000 -6.712797 96 124 156 +6.875000 4.375000 -6.712797 95 122 155 +6.937500 4.375000 -6.712797 95 122 155 +7.000000 4.375000 -6.712797 93 121 154 +7.062500 4.375000 -6.712797 91 120 152 +7.125000 4.375000 -6.712797 91 120 152 +7.187500 4.375000 -6.712797 90 119 151 +7.250000 4.375000 -6.712797 91 119 150 +7.312500 4.375000 -6.712797 90 118 148 +7.375000 4.375000 -6.712797 90 118 148 +7.437500 4.375000 -6.712797 90 117 148 +7.500000 4.375000 -6.712797 88 115 146 +7.562500 4.375000 -6.712797 88 115 146 +-0.891986 3.846690 -5.987721 144 173 203 +-0.833333 3.833333 -5.966931 145 173 203 +-0.780488 3.846690 -5.987721 145 173 203 +-0.722222 3.833333 -5.966931 145 173 203 +-0.666667 3.833333 -5.966931 145 173 204 +-0.613240 3.846690 -5.987721 144 173 203 +-0.559441 3.860140 -6.008657 144 173 203 +-0.402516 3.471698 -5.404013 145 173 204 +-0.350000 3.450000 -5.370238 144 173 204 +-0.300000 3.450000 -5.370238 144 173 205 +-0.250000 3.450000 -5.370238 144 173 204 +-0.200000 3.450000 -5.370238 144 173 204 +-0.150000 3.450000 -5.370238 144 173 204 +-0.100000 3.450000 -5.370238 144 173 205 +-0.050000 3.450000 -5.370238 144 173 204 +0.000000 3.450000 -5.370238 144 173 204 +0.050000 3.450000 -5.370238 144 173 204 +0.100000 3.450000 -5.370238 144 173 205 +0.150000 3.450000 -5.370238 144 173 204 +0.200000 3.450000 -5.370238 144 173 204 +0.249221 3.439252 -5.353508 144 173 204 +0.299065 3.439252 -5.353508 144 173 205 +0.348910 3.439252 -5.353508 144 173 204 +0.398754 3.439252 -5.353508 144 173 204 +0.448598 3.439252 -5.353508 145 173 203 +0.500000 3.450000 -5.370238 145 173 203 +0.550000 3.450000 -5.370238 145 173 203 +0.600000 3.450000 -5.370238 144 173 203 +0.652038 3.460815 -5.387073 144 173 204 +0.702194 3.460815 -5.387073 143 173 203 +0.752351 3.460815 -5.387073 143 172 203 +0.800000 3.450000 -5.370238 143 173 203 +0.850000 3.450000 -5.370238 143 172 203 +0.900000 3.450000 -5.370238 143 172 203 +1.375000 4.312500 -6.712797 141 170 202 +1.437500 4.312500 -6.712797 141 170 202 +1.812500 4.312500 -6.712797 141 170 200 +1.875000 4.312500 -6.712797 140 169 200 +1.937500 4.312500 -6.712797 139 169 200 +2.000000 4.312500 -6.712797 139 168 200 +2.062500 4.312500 -6.712797 138 168 200 +2.125000 4.312500 -6.712797 137 167 200 +2.187500 4.312500 -6.712797 136 167 200 +2.250000 4.312500 -6.712797 136 167 200 +2.312500 4.312500 -6.712797 135 167 200 +2.375000 4.312500 -6.712797 135 167 200 +2.437500 4.312500 -6.712797 134 167 200 +2.500000 4.312500 -6.712797 134 166 200 +2.562500 4.312500 -6.712797 133 165 199 +2.625000 4.312500 -6.712797 133 164 198 +2.687500 4.312500 -6.712797 133 164 198 +2.750000 4.312500 -6.712797 132 164 198 +2.812500 4.312500 -6.712797 131 163 197 +2.875000 4.312500 -6.712797 132 163 197 +2.937500 4.312500 -6.712797 131 163 197 +3.000000 4.312500 -6.712797 131 163 197 +3.062500 4.312500 -6.712797 130 162 196 +3.125000 4.312500 -6.712797 130 161 196 +3.187500 4.312500 -6.712797 129 161 196 +3.250000 4.312500 -6.712797 129 161 195 +3.312500 4.312500 -6.712797 129 160 195 +3.375000 4.312500 -6.712797 128 160 194 +3.437500 4.312500 -6.712797 128 160 194 +3.500000 4.312500 -6.712797 128 160 194 +3.562500 4.312500 -6.712797 127 158 193 +3.625000 4.312500 -6.712797 126 157 192 +3.687500 4.312500 -6.712797 126 157 191 +3.750000 4.312500 -6.712797 127 157 190 +3.812500 4.312500 -6.712797 126 156 190 +3.875000 4.312500 -6.712797 125 155 190 +3.937500 4.312500 -6.712797 125 155 190 +4.000000 4.312500 -6.712797 124 155 189 +4.062500 4.312500 -6.712797 124 155 189 +4.125000 4.312500 -6.712797 124 154 189 +4.187500 4.312500 -6.712797 122 154 188 +5.250000 4.312500 -6.712797 115 143 175 +5.312500 4.312500 -6.712797 114 142 175 +5.375000 4.312500 -6.712797 114 141 174 +5.437500 4.312500 -6.712797 114 141 173 +5.500000 4.312500 -6.712797 114 141 173 +5.562500 4.312500 -6.712797 113 140 172 +5.625000 4.312500 -6.712797 113 139 171 +5.687500 4.312500 -6.712797 110 138 170 +5.750000 4.312500 -6.712797 109 137 170 +5.812500 4.312500 -6.712797 108 136 170 +5.875000 4.312500 -6.712797 108 135 169 +5.937500 4.312500 -6.712797 106 134 167 +6.000000 4.312500 -6.712797 106 134 167 +6.062500 4.312500 -6.712797 106 133 167 +6.125000 4.312500 -6.712797 105 132 165 +6.187500 4.312500 -6.712797 104 132 165 +6.250000 4.312500 -6.712797 102 131 163 +6.562500 4.312500 -6.712797 99 127 160 +6.625000 4.312500 -6.712797 99 127 160 +6.687500 4.312500 -6.712797 99 126 159 +6.750000 4.312500 -6.712797 98 126 158 +6.812500 4.312500 -6.712797 96 125 156 +6.875000 4.312500 -6.712797 98 125 156 +6.937500 4.312500 -6.712797 96 124 155 +7.000000 4.312500 -6.712797 96 122 154 +7.062500 4.312500 -6.712797 95 122 153 +7.125000 4.312500 -6.712797 95 121 152 +7.187500 4.312500 -6.712797 93 120 151 +7.250000 4.312500 -6.712797 93 120 150 +7.312500 4.312500 -6.712797 93 119 149 +7.375000 4.312500 -6.712797 91 118 148 +7.437500 4.312500 -6.712797 91 117 148 +7.500000 4.312500 -6.712797 90 117 147 +7.562500 4.312500 -6.712797 90 115 146 +-0.895105 3.804196 -6.008657 146 173 203 +-0.836237 3.790941 -5.987721 147 174 203 +-0.783217 3.804196 -6.008657 147 174 204 +-0.727273 3.804196 -6.008657 147 174 204 +-0.671329 3.804196 -6.008657 147 175 204 +-0.626335 3.871886 -6.115573 146 174 203 +-0.588235 4.000000 -6.317927 146 173 203 +-0.475836 4.044610 -6.388387 146 175 205 +-0.352201 3.421384 -5.404013 147 175 205 +-0.300000 3.400000 -5.370238 146 175 205 +-0.250000 3.400000 -5.370238 146 175 205 +-0.200000 3.400000 -5.370238 146 174 204 +-0.150000 3.400000 -5.370238 146 174 205 +-0.100000 3.400000 -5.370238 146 175 205 +-0.050000 3.400000 -5.370238 146 175 205 +0.000000 3.400000 -5.370238 146 175 205 +0.050000 3.400000 -5.370238 146 175 205 +0.100000 3.400000 -5.370238 145 174 205 +0.150000 3.400000 -5.370238 146 175 205 +0.200000 3.400000 -5.370238 146 175 205 +0.249221 3.389408 -5.353508 146 175 205 +0.299065 3.389408 -5.353508 146 175 205 +0.350000 3.400000 -5.370238 146 175 205 +0.400000 3.400000 -5.370238 145 174 205 +0.450000 3.400000 -5.370238 146 175 205 +0.500000 3.400000 -5.370238 145 174 204 +0.550000 3.400000 -5.370238 146 174 205 +0.600000 3.400000 -5.370238 145 174 205 +0.652038 3.410658 -5.387073 145 173 204 +0.702194 3.410658 -5.387073 145 174 205 +0.752351 3.410658 -5.387073 145 173 204 +0.800000 3.400000 -5.370238 145 173 204 +0.850000 3.400000 -5.370238 144 173 203 +1.312500 4.250000 -6.712797 144 173 203 +1.375000 4.250000 -6.712797 143 172 203 +1.437500 4.250000 -6.712797 142 172 203 +1.500000 4.250000 -6.712797 142 171 203 +1.750000 4.250000 -6.712797 142 171 202 +1.812500 4.250000 -6.712797 142 170 202 +1.875000 4.250000 -6.712797 141 170 201 +1.937500 4.250000 -6.712797 141 170 201 +2.000000 4.250000 -6.712797 140 169 200 +2.062500 4.250000 -6.712797 139 169 200 +2.125000 4.250000 -6.712797 138 169 200 +2.187500 4.250000 -6.712797 138 168 201 +2.250000 4.250000 -6.712797 136 168 200 +2.312500 4.250000 -6.712797 136 167 201 +2.375000 4.250000 -6.712797 136 168 200 +2.437500 4.250000 -6.712797 136 167 200 +2.500000 4.250000 -6.712797 135 167 200 +2.562500 4.250000 -6.712797 135 167 200 +2.625000 4.250000 -6.712797 134 166 199 +2.687500 4.250000 -6.712797 134 166 199 +2.750000 4.250000 -6.712797 133 165 199 +2.812500 4.250000 -6.712797 133 165 199 +2.875000 4.250000 -6.712797 132 164 197 +2.937500 4.250000 -6.712797 133 164 198 +3.000000 4.250000 -6.712797 132 163 197 +3.062500 4.250000 -6.712797 132 163 197 +3.125000 4.250000 -6.712797 131 163 196 +3.187500 4.250000 -6.712797 130 162 196 +3.250000 4.250000 -6.712797 131 162 196 +3.312500 4.250000 -6.712797 130 161 196 +3.375000 4.250000 -6.712797 129 161 195 +3.437500 4.250000 -6.712797 130 161 195 +3.500000 4.250000 -6.712797 129 160 195 +3.562500 4.250000 -6.712797 128 160 194 +3.625000 4.250000 -6.712797 128 159 193 +3.687500 4.250000 -6.712797 127 158 192 +3.750000 4.250000 -6.712797 127 157 191 +3.812500 4.250000 -6.712797 127 158 191 +3.875000 4.250000 -6.712797 127 157 191 +3.937500 4.250000 -6.712797 126 157 190 +4.000000 4.250000 -6.712797 126 156 190 +4.062500 4.250000 -6.712797 125 155 189 +4.125000 4.250000 -6.712797 125 155 189 +4.187500 4.250000 -6.712797 124 155 189 +5.187500 4.250000 -6.712797 118 145 176 +5.250000 4.250000 -6.712797 117 144 175 +5.312500 4.250000 -6.712797 115 143 175 +5.375000 4.250000 -6.712797 117 143 174 +5.437500 4.250000 -6.712797 117 142 173 +5.500000 4.250000 -6.712797 115 142 173 +5.562500 4.250000 -6.712797 114 141 172 +5.625000 4.250000 -6.712797 114 140 171 +5.687500 4.250000 -6.712797 114 140 171 +5.750000 4.250000 -6.712797 113 139 170 +5.812500 4.250000 -6.712797 110 137 170 +5.875000 4.250000 -6.712797 109 136 169 +5.937500 4.250000 -6.712797 109 136 169 +6.000000 4.250000 -6.712797 108 135 167 +6.062500 4.250000 -6.712797 106 134 167 +6.125000 4.250000 -6.712797 105 133 166 +6.187500 4.250000 -6.712797 106 133 166 +6.250000 4.250000 -6.712797 105 132 165 +6.312500 4.250000 -6.712797 104 131 164 +6.437500 4.250000 -6.712797 104 130 163 +6.500000 4.250000 -6.712797 102 129 161 +6.562500 4.250000 -6.712797 101 129 160 +6.625000 4.250000 -6.712797 101 128 160 +6.687500 4.250000 -6.712797 101 128 159 +6.750000 4.250000 -6.712797 99 127 158 +6.812500 4.250000 -6.712797 99 126 157 +6.875000 4.250000 -6.712797 99 126 156 +6.937500 4.250000 -6.712797 98 125 155 +7.000000 4.250000 -6.712797 98 124 154 +7.062500 4.250000 -6.712797 98 122 153 +7.125000 4.250000 -6.712797 96 122 152 +7.187500 4.250000 -6.712797 96 122 152 +7.250000 4.250000 -6.712797 95 120 150 +7.312500 4.250000 -6.712797 95 120 150 +7.375000 4.250000 -6.712797 95 120 149 +7.437500 4.250000 -6.712797 93 119 148 +7.500000 4.250000 -6.712797 93 118 148 +7.562500 4.250000 -6.712797 93 118 147 +-0.904594 3.787986 -6.072354 147 174 203 +-0.842105 3.761404 -6.029741 148 174 203 +-0.794326 3.801419 -6.093887 148 175 204 +-0.740214 3.814947 -6.115573 148 175 204 +-0.683274 3.814947 -6.115573 148 175 205 +-0.642336 3.912409 -6.271811 148 175 205 +-0.594796 3.985130 -6.388387 148 175 204 +-0.562500 4.187500 -6.712797 148 175 204 +-0.200000 3.350000 -5.370238 148 175 205 +-0.150000 3.350000 -5.370238 147 175 205 +-0.100000 3.350000 -5.370238 148 175 206 +-0.050000 3.350000 -5.370238 147 175 205 +0.000000 3.350000 -5.370238 147 175 205 +0.050000 3.350000 -5.370238 148 176 206 +0.100000 3.350000 -5.370238 148 176 206 +0.150000 3.350000 -5.370238 147 176 206 +0.200000 3.350000 -5.370238 148 176 206 +0.250000 3.350000 -5.370238 147 175 205 +0.300000 3.350000 -5.370238 148 175 206 +0.350000 3.350000 -5.370238 147 175 205 +0.400000 3.350000 -5.370238 148 175 205 +0.450000 3.350000 -5.370238 148 175 206 +1.312500 4.187500 -6.712797 145 173 203 +1.375000 4.187500 -6.712797 144 173 203 +1.437500 4.187500 -6.712797 144 173 203 +1.500000 4.187500 -6.712797 144 173 203 +1.562500 4.187500 -6.712797 143 172 203 +1.625000 4.187500 -6.712797 143 172 202 +1.687500 4.187500 -6.712797 145 173 202 +1.750000 4.187500 -6.712797 144 172 202 +1.812500 4.187500 -6.712797 144 172 202 +1.875000 4.187500 -6.712797 143 171 202 +1.937500 4.187500 -6.712797 143 171 202 +2.000000 4.187500 -6.712797 142 170 201 +2.062500 4.187500 -6.712797 140 170 200 +2.125000 4.187500 -6.712797 139 170 201 +2.187500 4.187500 -6.712797 138 170 202 +2.250000 4.187500 -6.712797 137 169 202 +2.312500 4.187500 -6.712797 137 169 201 +2.375000 4.187500 -6.712797 138 169 201 +2.437500 4.187500 -6.712797 137 168 201 +2.500000 4.187500 -6.712797 137 168 200 +2.562500 4.187500 -6.712797 136 167 200 +2.625000 4.187500 -6.712797 136 167 200 +2.687500 4.187500 -6.712797 135 167 199 +2.750000 4.187500 -6.712797 135 166 199 +2.812500 4.187500 -6.712797 135 167 199 +2.875000 4.187500 -6.712797 134 165 199 +2.937500 4.187500 -6.712797 134 165 199 +3.000000 4.187500 -6.712797 133 164 198 +3.062500 4.187500 -6.712797 133 165 198 +3.125000 4.187500 -6.712797 133 164 197 +3.187500 4.187500 -6.712797 132 163 197 +3.250000 4.187500 -6.712797 131 163 196 +3.312500 4.187500 -6.712797 132 163 196 +3.375000 4.187500 -6.712797 131 163 196 +3.437500 4.187500 -6.712797 131 162 196 +3.500000 4.187500 -6.712797 131 162 196 +3.562500 4.187500 -6.712797 129 160 194 +3.625000 4.187500 -6.712797 130 160 194 +3.687500 4.187500 -6.712797 129 160 193 +3.750000 4.187500 -6.712797 129 159 192 +3.812500 4.187500 -6.712797 127 158 192 +3.875000 4.187500 -6.712797 127 158 192 +3.937500 4.187500 -6.712797 127 157 191 +4.000000 4.187500 -6.712797 126 157 190 +4.062500 4.187500 -6.712797 126 157 190 +4.125000 4.187500 -6.712797 125 155 189 +4.187500 4.187500 -6.712797 125 155 189 +4.250000 4.187500 -6.712797 125 155 188 +5.125000 4.187500 -6.712797 120 147 178 +5.187500 4.187500 -6.712797 119 146 177 +5.250000 4.187500 -6.712797 118 146 176 +5.312500 4.187500 -6.712797 118 144 175 +5.375000 4.187500 -6.712797 118 145 175 +5.437500 4.187500 -6.712797 118 143 174 +5.500000 4.187500 -6.712797 117 143 173 +5.562500 4.187500 -6.712797 117 142 173 +5.625000 4.187500 -6.712797 117 142 172 +5.687500 4.187500 -6.712797 117 141 170 +5.750000 4.187500 -6.712797 115 140 170 +5.812500 4.187500 -6.712797 113 139 170 +5.875000 4.187500 -6.712797 110 137 170 +5.937500 4.187500 -6.712797 110 137 169 +6.000000 4.187500 -6.712797 109 136 168 +6.062500 4.187500 -6.712797 108 135 167 +6.125000 4.187500 -6.712797 108 134 167 +6.187500 4.187500 -6.712797 108 134 166 +6.250000 4.187500 -6.712797 105 132 165 +6.312500 4.187500 -6.712797 106 133 165 +6.375000 4.187500 -6.712797 105 132 163 +6.437500 4.187500 -6.712797 104 131 162 +6.500000 4.187500 -6.712797 104 130 162 +6.562500 4.187500 -6.712797 104 130 161 +6.625000 4.187500 -6.712797 104 129 160 +6.687500 4.187500 -6.712797 102 129 160 +6.750000 4.187500 -6.712797 101 127 158 +6.812500 4.187500 -6.712797 101 127 157 +6.875000 4.187500 -6.712797 101 126 156 +6.937500 4.187500 -6.712797 101 126 155 +7.000000 4.187500 -6.712797 101 126 155 +7.062500 4.187500 -6.712797 99 124 154 +7.125000 4.187500 -6.712797 99 124 153 +7.187500 4.187500 -6.712797 98 124 152 +7.250000 4.187500 -6.712797 98 122 151 +7.312500 4.187500 -6.712797 96 121 150 +7.375000 4.187500 -6.712797 96 121 150 +7.437500 4.187500 -6.712797 96 120 149 +7.500000 4.187500 -6.712797 95 119 148 +7.562500 4.187500 -6.712797 93 118 147 +-0.920863 3.798561 -6.181569 148 175 204 +-0.851064 3.744681 -6.093887 148 175 204 +-0.811594 3.826087 -6.226363 148 175 204 +-0.756364 3.840000 -6.249004 149 176 205 +-0.700730 3.854015 -6.271811 149 175 205 +-0.647059 3.882353 -6.317927 149 175 204 +-0.594796 3.925651 -6.388387 148 175 205 +-0.539326 3.955056 -6.436240 148 175 205 +-0.500000 4.125000 -6.712797 148 176 205 +-0.150000 3.300000 -5.370238 148 176 206 +-0.100000 3.300000 -5.370238 148 177 206 +-0.050000 3.300000 -5.370238 148 177 206 +0.000000 3.300000 -5.370238 148 177 206 +0.050000 3.300000 -5.370238 148 177 206 +0.100000 3.300000 -5.370238 148 177 206 +0.150000 3.300000 -5.370238 148 177 206 +0.200000 3.300000 -5.370238 148 177 206 +0.250000 3.300000 -5.370238 148 177 206 +0.300000 3.300000 -5.370238 148 176 206 +1.250000 4.125000 -6.712797 148 175 204 +1.312500 4.125000 -6.712797 147 174 204 +1.375000 4.125000 -6.712797 147 175 204 +1.437500 4.125000 -6.712797 145 173 203 +1.500000 4.125000 -6.712797 146 174 203 +1.562500 4.125000 -6.712797 145 173 203 +1.625000 4.125000 -6.712797 146 174 203 +1.687500 4.125000 -6.712797 146 173 202 +1.750000 4.125000 -6.712797 146 173 203 +1.812500 4.125000 -6.712797 145 173 202 +1.875000 4.125000 -6.712797 145 173 202 +1.937500 4.125000 -6.712797 144 172 202 +2.000000 4.125000 -6.712797 143 172 202 +2.062500 4.125000 -6.712797 141 171 202 +2.125000 4.125000 -6.712797 141 171 202 +2.187500 4.125000 -6.712797 140 170 202 +2.250000 4.125000 -6.712797 139 170 202 +2.312500 4.125000 -6.712797 139 170 202 +2.375000 4.125000 -6.712797 138 170 201 +2.437500 4.125000 -6.712797 139 170 202 +2.500000 4.125000 -6.712797 138 169 200 +2.562500 4.125000 -6.712797 138 169 200 +2.625000 4.125000 -6.712797 137 168 200 +2.687500 4.125000 -6.712797 137 168 200 +2.750000 4.125000 -6.712797 137 167 200 +2.812500 4.125000 -6.712797 136 167 199 +2.875000 4.125000 -6.712797 136 167 199 +2.937500 4.125000 -6.712797 135 166 199 +3.000000 4.125000 -6.712797 135 166 199 +3.062500 4.125000 -6.712797 134 165 198 +3.125000 4.125000 -6.712797 134 165 198 +3.187500 4.125000 -6.712797 133 164 197 +3.250000 4.125000 -6.712797 133 164 197 +3.312500 4.125000 -6.712797 133 164 197 +3.375000 4.125000 -6.712797 132 163 197 +3.437500 4.125000 -6.712797 132 163 197 +3.500000 4.125000 -6.712797 131 163 196 +3.562500 4.125000 -6.712797 131 162 196 +3.625000 4.125000 -6.712797 131 162 195 +3.687500 4.125000 -6.712797 130 160 194 +3.750000 4.125000 -6.712797 130 160 193 +3.812500 4.125000 -6.712797 129 160 192 +3.875000 4.125000 -6.712797 129 160 192 +3.937500 4.125000 -6.712797 129 159 192 +4.000000 4.125000 -6.712797 128 158 191 +4.062500 4.125000 -6.712797 127 158 191 +4.125000 4.125000 -6.712797 127 156 190 +4.187500 4.125000 -6.712797 127 156 189 +4.250000 4.125000 -6.712797 127 156 188 +5.125000 4.125000 -6.712797 121 148 178 +5.187500 4.125000 -6.712797 120 148 178 +5.250000 4.125000 -6.712797 119 146 177 +5.312500 4.125000 -6.712797 119 146 176 +5.375000 4.125000 -6.712797 119 146 175 +5.437500 4.125000 -6.712797 119 145 175 +5.500000 4.125000 -6.712797 119 144 174 +5.562500 4.125000 -6.712797 119 144 173 +5.625000 4.125000 -6.712797 120 144 172 +5.687500 4.125000 -6.712797 120 143 172 +5.750000 4.125000 -6.712797 119 143 171 +5.812500 4.125000 -6.712797 115 140 170 +5.875000 4.125000 -6.712797 113 139 170 +5.937500 4.125000 -6.712797 112 138 170 +6.000000 4.125000 -6.712797 109 137 169 +6.062500 4.125000 -6.712797 109 136 168 +6.125000 4.125000 -6.712797 109 136 167 +6.187500 4.125000 -6.712797 108 134 166 +6.250000 4.125000 -6.712797 108 134 166 +6.312500 4.125000 -6.712797 108 133 165 +6.375000 4.125000 -6.712797 108 133 164 +6.437500 4.125000 -6.712797 106 133 163 +6.500000 4.125000 -6.712797 106 131 162 +6.562500 4.125000 -6.712797 105 131 162 +6.625000 4.125000 -6.712797 104 129 160 +6.687500 4.125000 -6.712797 104 129 160 +6.750000 4.125000 -6.712797 104 129 159 +6.812500 4.125000 -6.712797 104 129 158 +6.875000 4.125000 -6.712797 102 127 156 +6.937500 4.125000 -6.712797 101 127 155 +7.000000 4.125000 -6.712797 101 126 155 +7.062500 4.125000 -6.712797 101 126 155 +7.125000 4.125000 -6.712797 99 125 153 +7.187500 4.125000 -6.712797 98 124 152 +7.250000 4.125000 -6.712797 98 122 152 +7.312500 4.125000 -6.712797 98 122 151 +7.375000 4.125000 -6.712797 96 121 149 +7.437500 4.125000 -6.712797 95 120 149 +7.500000 4.125000 -6.712797 95 120 148 +7.562500 4.125000 -6.712797 95 119 148 +-0.920863 3.741007 -6.181569 149 176 205 +-0.863309 3.741007 -6.181569 150 176 205 +-0.811594 3.768116 -6.226363 149 176 205 +-0.759124 3.795620 -6.271811 150 176 205 +-0.705882 3.823529 -6.317927 150 177 205 +-0.654275 3.866171 -6.388387 150 177 205 +-0.599251 3.895131 -6.436240 149 176 205 +-0.562500 4.062500 -6.712797 149 176 205 +-0.500000 4.062500 -6.712797 149 177 205 +-0.437500 4.062500 -6.712797 149 177 206 +-0.187500 4.062500 -6.712797 149 177 206 +-0.100000 3.250000 -5.370238 149 178 207 +-0.050000 3.250000 -5.370238 149 178 207 +0.000000 3.250000 -5.370238 149 178 206 +0.050000 3.250000 -5.370238 149 178 206 +0.100000 3.250000 -5.370238 149 178 207 +0.150000 3.250000 -5.370238 149 178 206 +0.200000 3.250000 -5.370238 149 178 206 +0.937500 4.062500 -6.712797 148 176 205 +1.000000 4.062500 -6.712797 149 177 206 +1.062500 4.062500 -6.712797 149 176 205 +1.187500 4.062500 -6.712797 148 175 205 +1.250000 4.062500 -6.712797 148 176 205 +1.312500 4.062500 -6.712797 148 175 205 +1.375000 4.062500 -6.712797 148 175 204 +1.437500 4.062500 -6.712797 147 175 204 +1.500000 4.062500 -6.712797 147 175 204 +1.562500 4.062500 -6.712797 147 174 203 +1.625000 4.062500 -6.712797 148 175 203 +1.687500 4.062500 -6.712797 148 175 203 +1.750000 4.062500 -6.712797 148 174 203 +1.812500 4.062500 -6.712797 147 174 203 +1.875000 4.062500 -6.712797 146 173 203 +1.937500 4.062500 -6.712797 145 173 203 +2.000000 4.062500 -6.712797 144 173 203 +2.062500 4.062500 -6.712797 144 173 203 +2.125000 4.062500 -6.712797 143 172 203 +2.187500 4.062500 -6.712797 142 172 203 +2.250000 4.062500 -6.712797 141 171 202 +2.312500 4.062500 -6.712797 141 171 202 +2.375000 4.062500 -6.712797 140 170 202 +2.437500 4.062500 -6.712797 140 170 202 +2.500000 4.062500 -6.712797 139 170 201 +2.562500 4.062500 -6.712797 139 170 201 +2.625000 4.062500 -6.712797 139 170 201 +2.687500 4.062500 -6.712797 138 169 200 +2.750000 4.062500 -6.712797 138 169 200 +2.812500 4.062500 -6.712797 138 168 200 +2.875000 4.062500 -6.712797 137 167 200 +2.937500 4.062500 -6.712797 137 167 199 +3.000000 4.062500 -6.712797 137 167 199 +3.062500 4.062500 -6.712797 135 167 199 +3.125000 4.062500 -6.712797 135 166 199 +3.187500 4.062500 -6.712797 135 166 198 +3.250000 4.062500 -6.712797 135 166 198 +3.312500 4.062500 -6.712797 134 165 198 +3.375000 4.062500 -6.712797 133 164 197 +3.437500 4.062500 -6.712797 133 164 197 +3.500000 4.062500 -6.712797 133 163 197 +3.562500 4.062500 -6.712797 132 163 196 +3.625000 4.062500 -6.712797 132 163 196 +3.687500 4.062500 -6.712797 132 162 194 +3.750000 4.062500 -6.712797 131 161 194 +3.812500 4.062500 -6.712797 131 161 194 +3.875000 4.062500 -6.712797 130 161 194 +3.937500 4.062500 -6.712797 130 160 193 +4.000000 4.062500 -6.712797 129 160 192 +4.062500 4.062500 -6.712797 129 159 192 +4.125000 4.062500 -6.712797 129 158 190 +4.187500 4.062500 -6.712797 129 157 189 +4.250000 4.062500 -6.712797 129 157 189 +4.312500 4.062500 -6.712797 128 156 188 +5.062500 4.062500 -6.712797 124 149 179 +5.125000 4.062500 -6.712797 122 148 178 +5.187500 4.062500 -6.712797 121 148 178 +5.250000 4.062500 -6.712797 120 147 178 +5.312500 4.062500 -6.712797 120 147 178 +5.375000 4.062500 -6.712797 119 146 176 +5.437500 4.062500 -6.712797 119 145 175 +5.500000 4.062500 -6.712797 120 146 175 +5.562500 4.062500 -6.712797 120 145 174 +5.625000 4.062500 -6.712797 120 145 173 +5.687500 4.062500 -6.712797 120 144 173 +5.750000 4.062500 -6.712797 119 143 171 +5.812500 4.062500 -6.712797 117 142 171 +5.875000 4.062500 -6.712797 114 140 170 +5.937500 4.062500 -6.712797 113 139 170 +6.000000 4.062500 -6.712797 112 138 169 +6.062500 4.062500 -6.712797 112 138 169 +6.125000 4.062500 -6.712797 112 137 168 +6.187500 4.062500 -6.712797 110 136 167 +6.250000 4.062500 -6.712797 110 136 167 +6.312500 4.062500 -6.712797 109 134 165 +6.375000 4.062500 -6.712797 108 134 164 +6.437500 4.062500 -6.712797 108 133 163 +6.500000 4.062500 -6.712797 106 132 163 +6.562500 4.062500 -6.712797 106 132 162 +6.625000 4.062500 -6.712797 106 131 161 +6.687500 4.062500 -6.712797 106 131 160 +6.750000 4.062500 -6.712797 105 130 159 +6.812500 4.062500 -6.712797 104 129 158 +6.875000 4.062500 -6.712797 104 129 158 +6.937500 4.062500 -6.712797 104 128 157 +7.000000 4.062500 -6.712797 101 127 155 +7.062500 4.062500 -6.712797 101 126 155 +7.125000 4.062500 -6.712797 99 125 154 +7.187500 4.062500 -6.712797 99 125 154 +7.250000 4.062500 -6.712797 99 124 153 +7.312500 4.062500 -6.712797 99 124 152 +7.375000 4.062500 -6.712797 98 122 151 +7.437500 4.062500 -6.712797 98 122 150 +7.500000 4.062500 -6.712797 96 121 149 +7.562500 4.062500 -6.712797 96 120 148 +-0.927536 3.710145 -6.226363 151 177 205 +-0.869565 3.710145 -6.226363 151 177 205 +-0.820513 3.750916 -6.294784 151 177 205 +-0.764706 3.764706 -6.317927 151 178 205 +-0.713755 3.806691 -6.388387 151 177 205 +-0.664151 3.864151 -6.484816 151 178 206 +-0.625000 4.000000 -6.712797 151 178 206 +-0.562500 4.000000 -6.712797 151 178 206 +-0.500000 4.000000 -6.712797 151 178 206 +-0.437500 4.000000 -6.712797 151 178 206 +-0.375000 4.000000 -6.712797 151 178 206 +-0.312500 4.000000 -6.712797 151 178 206 +-0.250000 4.000000 -6.712797 151 179 207 +0.050000 3.200000 -5.370238 150 179 207 +0.100000 3.200000 -5.370238 151 179 208 +0.150000 3.200000 -5.370238 150 179 207 +0.750000 4.000000 -6.712797 150 178 206 +0.812500 4.000000 -6.712797 150 178 206 +0.875000 4.000000 -6.712797 150 178 206 +0.937500 4.000000 -6.712797 150 178 206 +1.000000 4.000000 -6.712797 150 178 206 +1.062500 4.000000 -6.712797 149 177 206 +1.125000 4.000000 -6.712797 149 177 205 +1.187500 4.000000 -6.712797 149 177 206 +1.250000 4.000000 -6.712797 149 177 206 +1.312500 4.000000 -6.712797 148 177 205 +1.375000 4.000000 -6.712797 148 176 205 +1.437500 4.000000 -6.712797 149 176 205 +1.500000 4.000000 -6.712797 148 176 205 +1.562500 4.000000 -6.712797 148 176 205 +1.625000 4.000000 -6.712797 149 176 204 +1.687500 4.000000 -6.712797 149 175 203 +1.750000 4.000000 -6.712797 148 175 203 +1.812500 4.000000 -6.712797 148 175 203 +1.875000 4.000000 -6.712797 147 174 203 +1.937500 4.000000 -6.712797 147 174 203 +2.000000 4.000000 -6.712797 146 174 203 +2.062500 4.000000 -6.712797 145 173 203 +2.125000 4.000000 -6.712797 144 173 203 +2.187500 4.000000 -6.712797 144 173 203 +2.250000 4.000000 -6.712797 143 173 203 +2.312500 4.000000 -6.712797 143 172 202 +2.375000 4.000000 -6.712797 143 171 202 +2.437500 4.000000 -6.712797 142 171 201 +2.500000 4.000000 -6.712797 142 171 202 +2.562500 4.000000 -6.712797 141 171 202 +2.625000 4.000000 -6.712797 140 170 201 +2.687500 4.000000 -6.712797 140 170 201 +2.750000 4.000000 -6.712797 140 170 201 +2.812500 4.000000 -6.712797 139 170 200 +2.875000 4.000000 -6.712797 139 169 200 +2.937500 4.000000 -6.712797 138 169 200 +3.000000 4.000000 -6.712797 138 168 200 +3.062500 4.000000 -6.712797 137 167 199 +3.125000 4.000000 -6.712797 136 167 199 +3.187500 4.000000 -6.712797 136 166 198 +3.250000 4.000000 -6.712797 136 167 199 +3.312500 4.000000 -6.712797 135 166 198 +3.375000 4.000000 -6.712797 135 166 198 +3.437500 4.000000 -6.712797 135 165 197 +3.500000 4.000000 -6.712797 134 164 197 +3.562500 4.000000 -6.712797 134 164 197 +3.625000 4.000000 -6.712797 133 163 196 +3.687500 4.000000 -6.712797 133 163 196 +3.750000 4.000000 -6.712797 132 163 195 +3.812500 4.000000 -6.712797 132 163 195 +3.875000 4.000000 -6.712797 131 162 194 +3.937500 4.000000 -6.712797 131 161 194 +4.000000 4.000000 -6.712797 131 160 193 +4.062500 4.000000 -6.712797 131 160 192 +4.125000 4.000000 -6.712797 130 159 190 +4.187500 4.000000 -6.712797 130 159 190 +4.250000 4.000000 -6.712797 129 158 189 +4.312500 4.000000 -6.712797 128 157 189 +5.062500 4.000000 -6.712797 128 152 180 +5.125000 4.000000 -6.712797 126 150 179 +5.187500 4.000000 -6.712797 124 149 179 +5.250000 4.000000 -6.712797 121 148 178 +5.312500 4.000000 -6.712797 119 147 178 +5.375000 4.000000 -6.712797 119 147 177 +5.437500 4.000000 -6.712797 119 146 176 +5.500000 4.000000 -6.712797 119 146 175 +5.562500 4.000000 -6.712797 120 146 175 +5.625000 4.000000 -6.712797 121 146 174 +5.687500 4.000000 -6.712797 121 145 173 +5.750000 4.000000 -6.712797 119 144 172 +5.812500 4.000000 -6.712797 118 143 172 +5.875000 4.000000 -6.712797 117 142 171 +5.937500 4.000000 -6.712797 117 141 170 +6.000000 4.000000 -6.712797 115 140 170 +6.062500 4.000000 -6.712797 115 140 169 +6.125000 4.000000 -6.712797 114 139 168 +6.187500 4.000000 -6.712797 114 138 167 +6.250000 4.000000 -6.712797 112 137 167 +6.312500 4.000000 -6.712797 110 136 167 +6.375000 4.000000 -6.712797 109 135 165 +6.437500 4.000000 -6.712797 108 133 164 +6.500000 4.000000 -6.712797 108 133 163 +6.562500 4.000000 -6.712797 108 133 163 +6.625000 4.000000 -6.712797 108 132 162 +6.687500 4.000000 -6.712797 106 131 161 +6.750000 4.000000 -6.712797 106 131 160 +6.812500 4.000000 -6.712797 105 130 160 +6.875000 4.000000 -6.712797 104 129 158 +6.937500 4.000000 -6.712797 104 128 157 +7.000000 4.000000 -6.712797 102 128 157 +7.062500 4.000000 -6.712797 102 127 155 +7.125000 4.000000 -6.712797 101 126 155 +7.187500 4.000000 -6.712797 101 126 155 +7.250000 4.000000 -6.712797 101 125 154 +7.312500 4.000000 -6.712797 98 124 152 +7.375000 4.000000 -6.712797 98 124 152 +7.437500 4.000000 -6.712797 98 122 151 +7.500000 4.000000 -6.712797 96 121 150 +7.562500 4.000000 -6.712797 96 121 149 +-0.920863 3.625899 -6.181569 152 178 205 +-0.863309 3.625899 -6.181569 152 178 205 +-0.750000 3.937500 -6.712797 153 178 206 +-0.687500 3.937500 -6.712797 152 178 206 +-0.625000 3.937500 -6.712797 152 178 206 +-0.562500 3.937500 -6.712797 152 178 206 +-0.500000 3.937500 -6.712797 152 178 206 +-0.437500 3.937500 -6.712797 152 178 206 +-0.375000 3.937500 -6.712797 152 179 206 +-0.312500 3.937500 -6.712797 153 180 207 +0.050000 3.150000 -5.370238 152 180 208 +0.100000 3.150000 -5.370238 153 180 208 +0.150000 3.150000 -5.370238 152 180 208 +0.687500 3.937500 -6.712797 152 179 207 +0.750000 3.937500 -6.712797 152 179 207 +0.812500 3.937500 -6.712797 151 178 207 +0.875000 3.937500 -6.712797 151 178 206 +0.937500 3.937500 -6.712797 151 178 206 +1.000000 3.937500 -6.712797 151 178 206 +1.062500 3.937500 -6.712797 150 178 206 +1.125000 3.937500 -6.712797 150 178 206 +1.187500 3.937500 -6.712797 149 178 206 +1.250000 3.937500 -6.712797 149 178 206 +1.312500 3.937500 -6.712797 150 178 206 +1.375000 3.937500 -6.712797 150 178 206 +1.437500 3.937500 -6.712797 149 177 205 +1.500000 3.937500 -6.712797 149 177 205 +1.562500 3.937500 -6.712797 148 176 205 +1.625000 3.937500 -6.712797 150 177 205 +1.687500 3.937500 -6.712797 150 176 204 +1.750000 3.937500 -6.712797 150 177 205 +1.812500 3.937500 -6.712797 149 176 204 +1.875000 3.937500 -6.712797 149 176 204 +1.937500 3.937500 -6.712797 148 175 203 +2.000000 3.937500 -6.712797 148 175 203 +2.062500 3.937500 -6.712797 147 175 203 +2.125000 3.937500 -6.712797 147 174 203 +2.187500 3.937500 -6.712797 147 174 203 +2.250000 3.937500 -6.712797 146 173 203 +2.312500 3.937500 -6.712797 146 173 202 +2.375000 3.937500 -6.712797 146 173 202 +2.437500 3.937500 -6.712797 146 173 202 +2.500000 3.937500 -6.712797 144 173 202 +2.562500 3.937500 -6.712797 144 172 202 +2.625000 3.937500 -6.712797 142 171 201 +2.687500 3.937500 -6.712797 142 170 201 +2.750000 3.937500 -6.712797 142 171 201 +2.812500 3.937500 -6.712797 141 170 201 +2.875000 3.937500 -6.712797 140 170 200 +2.937500 3.937500 -6.712797 140 170 200 +3.000000 3.937500 -6.712797 139 169 200 +3.062500 3.937500 -6.712797 138 168 199 +3.125000 3.937500 -6.712797 139 168 199 +3.187500 3.937500 -6.712797 138 168 199 +3.250000 3.937500 -6.712797 137 167 199 +3.312500 3.937500 -6.712797 137 167 199 +3.375000 3.937500 -6.712797 136 167 198 +3.437500 3.937500 -6.712797 137 167 198 +3.500000 3.937500 -6.712797 136 166 197 +3.562500 3.937500 -6.712797 135 165 197 +3.625000 3.937500 -6.712797 135 164 196 +3.687500 3.937500 -6.712797 134 164 196 +3.750000 3.937500 -6.712797 133 163 196 +3.812500 3.937500 -6.712797 133 163 196 +3.875000 3.937500 -6.712797 133 163 196 +3.937500 3.937500 -6.712797 133 163 194 +4.000000 3.937500 -6.712797 131 161 193 +4.062500 3.937500 -6.712797 131 160 192 +4.125000 3.937500 -6.712797 131 160 191 +4.187500 3.937500 -6.712797 130 160 191 +4.250000 3.937500 -6.712797 130 159 190 +4.312500 3.937500 -6.712797 129 158 190 +4.375000 3.937500 -6.712797 129 157 189 +5.062500 3.937500 -6.712797 130 153 180 +5.125000 3.937500 -6.712797 128 152 180 +5.187500 3.937500 -6.712797 126 151 180 +5.250000 3.937500 -6.712797 122 149 179 +5.312500 3.937500 -6.712797 121 148 179 +5.375000 3.937500 -6.712797 121 148 178 +5.437500 3.937500 -6.712797 120 147 177 +5.500000 3.937500 -6.712797 120 147 176 +5.562500 3.937500 -6.712797 121 147 175 +5.625000 3.937500 -6.712797 121 147 175 +5.687500 3.937500 -6.712797 121 146 174 +5.750000 3.937500 -6.712797 119 144 173 +5.812500 3.937500 -6.712797 119 144 172 +5.875000 3.937500 -6.712797 119 144 172 +5.937500 3.937500 -6.712797 119 143 171 +6.000000 3.937500 -6.712797 119 143 170 +6.062500 3.937500 -6.712797 119 142 170 +6.125000 3.937500 -6.712797 118 141 168 +6.187500 3.937500 -6.712797 117 140 167 +6.250000 3.937500 -6.712797 115 139 167 +6.312500 3.937500 -6.712797 113 137 167 +6.375000 3.937500 -6.712797 110 136 166 +6.437500 3.937500 -6.712797 110 135 166 +6.500000 3.937500 -6.712797 109 134 164 +6.562500 3.937500 -6.712797 108 133 163 +6.625000 3.937500 -6.712797 108 133 163 +6.687500 3.937500 -6.712797 106 132 162 +6.750000 3.937500 -6.712797 106 131 161 +6.812500 3.937500 -6.712797 106 131 160 +6.875000 3.937500 -6.712797 105 130 160 +6.937500 3.937500 -6.712797 104 129 158 +7.000000 3.937500 -6.712797 104 129 158 +7.062500 3.937500 -6.712797 102 127 156 +7.125000 3.937500 -6.712797 101 127 155 +7.187500 3.937500 -6.712797 101 126 155 +7.250000 3.937500 -6.712797 99 125 154 +7.312500 3.937500 -6.712797 99 125 154 +7.375000 3.937500 -6.712797 98 125 153 +7.437500 3.937500 -6.712797 98 122 152 +7.500000 3.937500 -6.712797 96 122 151 +7.562500 3.937500 -6.712797 96 121 150 +-0.687500 3.875000 -6.712797 153 179 206 +-0.625000 3.875000 -6.712797 153 179 206 +-0.562500 3.875000 -6.712797 154 180 206 +-0.500000 3.875000 -6.712797 154 180 207 +-0.437500 3.875000 -6.712797 154 180 207 +-0.375000 3.875000 -6.712797 154 180 207 +-0.049689 3.080745 -5.336882 154 181 209 +0.000000 3.071208 -5.320359 154 181 209 +0.049536 3.071208 -5.320359 154 181 209 +0.099071 3.071208 -5.320359 154 181 209 +0.148607 3.071208 -5.320359 153 181 209 +0.937500 3.875000 -6.712797 152 180 208 +1.000000 3.875000 -6.712797 152 180 208 +1.062500 3.875000 -6.712797 152 180 208 +1.125000 3.875000 -6.712797 151 179 208 +1.187500 3.875000 -6.712797 151 179 207 +1.250000 3.875000 -6.712797 151 179 208 +1.312500 3.875000 -6.712797 151 179 207 +1.375000 3.875000 -6.712797 151 178 206 +1.437500 3.875000 -6.712797 150 178 206 +1.500000 3.875000 -6.712797 150 178 206 +1.562500 3.875000 -6.712797 150 178 206 +1.625000 3.875000 -6.712797 151 178 205 +1.687500 3.875000 -6.712797 151 178 205 +1.750000 3.875000 -6.712797 151 178 205 +1.812500 3.875000 -6.712797 150 177 204 +1.875000 3.875000 -6.712797 150 177 205 +1.937500 3.875000 -6.712797 150 176 204 +2.000000 3.875000 -6.712797 149 176 203 +2.062500 3.875000 -6.712797 149 175 203 +2.125000 3.875000 -6.712797 149 175 203 +2.187500 3.875000 -6.712797 149 175 203 +2.250000 3.875000 -6.712797 148 175 203 +2.312500 3.875000 -6.712797 148 175 203 +2.375000 3.875000 -6.712797 148 175 202 +2.437500 3.875000 -6.712797 148 174 202 +2.500000 3.875000 -6.712797 146 173 202 +2.562500 3.875000 -6.712797 146 173 202 +2.625000 3.875000 -6.712797 145 173 202 +2.687500 3.875000 -6.712797 144 172 201 +2.750000 3.875000 -6.712797 143 172 201 +2.812500 3.875000 -6.712797 142 171 201 +2.875000 3.875000 -6.712797 141 170 200 +2.937500 3.875000 -6.712797 141 170 201 +3.000000 3.875000 -6.712797 141 170 200 +3.062500 3.875000 -6.712797 140 170 200 +3.125000 3.875000 -6.712797 139 169 199 +3.187500 3.875000 -6.712797 139 169 199 +3.250000 3.875000 -6.712797 139 169 199 +3.312500 3.875000 -6.712797 139 168 199 +3.375000 3.875000 -6.712797 137 167 199 +3.437500 3.875000 -6.712797 137 167 199 +3.500000 3.875000 -6.712797 137 167 198 +3.562500 3.875000 -6.712797 137 167 197 +3.625000 3.875000 -6.712797 136 166 197 +3.687500 3.875000 -6.712797 136 166 197 +3.750000 3.875000 -6.712797 136 166 197 +3.812500 3.875000 -6.712797 135 165 196 +3.875000 3.875000 -6.712797 134 164 196 +3.937500 3.875000 -6.712797 133 163 195 +4.000000 3.875000 -6.712797 133 163 194 +4.062500 3.875000 -6.712797 133 162 193 +4.125000 3.875000 -6.712797 132 161 193 +4.187500 3.875000 -6.712797 131 160 192 +4.250000 3.875000 -6.712797 131 160 192 +4.312500 3.875000 -6.712797 131 160 191 +4.375000 3.875000 -6.712797 131 159 190 +4.437500 3.875000 -6.712797 131 158 189 +4.500000 3.875000 -6.712797 131 158 188 +4.562500 3.875000 -6.712797 131 158 187 +4.625000 3.875000 -6.712797 129 156 186 +4.687500 3.875000 -6.712797 130 156 186 +5.000000 3.875000 -6.712797 131 155 182 +5.062500 3.875000 -6.712797 131 155 181 +5.125000 3.875000 -6.712797 129 153 180 +5.187500 3.875000 -6.712797 126 151 180 +5.250000 3.875000 -6.712797 124 150 180 +5.312500 3.875000 -6.712797 122 149 180 +5.375000 3.875000 -6.712797 122 149 179 +5.437500 3.875000 -6.712797 121 148 178 +5.500000 3.875000 -6.712797 121 148 177 +5.562500 3.875000 -6.712797 121 147 176 +5.625000 3.875000 -6.712797 121 147 175 +5.687500 3.875000 -6.712797 121 147 175 +5.750000 3.875000 -6.712797 120 146 174 +5.812500 3.875000 -6.712797 121 146 173 +5.875000 3.875000 -6.712797 121 145 173 +5.937500 3.875000 -6.712797 120 144 172 +6.000000 3.875000 -6.712797 119 143 170 +6.062500 3.875000 -6.712797 119 142 170 +6.125000 3.875000 -6.712797 117 141 169 +6.187500 3.875000 -6.712797 115 140 168 +6.250000 3.875000 -6.712797 114 139 167 +6.312500 3.875000 -6.712797 112 138 167 +6.375000 3.875000 -6.712797 110 137 167 +6.437500 3.875000 -6.712797 110 136 166 +6.500000 3.875000 -6.712797 109 135 165 +6.562500 3.875000 -6.712797 108 134 164 +6.625000 3.875000 -6.712797 108 133 163 +6.687500 3.875000 -6.712797 106 133 163 +6.750000 3.875000 -6.712797 106 132 162 +6.812500 3.875000 -6.712797 106 131 161 +6.875000 3.875000 -6.712797 105 130 160 +6.937500 3.875000 -6.712797 104 130 159 +7.000000 3.875000 -6.712797 104 130 159 +7.062500 3.875000 -6.712797 102 128 158 +7.125000 3.875000 -6.712797 102 128 157 +7.187500 3.875000 -6.712797 101 127 156 +7.250000 3.875000 -6.712797 101 127 155 +7.312500 3.875000 -6.712797 99 125 155 +7.375000 3.875000 -6.712797 98 125 154 +7.437500 3.875000 -6.712797 98 124 153 +7.500000 3.875000 -6.712797 98 124 153 +7.562500 3.875000 -6.712797 96 122 151 +-0.625000 3.812500 -6.712797 155 180 206 +-0.562500 3.812500 -6.712797 155 180 207 +-0.500000 3.812500 -6.712797 155 180 207 +-0.437500 3.812500 -6.712797 155 181 208 +-0.099379 3.031056 -5.336882 155 181 208 +-0.049536 3.021672 -5.320359 155 182 209 +0.000000 3.012346 -5.303938 155 182 209 +0.049080 2.993865 -5.271399 155 182 209 +0.098160 2.993865 -5.271399 155 182 209 +0.147239 2.993865 -5.271399 155 182 209 +0.195122 2.975610 -5.239256 155 182 209 +0.242424 2.957576 -5.207503 155 182 209 +0.290909 2.957576 -5.207503 155 182 209 +0.339394 2.957576 -5.207503 155 182 209 +0.384384 2.930931 -5.160589 155 182 208 +1.062500 3.812500 -6.712797 153 180 208 +1.125000 3.812500 -6.712797 153 180 208 +1.187500 3.812500 -6.712797 152 180 208 +1.250000 3.812500 -6.712797 152 180 208 +1.312500 3.812500 -6.712797 152 180 208 +1.375000 3.812500 -6.712797 152 180 208 +1.437500 3.812500 -6.712797 152 180 207 +1.500000 3.812500 -6.712797 151 179 206 +1.562500 3.812500 -6.712797 151 178 206 +1.625000 3.812500 -6.712797 152 178 206 +1.687500 3.812500 -6.712797 152 178 205 +1.750000 3.812500 -6.712797 152 178 205 +1.812500 3.812500 -6.712797 152 178 205 +1.875000 3.812500 -6.712797 151 178 205 +1.937500 3.812500 -6.712797 152 178 205 +2.000000 3.812500 -6.712797 152 178 205 +2.062500 3.812500 -6.712797 151 177 204 +2.125000 3.812500 -6.712797 151 177 204 +2.187500 3.812500 -6.712797 151 176 203 +2.250000 3.812500 -6.712797 150 176 203 +2.312500 3.812500 -6.712797 150 175 203 +2.375000 3.812500 -6.712797 149 175 203 +2.437500 3.812500 -6.712797 148 175 203 +2.500000 3.812500 -6.712797 148 175 203 +2.562500 3.812500 -6.712797 147 174 202 +2.625000 3.812500 -6.712797 147 173 202 +2.687500 3.812500 -6.712797 146 173 202 +2.750000 3.812500 -6.712797 145 173 202 +2.812500 3.812500 -6.712797 145 173 202 +2.875000 3.812500 -6.712797 144 173 202 +2.937500 3.812500 -6.712797 143 172 202 +3.000000 3.812500 -6.712797 143 171 201 +3.062500 3.812500 -6.712797 143 171 200 +3.125000 3.812500 -6.712797 142 170 200 +3.187500 3.812500 -6.712797 141 170 200 +3.250000 3.812500 -6.712797 140 169 199 +3.312500 3.812500 -6.712797 140 170 200 +3.375000 3.812500 -6.712797 139 169 199 +3.437500 3.812500 -6.712797 139 168 199 +3.500000 3.812500 -6.712797 138 168 199 +3.562500 3.812500 -6.712797 138 167 199 +3.625000 3.812500 -6.712797 138 167 198 +3.687500 3.812500 -6.712797 137 167 197 +3.750000 3.812500 -6.712797 137 167 197 +3.812500 3.812500 -6.712797 136 166 197 +3.875000 3.812500 -6.712797 135 165 196 +3.937500 3.812500 -6.712797 135 165 196 +4.000000 3.812500 -6.712797 135 164 195 +4.062500 3.812500 -6.712797 134 164 195 +4.125000 3.812500 -6.712797 133 163 194 +4.187500 3.812500 -6.712797 133 163 193 +4.250000 3.812500 -6.712797 133 162 192 +4.312500 3.812500 -6.712797 132 161 192 +4.375000 3.812500 -6.712797 132 160 190 +4.437500 3.812500 -6.712797 132 160 190 +4.500000 3.812500 -6.712797 131 159 189 +4.562500 3.812500 -6.712797 130 158 188 +4.625000 3.812500 -6.712797 129 157 187 +4.687500 3.812500 -6.712797 130 157 187 +4.750000 3.812500 -6.712797 129 156 186 +4.812500 3.812500 -6.712797 129 156 186 +4.875000 3.812500 -6.712797 130 155 184 +4.937500 3.812500 -6.712797 130 155 184 +5.000000 3.812500 -6.712797 130 155 182 +5.062500 3.812500 -6.712797 130 155 182 +5.125000 3.812500 -6.712797 128 153 181 +5.187500 3.812500 -6.712797 127 152 181 +5.250000 3.812500 -6.712797 126 151 180 +5.312500 3.812500 -6.712797 125 151 180 +5.375000 3.812500 -6.712797 124 150 180 +5.437500 3.812500 -6.712797 124 149 179 +5.500000 3.812500 -6.712797 124 149 178 +5.562500 3.812500 -6.712797 122 148 178 +5.625000 3.812500 -6.712797 122 148 177 +5.687500 3.812500 -6.712797 121 147 175 +5.750000 3.812500 -6.712797 121 147 175 +5.812500 3.812500 -6.712797 121 146 174 +5.875000 3.812500 -6.712797 120 145 173 +5.937500 3.812500 -6.712797 121 145 173 +6.000000 3.812500 -6.712797 120 144 172 +6.062500 3.812500 -6.712797 118 143 170 +6.125000 3.812500 -6.712797 118 142 170 +6.187500 3.812500 -6.712797 115 141 169 +6.250000 3.812500 -6.712797 115 140 169 +6.312500 3.812500 -6.712797 113 139 168 +6.375000 3.812500 -6.712797 112 138 168 +6.437500 3.812500 -6.712797 112 137 167 +6.500000 3.812500 -6.712797 109 136 166 +6.562500 3.812500 -6.712797 109 135 165 +6.625000 3.812500 -6.712797 109 135 165 +6.687500 3.812500 -6.712797 108 134 163 +6.750000 3.812500 -6.712797 108 133 163 +6.812500 3.812500 -6.712797 108 133 162 +6.875000 3.812500 -6.712797 106 132 161 +6.937500 3.812500 -6.712797 105 131 160 +7.000000 3.812500 -6.712797 105 130 160 +7.062500 3.812500 -6.712797 104 130 159 +7.125000 3.812500 -6.712797 102 129 158 +7.187500 3.812500 -6.712797 102 129 158 +7.250000 3.812500 -6.712797 101 128 157 +7.312500 3.812500 -6.712797 99 127 156 +7.375000 3.812500 -6.712797 101 127 155 +7.437500 3.812500 -6.712797 99 125 155 +7.500000 3.812500 -6.712797 99 125 154 +7.562500 3.812500 -6.712797 98 124 153 +-0.562500 3.750000 -6.712797 156 181 207 +-0.500000 3.750000 -6.712797 158 181 208 +-0.437500 3.750000 -6.712797 155 181 208 +-0.099071 2.972136 -5.320359 155 182 209 +-0.049383 2.962963 -5.303938 156 182 209 +0.000000 2.953846 -5.287619 156 183 209 +0.049080 2.944785 -5.271399 156 182 209 +0.097859 2.935780 -5.255279 156 182 209 +0.145455 2.909091 -5.207503 156 182 209 +0.193353 2.900302 -5.191771 157 183 209 +0.240240 2.882883 -5.160589 156 182 209 +0.288288 2.882883 -5.160589 157 182 209 +0.335329 2.874252 -5.145138 157 182 209 +0.383234 2.874252 -5.145138 156 182 209 +0.431138 2.874252 -5.145138 156 182 209 +1.187500 3.750000 -6.712797 153 181 209 +1.250000 3.750000 -6.712797 153 181 209 +1.312500 3.750000 -6.712797 154 181 209 +1.375000 3.750000 -6.712797 153 180 208 +1.437500 3.750000 -6.712797 153 180 208 +1.500000 3.750000 -6.712797 153 180 208 +1.562500 3.750000 -6.712797 153 180 207 +1.625000 3.750000 -6.712797 154 180 206 +1.687500 3.750000 -6.712797 155 180 206 +1.750000 3.750000 -6.712797 154 180 206 +1.812500 3.750000 -6.712797 154 179 206 +1.875000 3.750000 -6.712797 154 179 206 +1.937500 3.750000 -6.712797 153 179 206 +2.000000 3.750000 -6.712797 153 178 205 +2.062500 3.750000 -6.712797 152 178 205 +2.125000 3.750000 -6.712797 152 178 205 +2.187500 3.750000 -6.712797 152 178 204 +2.250000 3.750000 -6.712797 152 178 204 +2.312500 3.750000 -6.712797 151 177 204 +2.375000 3.750000 -6.712797 149 176 203 +2.437500 3.750000 -6.712797 150 176 203 +2.500000 3.750000 -6.712797 149 175 203 +2.562500 3.750000 -6.712797 148 175 202 +2.625000 3.750000 -6.712797 148 175 203 +2.687500 3.750000 -6.712797 148 175 203 +2.750000 3.750000 -6.712797 147 174 202 +2.812500 3.750000 -6.712797 146 173 202 +2.875000 3.750000 -6.712797 145 173 202 +2.937500 3.750000 -6.712797 144 173 202 +3.000000 3.750000 -6.712797 144 172 202 +3.062500 3.750000 -6.712797 144 172 201 +3.125000 3.750000 -6.712797 144 172 200 +3.187500 3.750000 -6.712797 143 171 200 +3.250000 3.750000 -6.712797 142 171 200 +3.312500 3.750000 -6.712797 141 170 200 +3.375000 3.750000 -6.712797 141 170 200 +3.437500 3.750000 -6.712797 141 170 200 +3.500000 3.750000 -6.712797 140 170 200 +3.562500 3.750000 -6.712797 140 170 199 +3.625000 3.750000 -6.712797 139 169 199 +3.687500 3.750000 -6.712797 138 168 199 +3.750000 3.750000 -6.712797 138 167 198 +3.812500 3.750000 -6.712797 138 167 197 +3.875000 3.750000 -6.712797 137 167 197 +3.937500 3.750000 -6.712797 137 166 197 +4.000000 3.750000 -6.712797 136 165 196 +4.062500 3.750000 -6.712797 136 165 196 +4.125000 3.750000 -6.712797 134 163 194 +4.187500 3.750000 -6.712797 134 163 194 +4.250000 3.750000 -6.712797 134 163 194 +4.312500 3.750000 -6.712797 133 162 192 +4.375000 3.750000 -6.712797 132 161 192 +4.437500 3.750000 -6.712797 132 160 191 +4.500000 3.750000 -6.712797 132 160 190 +4.562500 3.750000 -6.712797 131 159 189 +4.625000 3.750000 -6.712797 131 159 189 +4.687500 3.750000 -6.712797 130 158 188 +4.750000 3.750000 -6.712797 130 157 187 +4.812500 3.750000 -6.712797 130 156 186 +4.875000 3.750000 -6.712797 130 156 185 +4.937500 3.750000 -6.712797 130 155 184 +5.000000 3.750000 -6.712797 130 155 184 +5.062500 3.750000 -6.712797 129 155 183 +5.125000 3.750000 -6.712797 128 154 182 +5.187500 3.750000 -6.712797 128 154 182 +5.250000 3.750000 -6.712797 127 153 182 +5.312500 3.750000 -6.712797 126 152 180 +5.375000 3.750000 -6.712797 126 151 180 +5.437500 3.750000 -6.712797 126 151 179 +5.500000 3.750000 -6.712797 124 149 178 +5.562500 3.750000 -6.712797 122 148 178 +5.625000 3.750000 -6.712797 122 148 178 +5.687500 3.750000 -6.712797 121 148 176 +5.750000 3.750000 -6.712797 121 147 175 +5.812500 3.750000 -6.712797 121 147 175 +5.875000 3.750000 -6.712797 121 146 173 +5.937500 3.750000 -6.712797 121 146 173 +6.000000 3.750000 -6.712797 120 145 173 +6.062500 3.750000 -6.712797 119 144 172 +6.125000 3.750000 -6.712797 118 143 171 +6.187500 3.750000 -6.712797 117 142 170 +6.250000 3.750000 -6.712797 115 141 170 +6.312500 3.750000 -6.712797 114 140 170 +6.375000 3.750000 -6.712797 113 139 169 +6.437500 3.750000 -6.712797 112 139 168 +6.500000 3.750000 -6.712797 112 138 168 +6.562500 3.750000 -6.712797 110 137 167 +6.625000 3.750000 -6.712797 109 135 165 +6.687500 3.750000 -6.712797 108 135 164 +6.750000 3.750000 -6.712797 108 134 164 +6.812500 3.750000 -6.712797 108 133 163 +6.875000 3.750000 -6.712797 108 133 163 +6.937500 3.750000 -6.712797 106 132 162 +7.000000 3.750000 -6.712797 105 132 161 +7.062500 3.750000 -6.712797 104 130 160 +7.125000 3.750000 -6.712797 104 130 160 +7.187500 3.750000 -6.712797 102 129 159 +7.250000 3.750000 -6.712797 102 129 159 +7.312500 3.750000 -6.712797 101 128 158 +7.375000 3.750000 -6.712797 101 127 156 +7.437500 3.750000 -6.712797 99 126 155 +7.500000 3.750000 -6.712797 99 126 155 +7.562500 3.750000 -6.712797 98 125 154 +-0.049536 2.922601 -5.320359 157 182 209 +0.000000 2.913580 -5.303938 157 183 209 +0.049080 2.895705 -5.271399 158 183 209 +0.097561 2.878049 -5.239256 158 183 209 +0.145015 2.851964 -5.191771 158 184 209 +0.191045 2.817910 -5.129779 158 183 209 +0.238095 2.809524 -5.114512 158 184 209 +0.285714 2.809524 -5.114512 159 184 209 +0.333333 2.809524 -5.114512 158 183 209 +0.380952 2.809524 -5.114512 158 183 209 +0.428571 2.809524 -5.114512 158 184 209 +0.476190 2.809524 -5.114512 157 184 209 +0.347826 1.865613 -3.396198 157 183 209 +0.378698 1.861933 -3.389499 157 184 210 +1.250000 3.687500 -6.712797 155 182 209 +1.312500 3.687500 -6.712797 155 182 209 +1.375000 3.687500 -6.712797 154 181 208 +1.437500 3.687500 -6.712797 154 181 208 +1.500000 3.687500 -6.712797 155 181 208 +1.562500 3.687500 -6.712797 154 180 207 +1.625000 3.687500 -6.712797 155 180 207 +1.687500 3.687500 -6.712797 155 181 207 +1.750000 3.687500 -6.712797 155 180 206 +1.812500 3.687500 -6.712797 155 180 206 +1.875000 3.687500 -6.712797 155 180 206 +1.937500 3.687500 -6.712797 154 180 206 +2.000000 3.687500 -6.712797 154 179 205 +2.062500 3.687500 -6.712797 154 179 205 +2.125000 3.687500 -6.712797 154 179 205 +2.187500 3.687500 -6.712797 153 178 205 +2.250000 3.687500 -6.712797 153 178 205 +2.312500 3.687500 -6.712797 152 178 205 +2.375000 3.687500 -6.712797 150 177 204 +2.437500 3.687500 -6.712797 151 177 204 +2.500000 3.687500 -6.712797 151 177 204 +2.562500 3.687500 -6.712797 150 176 203 +2.625000 3.687500 -6.712797 149 175 203 +2.687500 3.687500 -6.712797 148 175 203 +2.750000 3.687500 -6.712797 148 175 202 +2.812500 3.687500 -6.712797 147 174 202 +2.875000 3.687500 -6.712797 147 174 203 +2.937500 3.687500 -6.712797 147 174 203 +3.000000 3.687500 -6.712797 147 174 202 +3.062500 3.687500 -6.712797 146 173 202 +3.125000 3.687500 -6.712797 144 173 201 +3.187500 3.687500 -6.712797 144 173 201 +3.250000 3.687500 -6.712797 143 172 201 +3.312500 3.687500 -6.712797 144 172 202 +3.375000 3.687500 -6.712797 142 171 200 +3.437500 3.687500 -6.712797 142 171 200 +3.500000 3.687500 -6.712797 141 170 200 +3.562500 3.687500 -6.712797 141 170 200 +3.625000 3.687500 -6.712797 140 170 199 +3.687500 3.687500 -6.712797 140 169 199 +3.750000 3.687500 -6.712797 139 169 199 +3.812500 3.687500 -6.712797 139 168 198 +3.875000 3.687500 -6.712797 138 167 197 +3.937500 3.687500 -6.712797 138 167 197 +4.000000 3.687500 -6.712797 137 167 197 +4.062500 3.687500 -6.712797 137 167 196 +4.125000 3.687500 -6.712797 136 165 196 +4.187500 3.687500 -6.712797 135 164 195 +4.250000 3.687500 -6.712797 135 164 194 +4.312500 3.687500 -6.712797 135 164 194 +4.375000 3.687500 -6.712797 134 163 194 +4.437500 3.687500 -6.712797 133 162 192 +4.500000 3.687500 -6.712797 133 161 191 +4.562500 3.687500 -6.712797 133 160 190 +4.625000 3.687500 -6.712797 132 160 189 +4.687500 3.687500 -6.712797 132 160 189 +4.750000 3.687500 -6.712797 131 158 188 +4.812500 3.687500 -6.712797 131 158 186 +4.875000 3.687500 -6.712797 131 157 186 +4.937500 3.687500 -6.712797 130 157 186 +5.000000 3.687500 -6.712797 129 155 184 +5.062500 3.687500 -6.712797 128 155 184 +5.125000 3.687500 -6.712797 128 155 184 +5.187500 3.687500 -6.712797 128 154 182 +5.250000 3.687500 -6.712797 127 153 182 +5.312500 3.687500 -6.712797 127 152 181 +5.375000 3.687500 -6.712797 127 152 180 +5.437500 3.687500 -6.712797 126 151 180 +5.500000 3.687500 -6.712797 126 151 180 +5.562500 3.687500 -6.712797 125 150 178 +5.625000 3.687500 -6.712797 124 149 178 +5.687500 3.687500 -6.712797 124 148 177 +5.750000 3.687500 -6.712797 122 148 175 +5.812500 3.687500 -6.712797 124 148 175 +5.875000 3.687500 -6.712797 124 148 175 +5.937500 3.687500 -6.712797 122 147 174 +6.000000 3.687500 -6.712797 121 146 173 +6.062500 3.687500 -6.712797 120 145 173 +6.125000 3.687500 -6.712797 119 144 172 +6.187500 3.687500 -6.712797 118 143 171 +6.250000 3.687500 -6.712797 117 142 171 +6.312500 3.687500 -6.712797 114 141 170 +6.375000 3.687500 -6.712797 113 140 170 +6.875000 3.687500 -6.712797 108 134 163 +6.937500 3.687500 -6.712797 106 133 163 +7.000000 3.687500 -6.712797 106 132 163 +7.062500 3.687500 -6.712797 106 132 162 +7.125000 3.687500 -6.712797 105 132 161 +7.187500 3.687500 -6.712797 104 131 160 +7.250000 3.687500 -6.712797 104 130 160 +7.312500 3.687500 -6.712797 104 130 159 +7.375000 3.687500 -6.712797 102 128 158 +7.437500 3.687500 -6.712797 101 127 156 +7.500000 3.687500 -6.712797 101 127 155 +7.562500 3.687500 -6.712797 99 126 155 +0.145015 2.803625 -5.191771 159 184 209 +0.190476 2.761905 -5.114512 160 184 209 +0.237389 2.753709 -5.099336 160 184 209 +0.284866 2.753709 -5.099336 160 184 209 +0.332344 2.753709 -5.099336 160 184 210 +0.380952 2.761905 -5.114512 160 184 209 +0.428571 2.761905 -5.114512 160 184 210 +0.316832 1.837624 -3.402923 159 184 210 +0.347140 1.830375 -3.389499 159 184 210 +0.376471 1.819608 -3.369561 158 184 210 +0.407045 1.816047 -3.362967 158 184 210 +0.437500 1.812500 -3.356399 157 184 210 +1.312500 3.625000 -6.712797 156 183 209 +1.375000 3.625000 -6.712797 155 182 209 +1.687500 3.625000 -6.712797 156 182 207 +1.750000 3.625000 -6.712797 157 182 206 +1.812500 3.625000 -6.712797 157 181 206 +1.875000 3.625000 -6.712797 156 181 206 +1.937500 3.625000 -6.712797 155 180 206 +2.000000 3.625000 -6.712797 155 180 206 +2.062500 3.625000 -6.712797 155 180 206 +2.125000 3.625000 -6.712797 155 180 206 +2.187500 3.625000 -6.712797 154 179 205 +2.250000 3.625000 -6.712797 154 179 205 +2.312500 3.625000 -6.712797 153 178 205 +2.375000 3.625000 -6.712797 152 178 205 +2.437500 3.625000 -6.712797 152 178 205 +2.500000 3.625000 -6.712797 152 178 205 +2.562500 3.625000 -6.712797 150 177 204 +2.625000 3.625000 -6.712797 150 177 204 +2.687500 3.625000 -6.712797 149 176 203 +2.750000 3.625000 -6.712797 149 176 203 +2.812500 3.625000 -6.712797 148 175 203 +2.875000 3.625000 -6.712797 148 175 203 +2.937500 3.625000 -6.712797 148 175 203 +3.000000 3.625000 -6.712797 147 174 202 +3.062500 3.625000 -6.712797 147 174 202 +3.125000 3.625000 -6.712797 147 174 202 +3.187500 3.625000 -6.712797 145 173 202 +3.250000 3.625000 -6.712797 145 173 202 +3.312500 3.625000 -6.712797 145 173 202 +3.375000 3.625000 -6.712797 144 173 202 +3.437500 3.625000 -6.712797 144 173 201 +3.500000 3.625000 -6.712797 143 171 200 +3.562500 3.625000 -6.712797 143 172 200 +3.625000 3.625000 -6.712797 142 170 200 +3.687500 3.625000 -6.712797 141 170 199 +3.750000 3.625000 -6.712797 141 170 199 +3.812500 3.625000 -6.712797 141 170 199 +3.875000 3.625000 -6.712797 140 169 199 +3.937500 3.625000 -6.712797 140 168 198 +4.000000 3.625000 -6.712797 139 168 197 +4.062500 3.625000 -6.712797 139 167 197 +4.125000 3.625000 -6.712797 138 167 196 +4.187500 3.625000 -6.712797 137 166 196 +4.250000 3.625000 -6.712797 136 165 195 +4.312500 3.625000 -6.712797 137 166 195 +4.375000 3.625000 -6.712797 135 164 194 +4.437500 3.625000 -6.712797 135 163 192 +4.500000 3.625000 -6.712797 135 163 192 +4.562500 3.625000 -6.712797 134 161 190 +4.625000 3.625000 -6.712797 134 161 190 +4.687500 3.625000 -6.712797 133 160 189 +4.750000 3.625000 -6.712797 132 159 189 +4.812500 3.625000 -6.712797 132 159 188 +4.875000 3.625000 -6.712797 131 158 188 +4.937500 3.625000 -6.712797 130 157 186 +5.000000 3.625000 -6.712797 130 157 186 +5.062500 3.625000 -6.712797 129 156 186 +5.125000 3.625000 -6.712797 129 155 184 +5.187500 3.625000 -6.712797 129 155 184 +5.250000 3.625000 -6.712797 128 154 183 +5.312500 3.625000 -6.712797 128 154 182 +5.375000 3.625000 -6.712797 127 153 181 +5.437500 3.625000 -6.712797 127 152 180 +5.500000 3.625000 -6.712797 127 152 180 +5.562500 3.625000 -6.712797 125 151 179 +5.625000 3.625000 -6.712797 125 150 178 +5.687500 3.625000 -6.712797 125 149 177 +5.750000 3.625000 -6.712797 125 149 176 +5.812500 3.625000 -6.712797 125 148 175 +5.875000 3.625000 -6.712797 125 148 175 +5.937500 3.625000 -6.712797 124 148 175 +6.000000 3.625000 -6.712797 122 147 174 +6.062500 3.625000 -6.712797 121 146 173 +6.125000 3.625000 -6.712797 120 145 173 +6.187500 3.625000 -6.712797 118 144 172 +6.250000 3.625000 -6.712797 117 143 172 +6.937500 3.625000 -6.712797 109 135 165 +7.000000 3.625000 -6.712797 108 134 163 +7.062500 3.625000 -6.712797 108 133 163 +7.125000 3.625000 -6.712797 106 132 162 +7.187500 3.625000 -6.712797 106 132 161 +7.250000 3.625000 -6.712797 105 131 160 +7.312500 3.625000 -6.712797 104 130 159 +7.375000 3.625000 -6.712797 104 129 158 +7.437500 3.625000 -6.712797 102 128 157 +7.500000 3.625000 -6.712797 101 127 155 +7.562500 3.625000 -6.712797 101 127 155 +0.188791 2.690265 -5.069251 160 185 209 +0.235988 2.690265 -5.069251 161 186 210 +0.283186 2.690265 -5.069251 161 186 210 +0.332344 2.706231 -5.099336 161 186 210 +0.316832 1.805941 -3.402923 160 185 210 +0.347140 1.798817 -3.389499 160 186 210 +0.375734 1.784736 -3.362967 160 186 211 +0.406250 1.781250 -3.356399 160 185 211 +0.436647 1.777778 -3.349856 159 185 211 +0.467836 1.777778 -3.349856 160 186 211 +0.499025 1.777778 -3.349856 159 186 211 +1.687500 3.562500 -6.712797 158 182 207 +1.750000 3.562500 -6.712797 159 183 207 +1.812500 3.562500 -6.712797 158 182 207 +1.875000 3.562500 -6.712797 158 182 207 +1.937500 3.562500 -6.712797 158 182 207 +2.000000 3.562500 -6.712797 157 182 206 +2.062500 3.562500 -6.712797 156 182 206 +2.125000 3.562500 -6.712797 155 180 206 +2.187500 3.562500 -6.712797 155 180 206 +2.250000 3.562500 -6.712797 155 180 206 +2.312500 3.562500 -6.712797 154 180 205 +2.375000 3.562500 -6.712797 154 179 205 +2.437500 3.562500 -6.712797 154 179 205 +2.500000 3.562500 -6.712797 153 178 205 +2.562500 3.562500 -6.712797 152 178 205 +2.625000 3.562500 -6.712797 152 178 205 +2.687500 3.562500 -6.712797 151 178 205 +2.750000 3.562500 -6.712797 151 178 205 +2.812500 3.562500 -6.712797 149 177 203 +2.875000 3.562500 -6.712797 149 176 203 +2.937500 3.562500 -6.712797 149 176 203 +3.000000 3.562500 -6.712797 148 175 203 +3.062500 3.562500 -6.712797 148 175 203 +3.125000 3.562500 -6.712797 148 175 203 +3.187500 3.562500 -6.712797 148 175 203 +3.250000 3.562500 -6.712797 147 175 203 +3.312500 3.562500 -6.712797 147 174 202 +3.375000 3.562500 -6.712797 146 174 202 +3.437500 3.562500 -6.712797 146 173 202 +3.500000 3.562500 -6.712797 145 173 201 +3.562500 3.562500 -6.712797 145 173 201 +3.625000 3.562500 -6.712797 144 172 200 +3.687500 3.562500 -6.712797 143 171 200 +3.750000 3.562500 -6.712797 143 171 200 +3.812500 3.562500 -6.712797 142 170 199 +3.875000 3.562500 -6.712797 142 170 199 +3.937500 3.562500 -6.712797 141 169 198 +4.000000 3.562500 -6.712797 140 169 198 +4.062500 3.562500 -6.712797 140 169 198 +4.125000 3.562500 -6.712797 140 168 197 +4.187500 3.562500 -6.712797 139 167 197 +4.250000 3.562500 -6.712797 138 167 196 +4.312500 3.562500 -6.712797 137 166 195 +4.375000 3.562500 -6.712797 137 165 194 +4.437500 3.562500 -6.712797 136 164 193 +4.500000 3.562500 -6.712797 136 163 192 +4.562500 3.562500 -6.712797 136 163 192 +4.625000 3.562500 -6.712797 135 162 191 +4.687500 3.562500 -6.712797 134 161 190 +4.750000 3.562500 -6.712797 134 161 190 +4.812500 3.562500 -6.712797 133 160 189 +4.875000 3.562500 -6.712797 132 160 188 +4.937500 3.562500 -6.712797 132 159 188 +5.000000 3.562500 -6.712797 131 158 187 +5.062500 3.562500 -6.712797 131 158 187 +5.125000 3.562500 -6.712797 130 156 186 +5.187500 3.562500 -6.712797 130 156 185 +5.250000 3.562500 -6.712797 129 155 184 +5.312500 3.562500 -6.712797 129 155 184 +5.375000 3.562500 -6.712797 128 154 182 +5.437500 3.562500 -6.712797 128 154 181 +5.500000 3.562500 -6.712797 127 153 180 +5.562500 3.562500 -6.712797 127 152 180 +5.625000 3.562500 -6.712797 127 152 179 +5.687500 3.562500 -6.712797 127 151 178 +5.750000 3.562500 -6.712797 127 151 177 +5.812500 3.562500 -6.712797 126 149 176 +5.875000 3.562500 -6.712797 126 149 175 +5.937500 3.562500 -6.712797 125 148 175 +6.000000 3.562500 -6.712797 124 148 175 +6.062500 3.562500 -6.712797 121 147 173 +6.125000 3.562500 -6.712797 120 146 173 +6.187500 3.562500 -6.712797 120 145 173 +6.328358 3.402985 -6.412224 114 140 170 +6.364312 3.390335 -6.388387 112 139 168 +6.520755 3.441509 -6.484816 112 138 167 +6.937500 3.562500 -6.712797 109 136 165 +7.000000 3.562500 -6.712797 109 135 164 +7.062500 3.562500 -6.712797 108 134 163 +7.125000 3.562500 -6.712797 106 133 162 +7.187500 3.562500 -6.712797 106 132 161 +7.250000 3.562500 -6.712797 105 131 160 +7.312500 3.562500 -6.712797 105 131 160 +7.375000 3.562500 -6.712797 104 130 159 +7.437500 3.562500 -6.712797 102 129 157 +7.500000 3.562500 -6.712797 104 129 157 +0.186589 2.612245 -5.010134 163 186 210 +0.234604 2.627566 -5.039519 163 186 210 +0.282353 2.635294 -5.054341 163 186 210 +0.347140 1.767258 -3.389499 162 186 211 +0.376471 1.756863 -3.369561 161 186 211 +0.406250 1.750000 -3.356399 161 186 211 +0.436647 1.746589 -3.349856 160 186 211 +0.467836 1.746589 -3.349856 160 186 211 +0.499025 1.746589 -3.349856 160 186 211 +0.529183 1.743191 -3.343339 160 186 211 +0.560311 1.743191 -3.343339 160 186 211 +1.687500 3.500000 -6.712797 159 184 208 +1.750000 3.500000 -6.712797 160 184 208 +1.812500 3.500000 -6.712797 160 183 208 +1.875000 3.500000 -6.712797 159 183 207 +1.937500 3.500000 -6.712797 159 183 208 +2.000000 3.500000 -6.712797 159 183 208 +2.062500 3.500000 -6.712797 158 182 207 +2.312500 3.500000 -6.712797 156 181 206 +2.375000 3.500000 -6.712797 155 181 206 +2.437500 3.500000 -6.712797 155 180 206 +2.500000 3.500000 -6.712797 155 180 205 +2.562500 3.500000 -6.712797 154 179 205 +2.625000 3.500000 -6.712797 153 179 205 +2.687500 3.500000 -6.712797 153 179 205 +2.750000 3.500000 -6.712797 152 178 205 +2.812500 3.500000 -6.712797 152 178 205 +2.875000 3.500000 -6.712797 151 178 204 +2.937500 3.500000 -6.712797 150 177 203 +3.000000 3.500000 -6.712797 150 177 203 +3.062500 3.500000 -6.712797 149 176 203 +3.125000 3.500000 -6.712797 149 176 203 +3.187500 3.500000 -6.712797 148 175 203 +3.250000 3.500000 -6.712797 148 175 203 +3.312500 3.500000 -6.712797 148 175 202 +3.375000 3.500000 -6.712797 147 174 202 +3.437500 3.500000 -6.712797 147 174 202 +3.500000 3.500000 -6.712797 146 173 201 +3.562500 3.500000 -6.712797 146 173 201 +3.625000 3.500000 -6.712797 145 173 200 +3.687500 3.500000 -6.712797 145 173 200 +3.750000 3.500000 -6.712797 144 172 200 +3.812500 3.500000 -6.712797 144 171 200 +3.875000 3.500000 -6.712797 143 171 199 +3.937500 3.500000 -6.712797 143 171 199 +4.000000 3.500000 -6.712797 142 170 199 +4.062500 3.500000 -6.712797 141 169 198 +4.125000 3.500000 -6.712797 140 169 197 +4.187500 3.500000 -6.712797 141 169 197 +4.250000 3.500000 -6.712797 140 168 197 +4.312500 3.500000 -6.712797 139 167 196 +4.375000 3.500000 -6.712797 138 166 194 +4.437500 3.500000 -6.712797 138 165 194 +4.500000 3.500000 -6.712797 137 164 193 +4.562500 3.500000 -6.712797 136 163 192 +4.625000 3.500000 -6.712797 136 163 192 +4.687500 3.500000 -6.712797 136 163 191 +4.750000 3.500000 -6.712797 135 162 190 +4.812500 3.500000 -6.712797 134 161 189 +4.875000 3.500000 -6.712797 134 161 190 +4.937500 3.500000 -6.712797 133 160 189 +5.000000 3.500000 -6.712797 132 159 188 +5.062500 3.500000 -6.712797 132 159 187 +5.125000 3.500000 -6.712797 132 158 186 +5.187500 3.500000 -6.712797 131 157 186 +5.250000 3.500000 -6.712797 130 156 184 +5.312500 3.500000 -6.712797 130 156 184 +5.375000 3.500000 -6.712797 129 155 183 +5.437500 3.500000 -6.712797 129 155 182 +5.500000 3.500000 -6.712797 129 155 182 +5.562500 3.500000 -6.712797 128 154 180 +5.625000 3.500000 -6.712797 127 152 179 +5.687500 3.500000 -6.712797 128 153 179 +5.750000 3.500000 -6.712797 128 152 178 +5.812500 3.500000 -6.712797 127 150 177 +5.875000 3.500000 -6.712797 127 150 177 +5.937500 3.500000 -6.712797 126 149 176 +6.000000 3.500000 -6.712797 124 148 175 +6.062500 3.500000 -6.712797 124 148 175 +6.125000 3.500000 -6.712797 121 147 174 +6.187500 3.500000 -6.712797 120 146 173 +6.268657 3.343284 -6.412224 114 141 170 +6.304833 3.330855 -6.388387 114 141 170 +6.364312 3.330855 -6.388387 113 140 169 +6.423792 3.330855 -6.388387 112 139 168 +6.937500 3.500000 -6.712797 112 137 166 +7.000000 3.500000 -6.712797 110 136 165 +7.062500 3.500000 -6.712797 109 135 163 +7.125000 3.500000 -6.712797 109 134 163 +7.187500 3.500000 -6.712797 108 133 161 +7.250000 3.500000 -6.712797 108 133 161 +7.312500 3.500000 -6.712797 106 132 160 +7.375000 3.500000 -6.712797 105 131 159 +7.437500 3.500000 -6.712797 105 130 159 +7.500000 3.500000 -6.712797 104 129 157 +0.140762 2.580645 -5.039519 167 188 209 +0.186047 2.558140 -4.995570 163 186 210 +0.232558 2.558140 -4.995570 164 188 211 +0.378698 1.735700 -3.389499 163 188 211 +0.407843 1.725490 -3.369561 162 187 211 +0.437500 1.718750 -3.356399 162 187 211 +0.467836 1.715400 -3.349856 162 187 211 +0.498054 1.712062 -3.343339 162 187 212 +0.529183 1.712062 -3.343339 162 187 212 +0.560311 1.712062 -3.343339 161 187 211 +1.750000 3.437500 -6.712797 160 184 209 +1.812500 3.437500 -6.712797 161 184 208 +1.875000 3.437500 -6.712797 161 184 208 +1.937500 3.437500 -6.712797 160 184 208 +2.000000 3.437500 -6.712797 160 184 208 +2.062500 3.437500 -6.712797 160 183 208 +2.312500 3.437500 -6.712797 158 182 206 +2.375000 3.437500 -6.712797 157 182 206 +2.437500 3.437500 -6.712797 156 182 206 +2.500000 3.437500 -6.712797 156 181 206 +2.562500 3.437500 -6.712797 155 180 206 +2.625000 3.437500 -6.712797 155 180 206 +2.687500 3.437500 -6.712797 154 180 206 +2.750000 3.437500 -6.712797 153 179 205 +2.812500 3.437500 -6.712797 153 179 205 +2.875000 3.437500 -6.712797 152 178 205 +2.937500 3.437500 -6.712797 151 178 204 +3.000000 3.437500 -6.712797 151 178 204 +3.062500 3.437500 -6.712797 151 177 203 +3.125000 3.437500 -6.712797 150 177 203 +3.187500 3.437500 -6.712797 150 177 203 +3.250000 3.437500 -6.712797 150 177 203 +3.312500 3.437500 -6.712797 149 176 203 +3.375000 3.437500 -6.712797 148 175 202 +3.437500 3.437500 -6.712797 148 175 202 +3.500000 3.437500 -6.712797 148 175 202 +3.562500 3.437500 -6.712797 148 175 202 +3.625000 3.437500 -6.712797 147 174 201 +3.687500 3.437500 -6.712797 146 173 200 +3.750000 3.437500 -6.712797 146 173 200 +3.812500 3.437500 -6.712797 145 173 200 +3.875000 3.437500 -6.712797 145 172 200 +3.937500 3.437500 -6.712797 144 171 199 +4.000000 3.437500 -6.712797 144 171 199 +4.062500 3.437500 -6.712797 143 170 199 +4.125000 3.437500 -6.712797 142 170 198 +4.187500 3.437500 -6.712797 142 170 197 +4.250000 3.437500 -6.712797 142 169 197 +4.312500 3.437500 -6.712797 140 168 196 +4.375000 3.437500 -6.712797 140 167 195 +4.437500 3.437500 -6.712797 139 166 194 +4.500000 3.437500 -6.712797 138 166 193 +4.562500 3.437500 -6.712797 138 165 193 +4.625000 3.437500 -6.712797 137 164 192 +4.687500 3.437500 -6.712797 137 163 192 +4.750000 3.437500 -6.712797 136 163 190 +4.812500 3.437500 -6.712797 136 163 190 +4.875000 3.437500 -6.712797 135 162 190 +4.937500 3.437500 -6.712797 134 161 189 +5.000000 3.437500 -6.712797 133 160 189 +5.062500 3.437500 -6.712797 133 160 188 +5.125000 3.437500 -6.712797 133 160 187 +5.187500 3.437500 -6.712797 132 158 186 +5.250000 3.437500 -6.712797 132 158 186 +5.312500 3.437500 -6.712797 131 157 185 +5.375000 3.437500 -6.712797 131 157 184 +5.437500 3.437500 -6.712797 129 155 183 +5.500000 3.437500 -6.712797 129 155 182 +5.562500 3.437500 -6.712797 129 155 182 +5.625000 3.437500 -6.712797 129 154 180 +5.687500 3.437500 -6.712797 129 153 180 +5.750000 3.437500 -6.712797 128 152 178 +5.812500 3.437500 -6.712797 128 151 178 +5.875000 3.437500 -6.712797 128 151 177 +5.937500 3.437500 -6.712797 126 150 176 +6.000000 3.437500 -6.712797 125 149 175 +6.062500 3.437500 -6.712797 125 148 175 +6.125000 3.437500 -6.712797 122 148 175 +6.187500 3.437500 -6.712797 122 148 174 +6.268657 3.283582 -6.412224 117 143 171 +6.328358 3.283582 -6.412224 115 142 170 +6.364312 3.271375 -6.388387 114 141 170 +6.447761 3.283582 -6.412224 114 141 169 +6.812500 3.437500 -6.712797 113 140 168 +6.875000 3.437500 -6.712797 113 139 167 +6.937500 3.437500 -6.712797 112 137 166 +7.000000 3.437500 -6.712797 112 137 164 +7.062500 3.437500 -6.712797 112 136 163 +7.125000 3.437500 -6.712797 110 135 163 +7.187500 3.437500 -6.712797 109 133 161 +7.250000 3.437500 -6.712797 109 133 161 +7.312500 3.437500 -6.712797 106 132 160 +7.375000 3.437500 -6.712797 106 131 160 +7.437500 3.437500 -6.712797 106 131 159 +7.500000 3.437500 -6.712797 105 130 158 +0.047619 2.571429 -5.114512 165 187 210 +0.093842 2.533724 -5.039519 165 188 211 +0.139942 2.518950 -5.010134 165 188 211 +0.184971 2.497110 -4.966694 165 188 211 +0.231214 2.497110 -4.966694 166 189 211 +0.499025 1.684211 -3.349856 163 188 212 +0.529183 1.680934 -3.343339 163 189 213 +0.560311 1.680934 -3.343339 163 188 212 +1.750000 3.375000 -6.712797 161 185 209 +1.812500 3.375000 -6.712797 162 185 209 +1.875000 3.375000 -6.712797 162 185 208 +1.937500 3.375000 -6.712797 162 185 208 +2.000000 3.375000 -6.712797 162 185 208 +2.312500 3.375000 -6.712797 160 183 207 +2.375000 3.375000 -6.712797 159 183 207 +2.437500 3.375000 -6.712797 158 182 206 +2.500000 3.375000 -6.712797 157 182 206 +2.562500 3.375000 -6.712797 156 182 206 +2.625000 3.375000 -6.712797 155 181 206 +2.687500 3.375000 -6.712797 155 180 206 +2.750000 3.375000 -6.712797 155 180 206 +2.812500 3.375000 -6.712797 155 180 205 +2.875000 3.375000 -6.712797 154 180 205 +2.937500 3.375000 -6.712797 154 179 205 +3.000000 3.375000 -6.712797 154 179 205 +3.062500 3.375000 -6.712797 153 178 204 +3.125000 3.375000 -6.712797 152 178 204 +3.187500 3.375000 -6.712797 151 178 203 +3.250000 3.375000 -6.712797 151 178 203 +3.312500 3.375000 -6.712797 150 177 203 +3.375000 3.375000 -6.712797 150 176 203 +3.437500 3.375000 -6.712797 149 175 202 +3.500000 3.375000 -6.712797 148 175 202 +3.562500 3.375000 -6.712797 148 175 202 +3.625000 3.375000 -6.712797 148 174 201 +3.687500 3.375000 -6.712797 148 175 202 +3.750000 3.375000 -6.712797 148 174 201 +3.812500 3.375000 -6.712797 147 173 200 +3.875000 3.375000 -6.712797 146 173 200 +3.937500 3.375000 -6.712797 146 173 200 +4.000000 3.375000 -6.712797 145 172 199 +4.062500 3.375000 -6.712797 145 172 199 +4.125000 3.375000 -6.712797 144 171 199 +4.187500 3.375000 -6.712797 143 170 198 +4.250000 3.375000 -6.712797 143 170 197 +4.312500 3.375000 -6.712797 142 169 197 +4.375000 3.375000 -6.712797 141 168 196 +4.437500 3.375000 -6.712797 141 167 195 +4.500000 3.375000 -6.712797 140 167 194 +4.562500 3.375000 -6.712797 140 167 194 +4.625000 3.375000 -6.712797 139 166 193 +4.687500 3.375000 -6.712797 138 165 192 +4.750000 3.375000 -6.712797 138 164 192 +4.812500 3.375000 -6.712797 137 163 190 +4.875000 3.375000 -6.712797 136 163 190 +4.937500 3.375000 -6.712797 135 162 190 +5.000000 3.375000 -6.712797 135 162 189 +5.062500 3.375000 -6.712797 135 161 189 +5.125000 3.375000 -6.712797 134 160 187 +5.187500 3.375000 -6.712797 133 160 186 +5.250000 3.375000 -6.712797 133 159 186 +5.312500 3.375000 -6.712797 132 159 186 +5.375000 3.375000 -6.712797 131 157 184 +5.437500 3.375000 -6.712797 131 156 184 +5.500000 3.375000 -6.712797 131 156 183 +5.562500 3.375000 -6.712797 130 155 182 +5.625000 3.375000 -6.712797 131 155 181 +5.687500 3.375000 -6.712797 130 154 180 +5.750000 3.375000 -6.712797 130 154 179 +5.812500 3.375000 -6.712797 129 152 178 +5.875000 3.375000 -6.712797 129 152 178 +5.937500 3.375000 -6.712797 127 151 177 +6.000000 3.375000 -6.712797 127 150 176 +6.062500 3.375000 -6.712797 125 149 175 +6.125000 3.375000 -6.712797 125 149 175 +6.187500 3.375000 -6.712797 124 148 175 +6.315790 3.248120 -6.460436 118 144 171 +6.328358 3.223881 -6.412224 118 144 171 +6.388060 3.223881 -6.412224 117 143 170 +6.520755 3.260377 -6.484816 115 141 169 +6.875000 3.375000 -6.712797 114 139 166 +6.937500 3.375000 -6.712797 114 138 165 +7.000000 3.375000 -6.712797 113 137 163 +7.062500 3.375000 -6.712797 113 136 163 +7.125000 3.375000 -6.712797 112 135 162 +7.187500 3.375000 -6.712797 110 135 161 +7.250000 3.375000 -6.712797 110 134 160 +7.312500 3.375000 -6.712797 109 133 160 +7.375000 3.375000 -6.712797 108 133 160 +7.437500 3.375000 -6.712797 106 131 159 +7.500000 3.375000 -6.712797 106 131 158 +0.000000 2.523809 -5.114512 167 189 211 +0.047198 2.501475 -5.069251 167 189 211 +0.093842 2.486804 -5.039519 167 189 211 +0.139535 2.465116 -4.995570 166 189 211 +0.184971 2.450867 -4.966694 167 189 211 +0.229226 2.429799 -4.924000 167 189 211 +0.274286 2.422857 -4.909932 167 190 212 +0.529183 1.649805 -3.343339 163 189 212 +0.560311 1.649805 -3.343339 163 189 212 +1.812500 3.312500 -6.712797 163 186 209 +1.875000 3.312500 -6.712797 163 186 209 +2.375000 3.312500 -6.712797 160 184 208 +2.437500 3.312500 -6.712797 160 184 208 +2.500000 3.312500 -6.712797 158 182 207 +2.562500 3.312500 -6.712797 157 182 207 +2.625000 3.312500 -6.712797 157 182 207 +2.687500 3.312500 -6.712797 156 182 207 +2.750000 3.312500 -6.712797 155 181 206 +2.812500 3.312500 -6.712797 155 181 206 +2.875000 3.312500 -6.712797 155 181 206 +2.937500 3.312500 -6.712797 155 180 205 +3.000000 3.312500 -6.712797 155 180 205 +3.062500 3.312500 -6.712797 154 179 205 +3.125000 3.312500 -6.712797 154 179 205 +3.187500 3.312500 -6.712797 153 178 204 +3.250000 3.312500 -6.712797 152 178 203 +3.312500 3.312500 -6.712797 152 178 204 +3.375000 3.312500 -6.712797 151 177 203 +3.437500 3.312500 -6.712797 151 177 202 +3.500000 3.312500 -6.712797 151 177 203 +3.562500 3.312500 -6.712797 150 176 202 +3.625000 3.312500 -6.712797 149 175 202 +3.687500 3.312500 -6.712797 148 175 202 +3.750000 3.312500 -6.712797 148 175 201 +3.812500 3.312500 -6.712797 148 174 201 +3.875000 3.312500 -6.712797 148 174 200 +3.937500 3.312500 -6.712797 148 174 200 +4.000000 3.312500 -6.712797 147 173 200 +4.062500 3.312500 -6.712797 146 173 199 +4.125000 3.312500 -6.712797 145 172 199 +4.187500 3.312500 -6.712797 145 171 199 +4.250000 3.312500 -6.712797 144 170 197 +4.312500 3.312500 -6.712797 144 170 197 +4.375000 3.312500 -6.712797 143 170 196 +4.437500 3.312500 -6.712797 142 168 195 +4.500000 3.312500 -6.712797 142 168 195 +4.562500 3.312500 -6.712797 141 167 194 +4.625000 3.312500 -6.712797 140 167 193 +4.687500 3.312500 -6.712797 140 166 193 +4.750000 3.312500 -6.712797 139 165 192 +4.812500 3.312500 -6.712797 138 165 192 +4.875000 3.312500 -6.712797 138 163 190 +4.937500 3.312500 -6.712797 137 163 190 +5.000000 3.312500 -6.712797 137 163 189 +5.062500 3.312500 -6.712797 137 162 189 +5.125000 3.312500 -6.712797 136 161 188 +5.187500 3.312500 -6.712797 135 160 187 +5.250000 3.312500 -6.712797 134 160 186 +5.312500 3.312500 -6.712797 134 160 186 +5.375000 3.312500 -6.712797 133 159 185 +5.437500 3.312500 -6.712797 132 158 184 +5.500000 3.312500 -6.712797 132 157 183 +5.562500 3.312500 -6.712797 132 156 182 +5.625000 3.312500 -6.712797 133 156 181 +5.687500 3.312500 -6.712797 133 155 180 +5.750000 3.312500 -6.712797 132 155 180 +5.812500 3.312500 -6.712797 131 154 178 +5.875000 3.312500 -6.712797 130 153 178 +5.937500 3.312500 -6.712797 130 153 178 +6.000000 3.312500 -6.712797 129 152 177 +6.062500 3.312500 -6.712797 127 150 176 +6.125000 3.312500 -6.712797 126 149 175 +6.187500 3.312500 -6.712797 126 149 175 +6.250000 3.312500 -6.712797 124 148 174 +6.375940 3.187970 -6.460436 119 144 171 +6.460377 3.200000 -6.484816 117 142 170 +6.875000 3.312500 -6.712797 117 140 165 +6.937500 3.312500 -6.712797 117 140 165 +7.000000 3.312500 -6.712797 115 139 164 +7.062500 3.312500 -6.712797 114 137 163 +7.125000 3.312500 -6.712797 113 136 163 +7.187500 3.312500 -6.712797 112 135 161 +7.250000 3.312500 -6.712797 110 135 160 +7.312500 3.312500 -6.712797 110 133 160 +7.375000 3.312500 -6.712797 110 134 160 +7.437500 3.312500 -6.712797 109 133 160 +7.500000 3.312500 -6.712797 108 131 158 +0.000000 2.476191 -5.114512 167 189 211 +0.047198 2.454277 -5.069251 167 189 211 +0.093842 2.439883 -5.039519 167 189 211 +0.139535 2.418605 -4.995570 168 190 211 +0.183908 2.390805 -4.938149 168 190 212 +0.228571 2.377143 -4.909932 168 190 212 +0.273504 2.370370 -4.895943 169 190 212 +0.319088 2.370370 -4.895943 169 190 213 +2.437500 3.250000 -6.712797 160 184 208 +2.500000 3.250000 -6.712797 160 184 208 +2.562500 3.250000 -6.712797 159 183 208 +2.625000 3.250000 -6.712797 159 183 208 +2.687500 3.250000 -6.712797 158 183 207 +2.750000 3.250000 -6.712797 157 182 206 +2.812500 3.250000 -6.712797 157 182 206 +2.875000 3.250000 -6.712797 157 182 206 +2.937500 3.250000 -6.712797 156 181 206 +3.000000 3.250000 -6.712797 155 181 205 +3.062500 3.250000 -6.712797 155 180 205 +3.125000 3.250000 -6.712797 155 180 205 +3.187500 3.250000 -6.712797 154 180 205 +3.250000 3.250000 -6.712797 154 179 205 +3.312500 3.250000 -6.712797 153 179 204 +3.375000 3.250000 -6.712797 153 178 203 +3.437500 3.250000 -6.712797 153 178 203 +3.500000 3.250000 -6.712797 152 178 203 +3.562500 3.250000 -6.712797 151 177 203 +3.625000 3.250000 -6.712797 151 177 203 +3.687500 3.250000 -6.712797 151 177 203 +3.750000 3.250000 -6.712797 149 176 202 +3.812500 3.250000 -6.712797 149 176 202 +3.875000 3.250000 -6.712797 148 175 202 +3.937500 3.250000 -6.712797 148 175 201 +4.000000 3.250000 -6.712797 148 174 200 +4.062500 3.250000 -6.712797 148 174 200 +4.125000 3.250000 -6.712797 147 173 200 +4.187500 3.250000 -6.712797 146 172 199 +4.250000 3.250000 -6.712797 146 172 199 +4.312500 3.250000 -6.712797 145 171 197 +4.375000 3.250000 -6.712797 145 170 197 +4.437500 3.250000 -6.712797 144 170 196 +4.500000 3.250000 -6.712797 143 169 195 +4.562500 3.250000 -6.712797 142 168 194 +4.625000 3.250000 -6.712797 142 167 194 +4.687500 3.250000 -6.712797 141 167 193 +4.750000 3.250000 -6.712797 141 167 193 +4.812500 3.250000 -6.712797 140 166 192 +4.875000 3.250000 -6.712797 140 165 192 +4.937500 3.250000 -6.712797 139 164 190 +5.000000 3.250000 -6.712797 138 163 189 +5.062500 3.250000 -6.712797 138 163 189 +5.125000 3.250000 -6.712797 138 163 189 +5.187500 3.250000 -6.712797 136 162 188 +5.250000 3.250000 -6.712797 136 161 187 +5.312500 3.250000 -6.712797 135 160 186 +5.375000 3.250000 -6.712797 135 160 186 +5.437500 3.250000 -6.712797 134 159 184 +5.500000 3.250000 -6.712797 134 158 183 +5.562500 3.250000 -6.712797 135 158 182 +5.625000 3.250000 -6.712797 136 158 181 +5.687500 3.250000 -6.712797 135 157 180 +5.750000 3.250000 -6.712797 135 157 180 +5.812500 3.250000 -6.712797 134 156 180 +5.875000 3.250000 -6.712797 133 155 178 +5.937500 3.250000 -6.712797 131 154 178 +6.000000 3.250000 -6.712797 130 153 177 +6.062500 3.250000 -6.712797 129 152 177 +6.125000 3.250000 -6.712797 128 151 176 +6.187500 3.250000 -6.712797 128 151 175 +6.250000 3.250000 -6.712797 126 150 175 +6.875000 3.250000 -6.712797 119 142 167 +6.937500 3.250000 -6.712797 118 140 165 +7.000000 3.250000 -6.712797 117 139 164 +7.062500 3.250000 -6.712797 117 139 163 +7.125000 3.250000 -6.712797 115 138 163 +7.187500 3.250000 -6.712797 113 137 162 +7.250000 3.250000 -6.712797 113 136 161 +7.312500 3.250000 -6.712797 112 135 160 +7.375000 3.250000 -6.712797 110 134 159 +7.437500 3.250000 -6.712797 110 134 159 +7.500000 3.250000 -6.712797 109 132 157 +0.000000 2.428571 -5.114512 170 190 212 +0.047198 2.407080 -5.069251 170 190 212 +0.093567 2.385965 -5.024784 170 190 212 +0.138728 2.358382 -4.966694 170 191 212 +0.183908 2.344828 -4.938149 170 192 213 +0.228571 2.331429 -4.909932 170 191 213 +0.273504 2.324786 -4.895943 170 192 213 +0.319088 2.324786 -4.895943 170 192 213 +2.437500 3.187500 -6.712797 162 185 208 +2.500000 3.187500 -6.712797 161 185 208 +2.562500 3.187500 -6.712797 160 184 208 +2.625000 3.187500 -6.712797 160 184 208 +2.687500 3.187500 -6.712797 160 184 208 +2.750000 3.187500 -6.712797 159 184 208 +2.812500 3.187500 -6.712797 158 182 206 +2.875000 3.187500 -6.712797 158 182 206 +2.937500 3.187500 -6.712797 158 182 206 +3.000000 3.187500 -6.712797 157 182 206 +3.062500 3.187500 -6.712797 157 182 206 +3.125000 3.187500 -6.712797 156 181 206 +3.187500 3.187500 -6.712797 155 180 205 +3.250000 3.187500 -6.712797 155 180 205 +3.312500 3.187500 -6.712797 155 180 205 +3.375000 3.187500 -6.712797 154 180 205 +3.437500 3.187500 -6.712797 155 180 205 +3.500000 3.187500 -6.712797 154 179 204 +3.562500 3.187500 -6.712797 153 179 204 +3.625000 3.187500 -6.712797 153 179 204 +3.687500 3.187500 -6.712797 152 178 203 +3.750000 3.187500 -6.712797 151 178 203 +3.812500 3.187500 -6.712797 151 177 202 +3.875000 3.187500 -6.712797 150 177 202 +3.937500 3.187500 -6.712797 150 176 202 +4.000000 3.187500 -6.712797 150 176 202 +4.062500 3.187500 -6.712797 149 175 201 +4.125000 3.187500 -6.712797 148 174 200 +4.187500 3.187500 -6.712797 148 173 199 +4.250000 3.187500 -6.712797 148 173 199 +4.312500 3.187500 -6.712797 146 172 197 +4.375000 3.187500 -6.712797 146 172 197 +4.437500 3.187500 -6.712797 145 170 196 +4.500000 3.187500 -6.712797 145 170 196 +4.562500 3.187500 -6.712797 144 169 195 +4.625000 3.187500 -6.712797 144 168 194 +4.687500 3.187500 -6.712797 143 168 194 +4.750000 3.187500 -6.712797 142 167 192 +4.812500 3.187500 -6.712797 142 167 192 +4.875000 3.187500 -6.712797 142 167 192 +4.937500 3.187500 -6.712797 142 166 191 +5.000000 3.187500 -6.712797 141 165 190 +5.062500 3.187500 -6.712797 140 164 189 +5.125000 3.187500 -6.712797 139 163 189 +5.187500 3.187500 -6.712797 138 163 188 +5.250000 3.187500 -6.712797 138 162 187 +5.312500 3.187500 -6.712797 138 162 186 +5.375000 3.187500 -6.712797 137 161 186 +5.437500 3.187500 -6.712797 137 160 184 +5.500000 3.187500 -6.712797 139 161 184 +5.562500 3.187500 -6.712797 140 160 182 +5.625000 3.187500 -6.712797 140 160 182 +5.687500 3.187500 -6.712797 140 160 181 +5.750000 3.187500 -6.712797 138 159 180 +5.812500 3.187500 -6.712797 136 157 180 +5.875000 3.187500 -6.712797 135 156 179 +5.937500 3.187500 -6.712797 133 155 178 +6.000000 3.187500 -6.712797 132 155 178 +6.062500 3.187500 -6.712797 130 153 177 +6.125000 3.187500 -6.712797 129 152 176 +6.187500 3.187500 -6.712797 128 151 175 +6.812500 3.187500 -6.712797 120 143 167 +6.875000 3.187500 -6.712797 120 142 167 +6.937500 3.187500 -6.712797 120 142 166 +7.000000 3.187500 -6.712797 119 141 164 +7.062500 3.187500 -6.712797 118 140 163 +7.125000 3.187500 -6.712797 117 139 163 +7.187500 3.187500 -6.712797 117 138 162 +7.250000 3.187500 -6.712797 115 137 161 +7.312500 3.187500 -6.712797 114 136 160 +7.375000 3.187500 -6.712797 113 135 159 +7.437500 3.187500 -6.712797 113 135 159 +7.500000 3.187500 -6.712797 112 134 158 +0.000000 2.380952 -5.114512 171 192 213 +0.047198 2.359882 -5.069251 171 192 213 +0.093567 2.339181 -5.024784 170 191 212 +0.138728 2.312139 -4.966694 170 192 213 +0.184438 2.305475 -4.952381 171 192 213 +0.229226 2.292264 -4.924000 172 193 214 +0.273504 2.279202 -4.895943 171 192 213 +0.319088 2.279202 -4.895943 171 193 214 +2.500000 3.125000 -6.712797 163 186 209 +2.562500 3.125000 -6.712797 162 186 209 +2.625000 3.125000 -6.712797 161 185 208 +2.687500 3.125000 -6.712797 160 184 208 +2.750000 3.125000 -6.712797 160 184 208 +2.812500 3.125000 -6.712797 160 184 207 +2.875000 3.125000 -6.712797 160 184 207 +2.937500 3.125000 -6.712797 159 183 206 +3.000000 3.125000 -6.712797 159 183 206 +3.062500 3.125000 -6.712797 158 183 206 +3.125000 3.125000 -6.712797 157 182 206 +3.187500 3.125000 -6.712797 157 182 206 +3.250000 3.125000 -6.712797 156 182 206 +3.312500 3.125000 -6.712797 155 181 206 +3.375000 3.125000 -6.712797 156 181 206 +3.437500 3.125000 -6.712797 155 181 205 +3.500000 3.125000 -6.712797 155 180 205 +3.562500 3.125000 -6.712797 155 180 205 +3.625000 3.125000 -6.712797 155 180 205 +3.687500 3.125000 -6.712797 154 180 204 +3.750000 3.125000 -6.712797 153 179 204 +3.812500 3.125000 -6.712797 153 178 203 +3.875000 3.125000 -6.712797 152 178 203 +3.937500 3.125000 -6.712797 152 178 202 +4.000000 3.125000 -6.712797 151 177 202 +4.062500 3.125000 -6.712797 150 175 201 +4.125000 3.125000 -6.712797 149 175 200 +4.187500 3.125000 -6.712797 149 175 200 +4.250000 3.125000 -6.712797 148 174 199 +4.312500 3.125000 -6.712797 148 173 198 +4.375000 3.125000 -6.712797 148 173 198 +4.437500 3.125000 -6.712797 147 171 197 +4.500000 3.125000 -6.712797 147 171 196 +4.562500 3.125000 -6.712797 146 170 195 +4.625000 3.125000 -6.712797 146 170 195 +4.687500 3.125000 -6.712797 145 169 194 +4.750000 3.125000 -6.712797 145 168 192 +4.812500 3.125000 -6.712797 145 168 192 +4.875000 3.125000 -6.712797 144 167 192 +4.937500 3.125000 -6.712797 144 167 190 +5.000000 3.125000 -6.712797 143 166 190 +5.062500 3.125000 -6.712797 142 165 189 +5.125000 3.125000 -6.712797 141 164 189 +5.187500 3.125000 -6.712797 140 163 188 +5.250000 3.125000 -6.712797 140 163 187 +5.312500 3.125000 -6.712797 140 163 186 +5.375000 3.125000 -6.712797 140 162 186 +5.437500 3.125000 -6.712797 141 163 185 +5.500000 3.125000 -6.712797 142 163 184 +5.562500 3.125000 -6.712797 144 163 183 +5.625000 3.125000 -6.712797 144 163 183 +5.687500 3.125000 -6.712797 142 161 182 +5.750000 3.125000 -6.712797 140 160 181 +5.812500 3.125000 -6.712797 138 158 180 +5.875000 3.125000 -6.712797 136 157 180 +5.937500 3.125000 -6.712797 134 156 179 +6.000000 3.125000 -6.712797 133 155 178 +6.062500 3.125000 -6.712797 131 154 178 +6.125000 3.125000 -6.712797 130 153 177 +6.187500 3.125000 -6.712797 129 152 176 +6.812500 3.125000 -6.712797 122 145 168 +6.875000 3.125000 -6.712797 122 144 167 +6.937500 3.125000 -6.712797 121 144 167 +7.000000 3.125000 -6.712797 121 142 165 +7.062500 3.125000 -6.712797 121 142 164 +7.125000 3.125000 -6.712797 119 141 163 +7.187500 3.125000 -6.712797 119 140 163 +7.250000 3.125000 -6.712797 118 139 162 +7.312500 3.125000 -6.712797 117 138 160 +7.375000 3.125000 -6.712797 117 137 160 +7.437500 3.125000 -6.712797 115 136 159 +7.500000 3.125000 -6.712797 114 135 158 +0.000000 2.333333 -5.114512 172 192 213 +0.047198 2.312684 -5.069251 172 192 213 +0.093842 2.299120 -5.039519 172 192 213 +0.138728 2.265896 -4.966694 173 193 214 +0.184438 2.259366 -4.952381 173 194 214 +0.229885 2.252874 -4.938149 173 194 214 +0.274286 2.240000 -4.909932 172 194 214 +0.320000 2.240000 -4.909932 173 194 214 +2.500000 3.062500 -6.712797 164 187 209 +2.562500 3.062500 -6.712797 163 186 209 +2.625000 3.062500 -6.712797 163 186 209 +2.687500 3.062500 -6.712797 163 186 208 +2.750000 3.062500 -6.712797 162 185 208 +2.812500 3.062500 -6.712797 161 185 208 +2.875000 3.062500 -6.712797 161 185 208 +2.937500 3.062500 -6.712797 160 184 207 +3.000000 3.062500 -6.712797 160 184 207 +3.062500 3.062500 -6.712797 159 184 207 +3.125000 3.062500 -6.712797 159 183 207 +3.187500 3.062500 -6.712797 159 183 207 +3.250000 3.062500 -6.712797 158 183 207 +3.312500 3.062500 -6.712797 158 183 207 +3.375000 3.062500 -6.712797 157 183 206 +3.437500 3.062500 -6.712797 157 182 206 +3.500000 3.062500 -6.712797 157 182 206 +3.562500 3.062500 -6.712797 155 181 205 +3.625000 3.062500 -6.712797 155 181 205 +3.687500 3.062500 -6.712797 155 180 205 +3.750000 3.062500 -6.712797 155 180 205 +3.812500 3.062500 -6.712797 154 180 204 +3.875000 3.062500 -6.712797 154 179 203 +3.937500 3.062500 -6.712797 153 178 203 +4.000000 3.062500 -6.712797 152 178 202 +4.062500 3.062500 -6.712797 152 177 202 +4.125000 3.062500 -6.712797 151 176 201 +4.187500 3.062500 -6.712797 151 175 200 +4.250000 3.062500 -6.712797 150 175 199 +4.312500 3.062500 -6.712797 149 174 199 +4.375000 3.062500 -6.712797 149 173 198 +4.437500 3.062500 -6.712797 148 173 197 +4.500000 3.062500 -6.712797 148 172 197 +4.562500 3.062500 -6.712797 148 171 196 +4.625000 3.062500 -6.712797 147 170 194 +4.687500 3.062500 -6.712797 148 170 194 +4.750000 3.062500 -6.712797 148 170 193 +4.812500 3.062500 -6.712797 147 169 192 +4.875000 3.062500 -6.712797 147 169 192 +4.937500 3.062500 -6.712797 147 168 191 +5.000000 3.062500 -6.712797 145 167 190 +5.062500 3.062500 -6.712797 144 167 190 +5.125000 3.062500 -6.712797 144 166 189 +5.187500 3.062500 -6.712797 143 165 189 +5.250000 3.062500 -6.712797 142 164 188 +5.312500 3.062500 -6.712797 142 163 187 +5.375000 3.062500 -6.712797 142 163 186 +5.437500 3.062500 -6.712797 145 164 185 +5.500000 3.062500 -6.712797 145 164 184 +5.562500 3.062500 -6.712797 146 164 184 +5.625000 3.062500 -6.712797 145 163 183 +5.687500 3.062500 -6.712797 143 162 182 +5.750000 3.062500 -6.712797 141 160 182 +5.812500 3.062500 -6.712797 139 160 181 +5.875000 3.062500 -6.712797 137 158 180 +5.937500 3.062500 -6.712797 135 157 179 +6.000000 3.062500 -6.712797 135 156 179 +6.062500 3.062500 -6.712797 133 155 178 +6.125000 3.062500 -6.712797 131 154 177 +6.187500 3.062500 -6.712797 130 153 177 +6.812500 3.062500 -6.712797 124 146 169 +6.875000 3.062500 -6.712797 122 145 167 +6.937500 3.062500 -6.712797 122 144 167 +7.000000 3.062500 -6.712797 121 143 165 +7.062500 3.062500 -6.712797 121 143 165 +7.125000 3.062500 -6.712797 121 142 164 +7.187500 3.062500 -6.712797 120 141 163 +7.250000 3.062500 -6.712797 119 140 162 +7.312500 3.062500 -6.712797 118 139 161 +7.375000 3.062500 -6.712797 118 138 160 +7.437500 3.062500 -6.712797 118 137 160 +7.500000 3.062500 -6.712797 117 137 159 +0.047337 2.272189 -5.084249 174 194 214 +0.093842 2.252199 -5.039519 173 194 214 +0.138728 2.219653 -4.966694 174 194 214 +0.184438 2.213257 -4.952381 175 195 215 +0.229885 2.206897 -4.938149 174 195 215 +0.274286 2.194286 -4.909932 174 195 215 +0.320000 2.194286 -4.909932 173 194 214 +0.445570 1.944304 -4.350572 173 194 214 +2.500000 3.000000 -6.712797 165 188 209 +2.562500 3.000000 -6.712797 164 187 209 +2.625000 3.000000 -6.712797 164 187 209 +2.687500 3.000000 -6.712797 163 187 209 +2.750000 3.000000 -6.712797 163 186 208 +2.812500 3.000000 -6.712797 163 186 209 +2.875000 3.000000 -6.712797 163 186 208 +2.937500 3.000000 -6.712797 162 186 208 +3.000000 3.000000 -6.712797 161 185 208 +3.062500 3.000000 -6.712797 160 185 208 +3.125000 3.000000 -6.712797 160 185 208 +3.187500 3.000000 -6.712797 160 184 208 +3.250000 3.000000 -6.712797 160 184 208 +3.312500 3.000000 -6.712797 160 184 208 +3.375000 3.000000 -6.712797 159 184 207 +3.437500 3.000000 -6.712797 159 184 207 +3.500000 3.000000 -6.712797 158 183 206 +3.562500 3.000000 -6.712797 157 182 206 +3.625000 3.000000 -6.712797 157 182 205 +3.687500 3.000000 -6.712797 156 182 205 +3.750000 3.000000 -6.712797 155 180 205 +3.812500 3.000000 -6.712797 155 180 204 +3.875000 3.000000 -6.712797 155 180 204 +3.937500 3.000000 -6.712797 154 179 203 +4.000000 3.000000 -6.712797 154 178 203 +4.062500 3.000000 -6.712797 154 178 202 +4.125000 3.000000 -6.712797 153 177 202 +4.187500 3.000000 -6.712797 151 176 200 +4.250000 3.000000 -6.712797 151 175 200 +4.312500 3.000000 -6.712797 150 175 199 +4.375000 3.000000 -6.712797 150 174 198 +4.437500 3.000000 -6.712797 150 174 197 +4.500000 3.000000 -6.712797 149 173 197 +4.562500 3.000000 -6.712797 148 172 196 +4.625000 3.000000 -6.712797 149 172 196 +4.687500 3.000000 -6.712797 150 172 194 +4.750000 3.000000 -6.712797 149 171 194 +4.812500 3.000000 -6.712797 149 170 192 +4.875000 3.000000 -6.712797 148 170 192 +4.937500 3.000000 -6.712797 148 169 192 +5.000000 3.000000 -6.712797 148 169 191 +5.062500 3.000000 -6.712797 146 167 190 +5.125000 3.000000 -6.712797 146 167 189 +5.187500 3.000000 -6.712797 145 167 189 +5.250000 3.000000 -6.712797 145 167 189 +5.312500 3.000000 -6.712797 145 166 187 +5.375000 3.000000 -6.712797 146 166 187 +5.437500 3.000000 -6.712797 146 165 186 +5.500000 3.000000 -6.712797 146 165 185 +5.562500 3.000000 -6.712797 145 164 184 +5.625000 3.000000 -6.712797 144 163 184 +5.687500 3.000000 -6.712797 142 162 183 +5.750000 3.000000 -6.712797 140 160 182 +5.812500 3.000000 -6.712797 139 160 182 +5.875000 3.000000 -6.712797 138 160 181 +5.937500 3.000000 -6.712797 137 158 180 +6.000000 3.000000 -6.712797 135 157 179 +6.062500 3.000000 -6.712797 134 156 179 +6.125000 3.000000 -6.712797 132 155 178 +6.187500 3.000000 -6.712797 132 155 178 +6.812500 3.000000 -6.712797 125 147 169 +6.875000 3.000000 -6.712797 124 145 168 +6.937500 3.000000 -6.712797 122 145 167 +7.000000 3.000000 -6.712797 122 144 167 +7.062500 3.000000 -6.712797 121 143 166 +7.125000 3.000000 -6.712797 121 143 165 +7.187500 3.000000 -6.712797 120 142 163 +7.250000 3.000000 -6.712797 120 140 163 +7.312500 3.000000 -6.712797 119 140 162 +7.375000 3.000000 -6.712797 119 139 161 +7.437500 3.000000 -6.712797 118 138 160 +7.500000 3.000000 -6.712797 118 138 160 +0.093842 2.205279 -5.039519 175 195 215 +0.138728 2.173410 -4.966694 175 196 215 +0.184438 2.167147 -4.952381 175 196 215 +0.229885 2.160919 -4.938149 175 196 215 +0.274286 2.148571 -4.909932 175 196 215 +0.320000 2.148571 -4.909932 175 196 215 +0.443325 1.894207 -4.328655 175 195 214 +0.483627 1.894207 -4.328655 175 195 214 +2.500000 2.937500 -6.712797 167 189 210 +2.562500 2.937500 -6.712797 166 189 210 +2.625000 2.937500 -6.712797 166 189 210 +2.687500 2.937500 -6.712797 165 188 209 +2.750000 2.937500 -6.712797 165 188 209 +2.812500 2.937500 -6.712797 164 187 209 +2.875000 2.937500 -6.712797 163 187 209 +2.937500 2.937500 -6.712797 163 186 209 +3.000000 2.937500 -6.712797 163 186 209 +3.062500 2.937500 -6.712797 163 186 209 +3.125000 2.937500 -6.712797 162 186 209 +3.187500 2.937500 -6.712797 161 186 208 +3.250000 2.937500 -6.712797 161 185 208 +3.312500 2.937500 -6.712797 161 186 208 +3.375000 2.937500 -6.712797 160 184 208 +3.437500 2.937500 -6.712797 160 184 207 +3.500000 2.937500 -6.712797 159 184 207 +3.562500 2.937500 -6.712797 159 183 206 +3.625000 2.937500 -6.712797 159 183 206 +3.687500 2.937500 -6.712797 158 182 206 +3.750000 2.937500 -6.712797 157 182 205 +3.812500 2.937500 -6.712797 157 182 205 +3.875000 2.937500 -6.712797 156 180 204 +3.937500 2.937500 -6.712797 155 180 203 +4.000000 2.937500 -6.712797 155 180 203 +4.062500 2.937500 -6.712797 155 179 202 +4.125000 2.937500 -6.712797 154 178 202 +4.187500 2.937500 -6.712797 153 178 201 +4.250000 2.937500 -6.712797 153 177 200 +4.312500 2.937500 -6.712797 152 176 200 +4.375000 2.937500 -6.712797 151 175 199 +4.437500 2.937500 -6.712797 151 175 199 +4.500000 2.937500 -6.712797 150 174 197 +4.562500 2.937500 -6.712797 150 173 196 +4.625000 2.937500 -6.712797 151 173 196 +4.687500 2.937500 -6.712797 152 173 194 +4.750000 2.937500 -6.712797 151 172 194 +4.812500 2.937500 -6.712797 151 172 193 +4.875000 2.937500 -6.712797 150 171 193 +4.937500 2.937500 -6.712797 149 170 192 +5.000000 2.937500 -6.712797 148 170 191 +5.062500 2.937500 -6.712797 148 170 191 +5.125000 2.937500 -6.712797 148 168 190 +5.187500 2.937500 -6.712797 147 168 190 +5.250000 2.937500 -6.712797 147 167 189 +5.312500 2.937500 -6.712797 146 167 188 +5.375000 2.937500 -6.712797 146 166 187 +5.437500 2.937500 -6.712797 146 166 186 +5.500000 2.937500 -6.712797 144 164 186 +5.562500 2.937500 -6.712797 144 164 185 +5.625000 2.937500 -6.712797 143 163 184 +5.687500 2.937500 -6.712797 142 163 184 +5.750000 2.937500 -6.712797 141 161 183 +5.812500 2.937500 -6.712797 139 160 182 +5.875000 2.937500 -6.712797 139 160 182 +5.937500 2.937500 -6.712797 138 159 181 +6.000000 2.937500 -6.712797 136 158 180 +6.062500 2.937500 -6.712797 134 156 179 +6.125000 2.937500 -6.712797 134 156 179 +6.187500 2.937500 -6.712797 133 155 178 +6.250000 2.937500 -6.712797 131 154 177 +6.312500 2.937500 -6.712797 131 154 176 +6.375000 2.937500 -6.712797 130 153 175 +6.437500 2.937500 -6.712797 129 152 175 +6.750000 2.937500 -6.712797 127 148 171 +6.812500 2.937500 -6.712797 126 148 170 +6.875000 2.937500 -6.712797 125 147 169 +6.937500 2.937500 -6.712797 124 146 167 +7.000000 2.937500 -6.712797 124 146 167 +7.062500 2.937500 -6.712797 122 144 167 +7.125000 2.937500 -6.712797 121 143 165 +7.187500 2.937500 -6.712797 121 142 164 +7.250000 2.937500 -6.712797 120 142 163 +7.312500 2.937500 -6.712797 120 141 163 +7.375000 2.937500 -6.712797 120 140 162 +7.437500 2.937500 -6.712797 119 139 160 +7.500000 2.937500 -6.712797 119 138 160 +0.093842 2.158358 -5.039519 177 196 215 +0.138728 2.127168 -4.966694 177 197 215 +0.184971 2.127168 -4.966694 177 196 215 +0.230548 2.121037 -4.952381 177 197 215 +0.275862 2.114943 -4.938149 176 197 215 +0.404040 1.858586 -4.339586 176 196 215 +0.443325 1.853904 -4.328655 175 196 215 +0.482412 1.849246 -4.317779 175 196 215 +2.562500 2.875000 -6.712797 167 190 211 +2.625000 2.875000 -6.712797 167 190 211 +2.687500 2.875000 -6.712797 167 189 210 +2.750000 2.875000 -6.712797 166 189 210 +2.812500 2.875000 -6.712797 165 188 209 +2.875000 2.875000 -6.712797 165 188 209 +2.937500 2.875000 -6.712797 165 188 209 +3.000000 2.875000 -6.712797 164 188 209 +3.062500 2.875000 -6.712797 163 187 209 +3.125000 2.875000 -6.712797 163 187 209 +3.187500 2.875000 -6.712797 163 187 209 +3.250000 2.875000 -6.712797 162 186 209 +3.312500 2.875000 -6.712797 163 186 209 +3.375000 2.875000 -6.712797 161 186 208 +3.437500 2.875000 -6.712797 161 186 208 +3.500000 2.875000 -6.712797 161 185 208 +3.562500 2.875000 -6.712797 160 184 207 +3.625000 2.875000 -6.712797 160 184 206 +3.687500 2.875000 -6.712797 160 183 206 +3.750000 2.875000 -6.712797 159 183 206 +3.812500 2.875000 -6.712797 158 182 205 +3.875000 2.875000 -6.712797 158 182 205 +3.937500 2.875000 -6.712797 157 181 204 +4.000000 2.875000 -6.712797 156 180 203 +4.062500 2.875000 -6.712797 156 180 203 +4.125000 2.875000 -6.712797 155 179 202 +4.187500 2.875000 -6.712797 155 179 202 +4.250000 2.875000 -6.712797 155 178 201 +4.312500 2.875000 -6.712797 154 177 200 +4.375000 2.875000 -6.712797 153 177 200 +4.437500 2.875000 -6.712797 152 175 199 +4.500000 2.875000 -6.712797 152 175 197 +4.562500 2.875000 -6.712797 153 175 197 +4.625000 2.875000 -6.712797 152 174 196 +4.687500 2.875000 -6.712797 152 173 195 +4.750000 2.875000 -6.712797 152 173 195 +4.812500 2.875000 -6.712797 151 173 194 +4.875000 2.875000 -6.712797 151 172 193 +4.937500 2.875000 -6.712797 151 172 193 +5.000000 2.875000 -6.712797 150 171 192 +5.062500 2.875000 -6.712797 150 170 191 +5.125000 2.875000 -6.712797 149 170 190 +5.187500 2.875000 -6.712797 148 169 190 +5.250000 2.875000 -6.712797 148 168 189 +5.312500 2.875000 -6.712797 148 168 189 +5.375000 2.875000 -6.712797 147 167 188 +5.437500 2.875000 -6.712797 146 166 187 +5.500000 2.875000 -6.712797 145 166 186 +5.562500 2.875000 -6.712797 144 164 186 +5.625000 2.875000 -6.712797 144 164 185 +5.687500 2.875000 -6.712797 142 163 184 +5.750000 2.875000 -6.712797 141 162 184 +5.812500 2.875000 -6.712797 140 161 183 +5.875000 2.875000 -6.712797 139 160 182 +5.937500 2.875000 -6.712797 137 159 181 +6.000000 2.875000 -6.712797 136 159 181 +6.062500 2.875000 -6.712797 135 158 180 +6.125000 2.875000 -6.712797 134 157 180 +6.187500 2.875000 -6.712797 133 156 178 +6.250000 2.875000 -6.712797 132 155 178 +6.312500 2.875000 -6.712797 131 154 177 +6.375000 2.875000 -6.712797 131 154 176 +6.437500 2.875000 -6.712797 130 153 175 +6.500000 2.875000 -6.712797 129 152 175 +6.562500 2.875000 -6.712797 129 152 174 +6.625000 2.875000 -6.712797 129 151 173 +6.687500 2.875000 -6.712797 127 149 172 +6.750000 2.875000 -6.712797 127 149 172 +6.812500 2.875000 -6.712797 126 148 170 +6.875000 2.875000 -6.712797 126 148 170 +6.937500 2.875000 -6.712797 125 147 169 +7.000000 2.875000 -6.712797 125 146 168 +7.062500 2.875000 -6.712797 124 145 167 +7.125000 2.875000 -6.712797 122 144 166 +7.187500 2.875000 -6.712797 122 144 165 +7.250000 2.875000 -6.712797 122 143 164 +7.312500 2.875000 -6.712797 122 142 163 +7.375000 2.875000 -6.712797 122 142 163 +7.437500 2.875000 -6.712797 120 140 161 +7.500000 2.875000 -6.712797 121 141 160 +0.093842 2.111437 -5.039519 178 197 215 +0.139130 2.086957 -4.981090 178 197 216 +0.184971 2.080925 -4.966694 178 197 216 +0.231214 2.080925 -4.966694 178 197 216 +0.275862 2.068965 -4.938149 178 197 216 +0.371134 1.855670 -4.429062 178 197 215 +0.404040 1.818182 -4.339586 177 197 215 +0.443325 1.813602 -4.328655 177 196 215 +0.482412 1.809045 -4.317779 176 196 215 +2.562500 2.812500 -6.712797 169 191 211 +2.625000 2.812500 -6.712797 168 190 211 +2.687500 2.812500 -6.712797 167 190 211 +2.750000 2.812500 -6.712797 167 190 210 +2.812500 2.812500 -6.712797 167 190 210 +2.875000 2.812500 -6.712797 167 189 210 +2.937500 2.812500 -6.712797 166 189 210 +3.000000 2.812500 -6.712797 165 189 210 +3.062500 2.812500 -6.712797 165 188 209 +3.125000 2.812500 -6.712797 165 189 210 +3.187500 2.812500 -6.712797 164 188 209 +3.250000 2.812500 -6.712797 163 187 209 +3.312500 2.812500 -6.712797 163 187 209 +3.375000 2.812500 -6.712797 163 187 209 +3.437500 2.812500 -6.712797 163 186 208 +3.500000 2.812500 -6.712797 162 186 208 +3.562500 2.812500 -6.712797 162 186 208 +3.625000 2.812500 -6.712797 161 184 207 +3.687500 2.812500 -6.712797 160 184 206 +3.750000 2.812500 -6.712797 160 183 206 +3.812500 2.812500 -6.712797 160 183 205 +3.875000 2.812500 -6.712797 159 182 205 +3.937500 2.812500 -6.712797 159 182 205 +4.000000 2.812500 -6.712797 158 182 204 +4.062500 2.812500 -6.712797 157 181 203 +4.125000 2.812500 -6.712797 156 180 202 +4.187500 2.812500 -6.712797 156 180 202 +4.250000 2.812500 -6.712797 155 179 202 +4.312500 2.812500 -6.712797 155 178 201 +4.375000 2.812500 -6.712797 155 178 200 +4.437500 2.812500 -6.712797 154 177 199 +4.500000 2.812500 -6.712797 153 175 198 +4.562500 2.812500 -6.712797 154 175 198 +4.625000 2.812500 -6.712797 153 175 197 +4.687500 2.812500 -6.712797 152 175 197 +4.750000 2.812500 -6.712797 151 174 196 +4.812500 2.812500 -6.712797 151 173 194 +4.875000 2.812500 -6.712797 151 173 194 +4.937500 2.812500 -6.712797 151 172 193 +5.000000 2.812500 -6.712797 151 171 192 +5.062500 2.812500 -6.712797 151 171 192 +5.125000 2.812500 -6.712797 150 170 191 +5.187500 2.812500 -6.712797 150 170 190 +5.250000 2.812500 -6.712797 149 169 190 +5.312500 2.812500 -6.712797 148 168 189 +5.375000 2.812500 -6.712797 148 167 189 +5.437500 2.812500 -6.712797 146 167 187 +5.500000 2.812500 -6.712797 146 167 187 +5.562500 2.812500 -6.712797 145 165 186 +5.625000 2.812500 -6.712797 144 165 186 +5.687500 2.812500 -6.712797 143 163 185 +5.750000 2.812500 -6.712797 141 163 184 +5.812500 2.812500 -6.712797 140 162 184 +5.875000 2.812500 -6.712797 139 161 182 +5.937500 2.812500 -6.712797 138 160 182 +6.000000 2.812500 -6.712797 137 160 182 +6.062500 2.812500 -6.712797 136 159 181 +6.125000 2.812500 -6.712797 135 158 180 +6.187500 2.812500 -6.712797 135 157 180 +6.250000 2.812500 -6.712797 133 155 178 +6.312500 2.812500 -6.712797 133 155 178 +6.375000 2.812500 -6.712797 132 155 177 +6.437500 2.812500 -6.712797 132 155 176 +6.500000 2.812500 -6.712797 130 153 175 +6.562500 2.812500 -6.712797 130 152 175 +6.625000 2.812500 -6.712797 129 151 173 +6.687500 2.812500 -6.712797 128 150 173 +6.750000 2.812500 -6.712797 128 150 172 +6.812500 2.812500 -6.712797 127 149 171 +6.875000 2.812500 -6.712797 127 148 170 +6.937500 2.812500 -6.712797 126 148 169 +7.000000 2.812500 -6.712797 126 147 168 +7.062500 2.812500 -6.712797 125 146 167 +7.125000 2.812500 -6.712797 125 146 167 +7.187500 2.812500 -6.712797 125 145 166 +7.250000 2.812500 -6.712797 125 144 164 +7.312500 2.812500 -6.712797 124 144 163 +7.375000 2.812500 -6.712797 124 143 162 +7.437500 2.812500 -6.712797 124 142 162 +7.500000 2.812500 -6.712797 124 142 161 +0.093842 2.064516 -5.039519 179 198 216 +0.139535 2.046512 -4.995570 180 199 216 +0.185507 2.040580 -4.981090 179 199 217 +0.231884 2.040580 -4.981090 179 199 217 +0.277457 2.034682 -4.966694 179 199 217 +0.362606 1.994334 -4.868204 179 198 216 +0.371134 1.814433 -4.429062 179 198 216 +0.405063 1.782279 -4.350572 178 197 216 +0.443325 1.773300 -4.328655 178 197 215 +0.482412 1.768844 -4.317779 178 197 215 +2.625000 2.750000 -6.712797 170 192 211 +2.687500 2.750000 -6.712797 170 192 212 +2.750000 2.750000 -6.712797 169 191 211 +2.812500 2.750000 -6.712797 168 190 211 +2.875000 2.750000 -6.712797 167 190 210 +2.937500 2.750000 -6.712797 167 190 210 +3.000000 2.750000 -6.712797 167 190 210 +3.062500 2.750000 -6.712797 167 189 210 +3.125000 2.750000 -6.712797 166 189 210 +3.187500 2.750000 -6.712797 166 189 210 +3.250000 2.750000 -6.712797 165 189 209 +3.312500 2.750000 -6.712797 165 189 209 +3.375000 2.750000 -6.712797 164 188 209 +3.437500 2.750000 -6.712797 164 187 209 +3.500000 2.750000 -6.712797 163 187 208 +3.562500 2.750000 -6.712797 163 186 208 +3.625000 2.750000 -6.712797 163 186 208 +3.687500 2.750000 -6.712797 162 185 207 +3.750000 2.750000 -6.712797 161 184 206 +3.812500 2.750000 -6.712797 160 184 206 +3.875000 2.750000 -6.712797 160 183 205 +3.937500 2.750000 -6.712797 160 183 205 +4.000000 2.750000 -6.712797 159 182 204 +4.062500 2.750000 -6.712797 159 182 203 +4.125000 2.750000 -6.712797 158 180 202 +4.187500 2.750000 -6.712797 157 180 202 +4.250000 2.750000 -6.712797 157 180 202 +4.312500 2.750000 -6.712797 156 179 201 +4.375000 2.750000 -6.712797 155 178 200 +4.437500 2.750000 -6.712797 155 178 200 +4.500000 2.750000 -6.712797 155 177 199 +4.562500 2.750000 -6.712797 154 176 198 +4.625000 2.750000 -6.712797 154 176 198 +4.687500 2.750000 -6.712797 152 175 197 +4.750000 2.750000 -6.712797 152 175 197 +4.812500 2.750000 -6.712797 152 174 196 +4.875000 2.750000 -6.712797 152 173 194 +4.937500 2.750000 -6.712797 152 173 194 +5.000000 2.750000 -6.712797 151 172 193 +5.062500 2.750000 -6.712797 151 172 192 +5.125000 2.750000 -6.712797 150 171 192 +5.187500 2.750000 -6.712797 150 170 191 +5.250000 2.750000 -6.712797 149 170 190 +5.312500 2.750000 -6.712797 148 169 190 +5.375000 2.750000 -6.712797 148 169 189 +5.437500 2.750000 -6.712797 147 167 188 +5.500000 2.750000 -6.712797 147 167 188 +5.562500 2.750000 -6.712797 146 167 187 +5.625000 2.750000 -6.712797 144 165 186 +5.687500 2.750000 -6.712797 144 164 186 +5.750000 2.750000 -6.712797 143 163 185 +5.812500 2.750000 -6.712797 142 163 184 +5.875000 2.750000 -6.712797 140 162 183 +5.937500 2.750000 -6.712797 139 161 182 +6.000000 2.750000 -6.712797 138 160 182 +6.062500 2.750000 -6.712797 138 160 181 +6.125000 2.750000 -6.712797 137 159 180 +6.187500 2.750000 -6.712797 136 158 179 +6.250000 2.750000 -6.712797 136 158 179 +6.312500 2.750000 -6.712797 134 156 178 +6.375000 2.750000 -6.712797 133 155 178 +6.437500 2.750000 -6.712797 133 155 177 +6.500000 2.750000 -6.712797 132 154 175 +6.562500 2.750000 -6.712797 131 153 175 +6.625000 2.750000 -6.712797 130 152 175 +6.687500 2.750000 -6.712797 129 152 173 +6.750000 2.750000 -6.712797 129 150 172 +6.812500 2.750000 -6.712797 128 150 171 +6.875000 2.750000 -6.712797 128 149 170 +6.937500 2.750000 -6.712797 128 148 170 +7.000000 2.750000 -6.712797 128 148 169 +7.062500 2.750000 -6.712797 128 148 167 +7.125000 2.750000 -6.712797 127 147 167 +7.187500 2.750000 -6.712797 126 146 166 +7.250000 2.750000 -6.712797 126 146 165 +7.312500 2.750000 -6.712797 126 145 164 +7.375000 2.750000 -6.712797 126 144 163 +7.437500 2.750000 -6.712797 125 143 162 +7.500000 2.750000 -6.712797 124 142 161 +0.093842 2.017595 -5.039519 180 199 216 +0.139535 2.000000 -4.995570 180 199 217 +0.185507 1.994203 -4.981090 180 199 217 +0.231214 1.988439 -4.966694 180 199 217 +0.275862 1.977011 -4.938149 180 199 217 +0.320917 1.971347 -4.924000 180 199 217 +0.360563 1.938028 -4.840778 180 199 216 +0.375000 1.791667 -4.475198 180 199 216 +0.406091 1.746193 -4.361614 180 199 216 +0.444444 1.737374 -4.339586 179 198 216 +0.484848 1.737374 -4.339586 179 198 216 +2.687500 2.687500 -6.712797 170 192 213 +2.750000 2.687500 -6.712797 170 192 212 +2.875000 2.687500 -6.712797 169 191 211 +2.937500 2.687500 -6.712797 169 191 211 +3.000000 2.687500 -6.712797 169 190 210 +3.062500 2.687500 -6.712797 168 190 210 +3.125000 2.687500 -6.712797 168 190 210 +3.187500 2.687500 -6.712797 167 189 209 +3.250000 2.687500 -6.712797 166 189 209 +3.312500 2.687500 -6.712797 166 189 209 +3.375000 2.687500 -6.712797 165 188 209 +3.437500 2.687500 -6.712797 165 188 209 +3.500000 2.687500 -6.712797 164 187 208 +3.562500 2.687500 -6.712797 164 187 208 +3.625000 2.687500 -6.712797 164 187 208 +3.687500 2.687500 -6.712797 163 186 206 +3.750000 2.687500 -6.712797 163 185 206 +3.812500 2.687500 -6.712797 162 185 206 +3.875000 2.687500 -6.712797 161 184 205 +3.937500 2.687500 -6.712797 160 183 205 +4.000000 2.687500 -6.712797 160 182 203 +4.062500 2.687500 -6.712797 160 182 203 +4.125000 2.687500 -6.712797 160 182 202 +4.187500 2.687500 -6.712797 160 181 202 +4.250000 2.687500 -6.712797 158 180 201 +4.312500 2.687500 -6.712797 157 180 201 +4.375000 2.687500 -6.712797 156 179 201 +4.437500 2.687500 -6.712797 156 178 200 +4.500000 2.687500 -6.712797 155 178 199 +4.562500 2.687500 -6.712797 155 177 199 +4.625000 2.687500 -6.712797 155 177 199 +4.687500 2.687500 -6.712797 154 177 199 +4.750000 2.687500 -6.712797 154 175 198 +4.812500 2.687500 -6.712797 153 175 196 +4.875000 2.687500 -6.712797 153 174 195 +4.937500 2.687500 -6.712797 153 174 195 +5.000000 2.687500 -6.712797 152 173 194 +5.062500 2.687500 -6.712797 151 173 193 +5.125000 2.687500 -6.712797 151 172 193 +5.187500 2.687500 -6.712797 150 171 192 +5.250000 2.687500 -6.712797 150 171 191 +5.312500 2.687500 -6.712797 149 170 190 +5.375000 2.687500 -6.712797 148 169 189 +5.437500 2.687500 -6.712797 148 169 189 +5.500000 2.687500 -6.712797 148 168 189 +5.562500 2.687500 -6.712797 147 167 187 +5.625000 2.687500 -6.712797 146 167 187 +5.687500 2.687500 -6.712797 144 165 186 +5.750000 2.687500 -6.712797 143 164 185 +5.812500 2.687500 -6.712797 143 164 185 +5.875000 2.687500 -6.712797 142 163 184 +5.937500 2.687500 -6.712797 141 162 183 +6.000000 2.687500 -6.712797 140 161 182 +6.062500 2.687500 -6.712797 139 160 182 +6.125000 2.687500 -6.712797 139 160 181 +6.187500 2.687500 -6.712797 138 160 180 +6.250000 2.687500 -6.712797 137 159 180 +6.312500 2.687500 -6.712797 136 158 178 +6.375000 2.687500 -6.712797 135 157 178 +6.437500 2.687500 -6.712797 134 156 178 +6.500000 2.687500 -6.712797 133 155 176 +6.562500 2.687500 -6.712797 132 155 175 +6.625000 2.687500 -6.712797 131 153 175 +6.687500 2.687500 -6.712797 131 153 173 +6.750000 2.687500 -6.712797 130 152 173 +6.812500 2.687500 -6.712797 131 152 172 +6.875000 2.687500 -6.712797 130 150 171 +6.937500 2.687500 -6.712797 129 149 170 +7.000000 2.687500 -6.712797 129 148 169 +7.062500 2.687500 -6.712797 129 148 168 +7.125000 2.687500 -6.712797 128 148 167 +7.187500 2.687500 -6.712797 128 148 167 +7.250000 2.687500 -6.712797 127 147 165 +7.312500 2.687500 -6.712797 127 146 164 +7.375000 2.687500 -6.712797 126 145 163 +7.437500 2.687500 -6.712797 126 145 163 +7.500000 2.687500 -6.712797 125 143 162 +0.093842 1.970675 -5.039519 181 199 216 +0.139535 1.953488 -4.995570 181 199 216 +0.184971 1.942196 -4.966694 181 199 217 +0.231214 1.942196 -4.966694 181 200 217 +0.275072 1.925501 -4.924000 181 200 217 +0.316384 1.898305 -4.854452 182 200 217 +0.343164 1.801609 -4.607174 181 200 217 +0.375000 1.750000 -4.475198 181 199 216 +0.408163 1.714286 -4.383868 180 199 216 +0.446701 1.705584 -4.361614 180 199 216 +0.489796 1.714286 -4.383868 180 199 216 +2.750000 2.625000 -6.712797 172 193 212 +2.875000 2.625000 -6.712797 172 192 210 +2.937500 2.625000 -6.712797 171 191 210 +3.000000 2.625000 -6.712797 171 191 210 +3.062500 2.625000 -6.712797 170 191 210 +3.125000 2.625000 -6.712797 170 190 209 +3.187500 2.625000 -6.712797 168 190 209 +3.250000 2.625000 -6.712797 168 189 209 +3.312500 2.625000 -6.712797 167 189 209 +3.375000 2.625000 -6.712797 167 189 209 +3.437500 2.625000 -6.712797 167 189 209 +3.500000 2.625000 -6.712797 166 188 208 +3.562500 2.625000 -6.712797 166 188 208 +3.625000 2.625000 -6.712797 164 187 207 +3.687500 2.625000 -6.712797 164 186 207 +3.750000 2.625000 -6.712797 164 186 206 +3.812500 2.625000 -6.712797 163 186 206 +3.875000 2.625000 -6.712797 163 185 206 +3.937500 2.625000 -6.712797 162 184 205 +4.000000 2.625000 -6.712797 162 184 204 +4.062500 2.625000 -6.712797 161 183 203 +4.125000 2.625000 -6.712797 161 182 202 +4.187500 2.625000 -6.712797 160 182 202 +4.250000 2.625000 -6.712797 160 181 202 +4.312500 2.625000 -6.712797 159 180 201 +4.375000 2.625000 -6.712797 158 180 200 +4.437500 2.625000 -6.712797 157 179 200 +4.500000 2.625000 -6.712797 157 179 200 +4.562500 2.625000 -6.712797 156 178 199 +4.625000 2.625000 -6.712797 156 178 199 +4.687500 2.625000 -6.712797 155 178 199 +4.750000 2.625000 -6.712797 155 176 198 +4.812500 2.625000 -6.712797 155 176 197 +4.875000 2.625000 -6.712797 155 175 196 +4.937500 2.625000 -6.712797 155 175 195 +5.000000 2.625000 -6.712797 154 174 194 +5.062500 2.625000 -6.712797 153 173 194 +5.125000 2.625000 -6.712797 153 173 193 +5.187500 2.625000 -6.712797 152 172 192 +5.250000 2.625000 -6.712797 152 172 192 +5.312500 2.625000 -6.712797 151 171 191 +5.375000 2.625000 -6.712797 150 170 190 +5.437500 2.625000 -6.712797 149 170 190 +5.500000 2.625000 -6.712797 148 168 189 +5.562500 2.625000 -6.712797 148 167 188 +5.625000 2.625000 -6.712797 147 167 187 +5.687500 2.625000 -6.712797 145 166 186 +5.750000 2.625000 -6.712797 145 165 186 +5.812500 2.625000 -6.712797 144 164 185 +5.875000 2.625000 -6.712797 144 164 184 +5.937500 2.625000 -6.712797 143 163 184 +6.000000 2.625000 -6.712797 142 163 183 +6.062500 2.625000 -6.712797 141 162 182 +6.125000 2.625000 -6.712797 141 161 182 +6.187500 2.625000 -6.712797 139 160 180 +6.250000 2.625000 -6.712797 138 160 180 +6.312500 2.625000 -6.712797 138 159 179 +6.375000 2.625000 -6.712797 136 158 178 +6.437500 2.625000 -6.712797 136 157 178 +6.500000 2.625000 -6.712797 134 155 177 +6.562500 2.625000 -6.712797 134 155 176 +6.625000 2.625000 -6.712797 133 154 175 +6.687500 2.625000 -6.712797 132 154 174 +6.750000 2.625000 -6.712797 132 153 173 +6.812500 2.625000 -6.712797 132 153 173 +6.875000 2.625000 -6.712797 131 151 171 +6.937500 2.625000 -6.712797 131 151 170 +7.000000 2.625000 -6.712797 131 150 170 +7.062500 2.625000 -6.712797 131 150 169 +7.125000 2.625000 -6.712797 131 149 167 +7.187500 2.625000 -6.712797 130 148 167 +7.250000 2.625000 -6.712797 128 148 166 +7.312500 2.625000 -6.712797 128 147 165 +7.375000 2.625000 -6.712797 127 146 163 +7.437500 2.625000 -6.712797 126 145 163 +7.500000 2.625000 -6.712797 126 145 163 +0.093842 1.923754 -5.039519 182 199 215 +0.139130 1.901449 -4.981090 182 199 216 +0.184971 1.895954 -4.966694 182 200 217 +0.230548 1.890490 -4.952381 182 200 217 +0.275072 1.879656 -4.924000 182 200 217 +0.314607 1.842697 -4.827180 183 200 217 +0.343164 1.758713 -4.607174 183 200 217 +0.375000 1.708333 -4.475198 182 200 217 +0.412371 1.690722 -4.429062 182 200 216 +0.451282 1.682051 -4.406349 182 200 216 +2.875000 2.562500 -6.712797 175 194 210 +2.937500 2.562500 -6.712797 176 193 210 +3.000000 2.562500 -6.712797 175 193 210 +3.062500 2.562500 -6.712797 173 192 209 +3.125000 2.562500 -6.712797 171 191 209 +3.187500 2.562500 -6.712797 170 191 209 +3.250000 2.562500 -6.712797 170 190 210 +3.312500 2.562500 -6.712797 169 190 209 +3.375000 2.562500 -6.712797 168 190 209 +3.437500 2.562500 -6.712797 167 189 209 +3.500000 2.562500 -6.712797 167 189 208 +3.562500 2.562500 -6.712797 167 189 208 +3.625000 2.562500 -6.712797 166 188 208 +3.687500 2.562500 -6.712797 166 188 208 +3.750000 2.562500 -6.712797 165 187 207 +3.812500 2.562500 -6.712797 164 186 206 +3.875000 2.562500 -6.712797 164 186 206 +3.937500 2.562500 -6.712797 163 186 206 +4.000000 2.562500 -6.712797 163 184 205 +4.062500 2.562500 -6.712797 162 184 203 +4.125000 2.562500 -6.712797 163 183 203 +4.187500 2.562500 -6.712797 162 182 202 +4.250000 2.562500 -6.712797 161 182 202 +4.312500 2.562500 -6.712797 160 182 202 +4.375000 2.562500 -6.712797 160 181 201 +4.437500 2.562500 -6.712797 159 180 201 +4.500000 2.562500 -6.712797 159 180 200 +4.562500 2.562500 -6.712797 158 180 200 +4.625000 2.562500 -6.712797 157 179 200 +4.687500 2.562500 -6.712797 156 178 199 +4.750000 2.562500 -6.712797 156 178 197 +4.812500 2.562500 -6.712797 157 177 197 +4.875000 2.562500 -6.712797 156 176 196 +4.937500 2.562500 -6.712797 155 175 195 +5.000000 2.562500 -6.712797 155 175 194 +5.062500 2.562500 -6.712797 155 175 194 +5.125000 2.562500 -6.712797 155 174 194 +5.187500 2.562500 -6.712797 154 173 193 +5.250000 2.562500 -6.712797 153 173 192 +5.312500 2.562500 -6.712797 152 172 192 +5.375000 2.562500 -6.712797 151 171 191 +5.437500 2.562500 -6.712797 149 170 190 +5.500000 2.562500 -6.712797 149 170 189 +5.562500 2.562500 -6.712797 148 168 189 +5.625000 2.562500 -6.712797 148 168 188 +5.687500 2.562500 -6.712797 148 167 187 +5.750000 2.562500 -6.712797 147 167 186 +5.812500 2.562500 -6.712797 146 166 186 +5.875000 2.562500 -6.712797 145 165 185 +5.937500 2.562500 -6.712797 145 165 184 +6.000000 2.562500 -6.712797 144 164 183 +6.062500 2.562500 -6.712797 144 163 182 +6.125000 2.562500 -6.712797 143 163 182 +6.187500 2.562500 -6.712797 141 161 180 +6.250000 2.562500 -6.712797 141 161 180 +6.312500 2.562500 -6.712797 140 160 180 +6.375000 2.562500 -6.712797 139 159 178 +6.437500 2.562500 -6.712797 138 158 178 +6.500000 2.562500 -6.712797 137 157 177 +6.562500 2.562500 -6.712797 137 157 176 +6.625000 2.562500 -6.712797 135 155 175 +6.687500 2.562500 -6.712797 134 155 174 +6.750000 2.562500 -6.712797 135 155 174 +6.812500 2.562500 -6.712797 134 154 173 +6.875000 2.562500 -6.712797 134 154 172 +6.937500 2.562500 -6.712797 134 153 170 +7.000000 2.562500 -6.712797 134 153 170 +7.062500 2.562500 -6.712797 134 152 170 +7.125000 2.562500 -6.712797 132 151 168 +7.187500 2.562500 -6.712797 132 150 167 +7.250000 2.562500 -6.712797 131 149 167 +7.312500 2.562500 -6.712797 130 148 166 +7.375000 2.562500 -6.712797 129 148 165 +7.437500 2.562500 -6.712797 128 146 163 +7.500000 2.562500 -6.712797 127 145 163 +0.093842 1.876833 -5.039519 183 199 215 +0.138728 1.849711 -4.966694 183 199 215 +0.184438 1.844380 -4.952381 182 200 215 +0.229226 1.833811 -4.924000 183 200 217 +0.272727 1.818182 -4.882034 184 201 217 +0.314607 1.797753 -4.827180 184 201 217 +0.343164 1.715818 -4.607174 184 201 217 +0.376963 1.675393 -4.498629 184 201 217 +0.413437 1.653747 -4.440506 183 200 216 +0.454780 1.653747 -4.440506 183 200 216 +2.875000 2.500000 -6.712797 180 196 211 +2.937500 2.500000 -6.712797 180 196 210 +3.000000 2.500000 -6.712797 178 194 210 +3.062500 2.500000 -6.712797 175 193 210 +3.125000 2.500000 -6.712797 173 192 210 +3.187500 2.500000 -6.712797 172 192 210 +3.250000 2.500000 -6.712797 171 192 210 +3.312500 2.500000 -6.712797 170 192 210 +3.375000 2.500000 -6.712797 170 190 209 +3.437500 2.500000 -6.712797 170 190 210 +3.500000 2.500000 -6.712797 169 190 209 +3.562500 2.500000 -6.712797 168 189 209 +3.625000 2.500000 -6.712797 167 189 208 +3.687500 2.500000 -6.712797 167 189 208 +3.750000 2.500000 -6.712797 167 188 207 +3.812500 2.500000 -6.712797 167 188 207 +3.875000 2.500000 -6.712797 166 187 206 +3.937500 2.500000 -6.712797 165 186 206 +4.000000 2.500000 -6.712797 165 185 205 +4.062500 2.500000 -6.712797 164 184 203 +4.125000 2.500000 -6.712797 164 184 203 +4.187500 2.500000 -6.712797 163 184 202 +4.250000 2.500000 -6.712797 163 183 202 +4.312500 2.500000 -6.712797 161 182 202 +4.375000 2.500000 -6.712797 161 182 202 +4.437500 2.500000 -6.712797 160 182 202 +4.500000 2.500000 -6.712797 160 181 201 +4.562500 2.500000 -6.712797 159 180 200 +4.625000 2.500000 -6.712797 159 180 200 +4.687500 2.500000 -6.712797 158 179 199 +4.750000 2.500000 -6.712797 160 179 198 +4.812500 2.500000 -6.712797 160 178 197 +4.875000 2.500000 -6.712797 159 178 197 +4.937500 2.500000 -6.712797 158 178 196 +5.000000 2.500000 -6.712797 156 176 195 +5.062500 2.500000 -6.712797 156 175 195 +5.125000 2.500000 -6.712797 155 175 194 +5.187500 2.500000 -6.712797 155 174 193 +5.250000 2.500000 -6.712797 154 173 192 +5.312500 2.500000 -6.712797 153 173 192 +5.375000 2.500000 -6.712797 152 172 191 +5.437500 2.500000 -6.712797 152 172 191 +5.500000 2.500000 -6.712797 151 171 190 +5.562500 2.500000 -6.712797 149 170 189 +5.625000 2.500000 -6.712797 149 169 189 +5.687500 2.500000 -6.712797 148 169 188 +5.750000 2.500000 -6.712797 148 167 187 +5.812500 2.500000 -6.712797 148 167 186 +5.875000 2.500000 -6.712797 147 167 186 +5.937500 2.500000 -6.712797 147 166 184 +6.000000 2.500000 -6.712797 146 165 184 +6.062500 2.500000 -6.712797 146 164 183 +6.125000 2.500000 -6.712797 145 163 182 +6.187500 2.500000 -6.712797 144 163 181 +6.250000 2.500000 -6.712797 144 163 180 +6.312500 2.500000 -6.712797 143 161 180 +6.375000 2.500000 -6.712797 142 160 178 +6.437500 2.500000 -6.712797 141 160 178 +6.500000 2.500000 -6.712797 141 160 178 +6.562500 2.500000 -6.712797 139 158 177 +6.625000 2.500000 -6.712797 138 157 175 +6.687500 2.500000 -6.712797 137 157 175 +6.750000 2.500000 -6.712797 137 156 174 +6.812500 2.500000 -6.712797 137 155 173 +6.875000 2.500000 -6.712797 137 155 173 +6.937500 2.500000 -6.712797 138 155 172 +7.000000 2.500000 -6.712797 136 154 170 +7.062500 2.500000 -6.712797 136 154 170 +7.125000 2.500000 -6.712797 134 152 169 +7.187500 2.500000 -6.712797 134 151 167 +7.250000 2.500000 -6.712797 133 150 167 +7.312500 2.500000 -6.712797 132 150 167 +7.375000 2.500000 -6.712797 131 149 165 +7.437500 2.500000 -6.712797 130 148 164 +7.500000 2.500000 -6.712797 130 147 163 +0.139535 1.813954 -4.995570 184 200 215 +0.183908 1.793103 -4.938149 184 200 216 +0.229226 1.787966 -4.924000 184 201 217 +0.272727 1.772727 -4.882034 185 202 217 +0.313726 1.747899 -4.813658 186 202 217 +0.343164 1.672922 -4.607174 185 202 217 +0.376963 1.633508 -4.498629 185 202 217 +0.413437 1.612403 -4.440506 185 202 216 +0.454780 1.612403 -4.440506 184 201 217 +2.937500 2.437500 -6.712797 180 196 211 +3.000000 2.437500 -6.712797 177 194 210 +3.062500 2.437500 -6.712797 175 193 210 +3.125000 2.437500 -6.712797 173 193 210 +3.187500 2.437500 -6.712797 173 192 210 +3.250000 2.437500 -6.712797 172 192 211 +3.312500 2.437500 -6.712797 172 193 211 +3.375000 2.437500 -6.712797 172 192 211 +3.437500 2.437500 -6.712797 171 192 210 +3.500000 2.437500 -6.712797 170 192 210 +3.562500 2.437500 -6.712797 170 190 209 +3.625000 2.437500 -6.712797 170 190 209 +3.687500 2.437500 -6.712797 169 190 208 +3.750000 2.437500 -6.712797 168 189 208 +3.812500 2.437500 -6.712797 167 189 207 +3.875000 2.437500 -6.712797 167 188 207 +3.937500 2.437500 -6.712797 167 187 205 +4.000000 2.437500 -6.712797 167 186 205 +4.062500 2.437500 -6.712797 166 186 204 +4.125000 2.437500 -6.712797 166 185 203 +4.187500 2.437500 -6.712797 165 185 203 +4.250000 2.437500 -6.712797 163 184 203 +4.312500 2.437500 -6.712797 163 184 203 +4.375000 2.437500 -6.712797 162 183 202 +4.437500 2.437500 -6.712797 161 182 202 +4.500000 2.437500 -6.712797 160 182 201 +4.562500 2.437500 -6.712797 161 181 200 +4.625000 2.437500 -6.712797 162 181 199 +4.687500 2.437500 -6.712797 163 181 198 +4.750000 2.437500 -6.712797 163 180 198 +4.812500 2.437500 -6.712797 162 180 197 +4.875000 2.437500 -6.712797 160 179 197 +4.937500 2.437500 -6.712797 159 178 196 +5.000000 2.437500 -6.712797 159 178 196 +5.062500 2.437500 -6.712797 158 177 195 +5.125000 2.437500 -6.712797 156 175 194 +5.187500 2.437500 -6.712797 156 175 194 +5.250000 2.437500 -6.712797 155 174 193 +5.312500 2.437500 -6.712797 155 174 192 +5.375000 2.437500 -6.712797 153 173 192 +5.437500 2.437500 -6.712797 153 172 191 +5.500000 2.437500 -6.712797 152 172 190 +5.562500 2.437500 -6.712797 151 170 189 +5.625000 2.437500 -6.712797 150 170 189 +5.687500 2.437500 -6.712797 149 169 189 +5.750000 2.437500 -6.712797 148 168 187 +5.812500 2.437500 -6.712797 148 168 186 +5.875000 2.437500 -6.712797 149 167 186 +5.937500 2.437500 -6.712797 148 167 185 +6.000000 2.437500 -6.712797 148 167 184 +6.062500 2.437500 -6.712797 148 166 183 +6.125000 2.437500 -6.712797 148 165 182 +6.187500 2.437500 -6.712797 147 164 182 +6.250000 2.437500 -6.712797 147 164 181 +6.312500 2.437500 -6.712797 146 163 180 +6.375000 2.437500 -6.712797 144 162 179 +6.437500 2.437500 -6.712797 143 161 178 +6.500000 2.437500 -6.712797 143 160 178 +6.562500 2.437500 -6.712797 141 160 177 +6.625000 2.437500 -6.712797 141 159 176 +6.687500 2.437500 -6.712797 140 158 175 +6.750000 2.437500 -6.712797 142 159 175 +6.812500 2.437500 -6.712797 142 159 174 +6.875000 2.437500 -6.712797 141 157 173 +6.937500 2.437500 -6.712797 140 156 172 +7.000000 2.437500 -6.712797 138 155 171 +7.062500 2.437500 -6.712797 136 154 170 +7.125000 2.437500 -6.712797 135 153 169 +7.187500 2.437500 -6.712797 134 152 168 +7.250000 2.437500 -6.712797 133 151 167 +7.312500 2.437500 -6.712797 133 151 167 +7.375000 2.437500 -6.712797 133 150 166 +7.437500 2.437500 -6.712797 131 148 164 +7.500000 2.437500 -6.712797 131 148 164 +-0.312500 2.375000 -6.712797 186 200 214 +-0.250000 2.375000 -6.712797 186 200 214 +-0.187500 2.375000 -6.712797 187 201 215 +-0.125000 2.375000 -6.712797 186 200 214 +0.230548 1.752161 -4.952381 186 202 217 +0.274286 1.737143 -4.909932 186 202 216 +0.313726 1.703081 -4.813658 186 202 217 +0.341333 1.621333 -4.582603 186 202 217 +0.375979 1.587467 -4.486883 186 202 217 +0.413437 1.571059 -4.440506 186 202 216 +0.454780 1.571059 -4.440506 186 202 217 +0.497409 1.575130 -4.452011 186 202 217 +3.000000 2.375000 -6.712797 176 195 211 +3.062500 2.375000 -6.712797 175 194 211 +3.125000 2.375000 -6.712797 175 195 212 +3.187500 2.375000 -6.712797 174 194 212 +3.250000 2.375000 -6.712797 174 194 212 +3.312500 2.375000 -6.712797 174 194 212 +3.375000 2.375000 -6.712797 173 194 211 +3.437500 2.375000 -6.712797 173 193 211 +3.500000 2.375000 -6.712797 172 193 211 +3.562500 2.375000 -6.712797 172 192 210 +3.625000 2.375000 -6.712797 171 192 210 +3.687500 2.375000 -6.712797 170 191 209 +3.750000 2.375000 -6.712797 170 190 209 +3.812500 2.375000 -6.712797 169 190 208 +3.875000 2.375000 -6.712797 169 189 207 +3.937500 2.375000 -6.712797 168 188 206 +4.000000 2.375000 -6.712797 168 187 205 +4.062500 2.375000 -6.712797 167 187 204 +4.125000 2.375000 -6.712797 167 186 203 +4.187500 2.375000 -6.712797 169 186 203 +4.250000 2.375000 -6.712797 167 186 203 +4.312500 2.375000 -6.712797 166 184 202 +4.375000 2.375000 -6.712797 166 184 202 +4.437500 2.375000 -6.712797 163 183 202 +4.500000 2.375000 -6.712797 162 182 202 +4.562500 2.375000 -6.712797 163 182 200 +4.625000 2.375000 -6.712797 165 182 199 +4.687500 2.375000 -6.712797 165 182 199 +4.750000 2.375000 -6.712797 164 182 198 +4.812500 2.375000 -6.712797 163 180 197 +4.875000 2.375000 -6.712797 161 180 197 +4.937500 2.375000 -6.712797 160 179 197 +5.000000 2.375000 -6.712797 160 178 196 +5.062500 2.375000 -6.712797 159 178 195 +5.125000 2.375000 -6.712797 158 177 195 +5.187500 2.375000 -6.712797 157 176 194 +5.250000 2.375000 -6.712797 156 175 194 +5.312500 2.375000 -6.712797 155 175 193 +5.375000 2.375000 -6.712797 155 173 192 +5.437500 2.375000 -6.712797 154 173 191 +5.500000 2.375000 -6.712797 154 173 191 +5.562500 2.375000 -6.712797 153 172 190 +5.625000 2.375000 -6.712797 152 171 189 +5.687500 2.375000 -6.712797 151 170 189 +5.750000 2.375000 -6.712797 150 170 188 +5.812500 2.375000 -6.712797 152 170 187 +5.875000 2.375000 -6.712797 152 170 186 +5.937500 2.375000 -6.712797 151 168 185 +6.000000 2.375000 -6.712797 151 168 184 +6.062500 2.375000 -6.712797 150 167 184 +6.125000 2.375000 -6.712797 148 166 183 +6.187500 2.375000 -6.712797 148 166 182 +6.250000 2.375000 -6.712797 148 165 182 +6.312500 2.375000 -6.712797 146 163 180 +6.375000 2.375000 -6.712797 145 163 180 +6.437500 2.375000 -6.712797 145 162 179 +6.500000 2.375000 -6.712797 144 161 178 +6.562500 2.375000 -6.712797 144 161 178 +6.625000 2.375000 -6.712797 143 160 176 +6.687500 2.375000 -6.712797 144 161 176 +6.750000 2.375000 -6.712797 145 160 175 +6.812500 2.375000 -6.712797 144 160 175 +6.875000 2.375000 -6.712797 142 158 174 +6.937500 2.375000 -6.712797 139 155 172 +7.000000 2.375000 -6.712797 136 155 171 +7.062500 2.375000 -6.712797 136 154 171 +7.125000 2.375000 -6.712797 135 153 170 +7.187500 2.375000 -6.712797 134 152 169 +7.250000 2.375000 -6.712797 133 151 167 +7.312500 2.375000 -6.712797 132 150 167 +7.375000 2.375000 -6.712797 132 149 166 +7.437500 2.375000 -6.712797 131 148 165 +7.500000 2.375000 -6.712797 131 148 164 +-0.562500 2.312500 -6.712797 186 202 215 +-0.500000 2.312500 -6.712797 186 201 215 +-0.437500 2.312500 -6.712797 185 200 215 +-0.375000 2.312500 -6.712797 186 200 215 +-0.312500 2.312500 -6.712797 187 201 215 +-0.250000 2.312500 -6.712797 188 201 214 +-0.187500 2.312500 -6.712797 189 202 215 +-0.125000 2.312500 -6.712797 189 202 215 +-0.062500 2.312500 -6.712797 187 201 215 +0.318182 1.681818 -4.882034 189 203 217 +0.336842 1.557895 -4.522305 188 203 217 +0.375000 1.541667 -4.475198 189 203 217 +0.411311 1.521851 -4.417676 188 203 217 +0.452442 1.521851 -4.417676 188 203 217 +3.000000 2.312500 -6.712797 177 196 212 +3.062500 2.312500 -6.712797 176 196 212 +3.125000 2.312500 -6.712797 175 196 212 +3.187500 2.312500 -6.712797 175 196 213 +3.250000 2.312500 -6.712797 175 196 213 +3.312500 2.312500 -6.712797 175 195 212 +3.375000 2.312500 -6.712797 175 194 212 +3.437500 2.312500 -6.712797 175 194 212 +3.500000 2.312500 -6.712797 174 194 211 +3.562500 2.312500 -6.712797 173 193 211 +3.625000 2.312500 -6.712797 172 192 210 +3.687500 2.312500 -6.712797 172 192 209 +3.750000 2.312500 -6.712797 171 191 208 +3.812500 2.312500 -6.712797 170 190 208 +3.875000 2.312500 -6.712797 170 189 206 +3.937500 2.312500 -6.712797 170 189 206 +4.000000 2.312500 -6.712797 171 189 205 +4.062500 2.312500 -6.712797 170 189 205 +4.125000 2.312500 -6.712797 170 188 204 +4.187500 2.312500 -6.712797 170 187 203 +4.250000 2.312500 -6.712797 170 187 203 +4.312500 2.312500 -6.712797 170 186 202 +4.375000 2.312500 -6.712797 169 186 202 +4.437500 2.312500 -6.712797 167 185 202 +4.500000 2.312500 -6.712797 165 184 201 +4.562500 2.312500 -6.712797 165 183 200 +4.625000 2.312500 -6.712797 167 184 200 +4.687500 2.312500 -6.712797 166 183 199 +4.750000 2.312500 -6.712797 165 182 199 +4.812500 2.312500 -6.712797 164 182 198 +4.875000 2.312500 -6.712797 163 181 197 +4.937500 2.312500 -6.712797 162 180 197 +5.000000 2.312500 -6.712797 161 180 196 +5.062500 2.312500 -6.712797 161 179 196 +5.125000 2.312500 -6.712797 160 178 195 +5.187500 2.312500 -6.712797 159 177 194 +5.250000 2.312500 -6.712797 159 177 194 +5.312500 2.312500 -6.712797 158 176 193 +5.375000 2.312500 -6.712797 158 175 192 +5.437500 2.312500 -6.712797 156 174 191 +5.500000 2.312500 -6.712797 155 174 191 +5.562500 2.312500 -6.712797 155 173 190 +5.625000 2.312500 -6.712797 154 173 189 +5.687500 2.312500 -6.712797 154 172 189 +5.750000 2.312500 -6.712797 153 171 189 +5.812500 2.312500 -6.712797 154 171 187 +5.875000 2.312500 -6.712797 153 170 186 +5.937500 2.312500 -6.712797 152 169 186 +6.000000 2.312500 -6.712797 151 168 185 +6.062500 2.312500 -6.712797 149 167 184 +6.125000 2.312500 -6.712797 149 167 184 +6.187500 2.312500 -6.712797 148 166 182 +6.250000 2.312500 -6.712797 148 165 182 +6.312500 2.312500 -6.712797 147 164 181 +6.375000 2.312500 -6.712797 146 163 180 +6.437500 2.312500 -6.712797 145 163 179 +6.500000 2.312500 -6.712797 145 162 179 +6.562500 2.312500 -6.712797 144 161 178 +6.625000 2.312500 -6.712797 143 160 177 +6.687500 2.312500 -6.712797 143 160 176 +6.750000 2.312500 -6.712797 142 160 175 +6.812500 2.312500 -6.712797 141 158 174 +6.875000 2.312500 -6.712797 139 157 174 +6.937500 2.312500 -6.712797 137 155 173 +7.000000 2.312500 -6.712797 136 155 172 +7.062500 2.312500 -6.712797 135 154 171 +7.125000 2.312500 -6.712797 134 153 170 +7.187500 2.312500 -6.712797 134 153 170 +7.250000 2.312500 -6.712797 133 151 168 +7.312500 2.312500 -6.712797 133 151 167 +7.375000 2.312500 -6.712797 132 150 167 +7.437500 2.312500 -6.712797 132 149 166 +7.500000 2.312500 -6.712797 130 148 164 +-0.625000 2.250000 -6.712797 188 202 215 +-0.562500 2.250000 -6.712797 188 202 215 +-0.500000 2.250000 -6.712797 187 202 215 +-0.437500 2.250000 -6.712797 186 201 215 +-0.375000 2.250000 -6.712797 187 201 215 +-0.312500 2.250000 -6.712797 188 202 215 +-0.250000 2.250000 -6.712797 190 202 215 +-0.187500 2.250000 -6.712797 190 203 215 +-0.125000 2.250000 -6.712797 190 203 215 +-0.062500 2.250000 -6.712797 189 202 215 +0.335079 1.507853 -4.498629 190 204 217 +0.374026 1.496104 -4.463574 190 203 217 +0.411311 1.480720 -4.417676 190 203 217 +0.450128 1.473146 -4.395080 190 203 217 +3.062500 2.250000 -6.712797 178 196 212 +3.125000 2.250000 -6.712797 178 196 212 +3.187500 2.250000 -6.712797 177 196 212 +3.250000 2.250000 -6.712797 176 196 212 +3.312500 2.250000 -6.712797 175 196 212 +3.375000 2.250000 -6.712797 175 196 212 +3.437500 2.250000 -6.712797 175 195 211 +3.500000 2.250000 -6.712797 174 194 211 +3.562500 2.250000 -6.712797 174 194 210 +3.625000 2.250000 -6.712797 173 193 210 +3.687500 2.250000 -6.712797 173 192 209 +3.750000 2.250000 -6.712797 173 191 208 +3.812500 2.250000 -6.712797 173 191 208 +3.875000 2.250000 -6.712797 173 190 207 +3.937500 2.250000 -6.712797 173 190 206 +4.000000 2.250000 -6.712797 173 190 205 +4.062500 2.250000 -6.712797 173 189 205 +4.125000 2.250000 -6.712797 173 189 204 +4.187500 2.250000 -6.712797 172 189 203 +4.250000 2.250000 -6.712797 172 188 203 +4.312500 2.250000 -6.712797 171 188 203 +4.375000 2.250000 -6.712797 172 188 203 +4.437500 2.250000 -6.712797 171 187 202 +4.500000 2.250000 -6.712797 170 186 202 +4.562500 2.250000 -6.712797 170 186 201 +4.625000 2.250000 -6.712797 169 184 200 +4.687500 2.250000 -6.712797 168 184 199 +4.750000 2.250000 -6.712797 167 184 199 +4.812500 2.250000 -6.712797 167 183 199 +4.875000 2.250000 -6.712797 166 182 198 +4.937500 2.250000 -6.712797 165 182 197 +5.000000 2.250000 -6.712797 164 181 197 +5.062500 2.250000 -6.712797 163 180 196 +5.125000 2.250000 -6.712797 163 180 196 +5.187500 2.250000 -6.712797 162 179 194 +5.250000 2.250000 -6.712797 162 178 194 +5.312500 2.250000 -6.712797 161 178 193 +5.375000 2.250000 -6.712797 160 177 192 +5.437500 2.250000 -6.712797 160 177 192 +5.500000 2.250000 -6.712797 158 175 191 +5.562500 2.250000 -6.712797 157 175 191 +5.625000 2.250000 -6.712797 156 173 190 +5.687500 2.250000 -6.712797 155 173 189 +5.750000 2.250000 -6.712797 155 173 189 +5.812500 2.250000 -6.712797 155 172 188 +5.875000 2.250000 -6.712797 154 171 187 +5.937500 2.250000 -6.712797 152 170 186 +6.000000 2.250000 -6.712797 151 169 185 +6.062500 2.250000 -6.712797 150 168 184 +6.125000 2.250000 -6.712797 149 167 184 +6.187500 2.250000 -6.712797 148 167 183 +6.250000 2.250000 -6.712797 148 166 182 +6.312500 2.250000 -6.712797 148 165 182 +6.375000 2.250000 -6.712797 147 164 180 +6.437500 2.250000 -6.712797 146 163 180 +6.500000 2.250000 -6.712797 144 162 179 +6.562500 2.250000 -6.712797 144 161 178 +6.625000 2.250000 -6.712797 144 161 178 +6.687500 2.250000 -6.712797 142 160 176 +6.750000 2.250000 -6.712797 141 160 175 +6.812500 2.250000 -6.712797 140 158 175 +6.875000 2.250000 -6.712797 139 157 174 +6.937500 2.250000 -6.712797 138 156 173 +7.000000 2.250000 -6.712797 137 156 173 +7.062500 2.250000 -6.712797 136 155 172 +7.125000 2.250000 -6.712797 135 154 170 +7.187500 2.250000 -6.712797 134 153 170 +7.250000 2.250000 -6.712797 134 152 169 +7.312500 2.250000 -6.712797 133 152 168 +7.375000 2.250000 -6.712797 133 151 167 +7.437500 2.250000 -6.712797 131 149 166 +7.500000 2.250000 -6.712797 130 148 165 +-0.687500 2.187500 -6.712797 189 203 216 +-0.625000 2.187500 -6.712797 189 203 216 +-0.562500 2.187500 -6.712797 189 203 215 +-0.500000 2.187500 -6.712797 189 203 215 +-0.437500 2.187500 -6.712797 188 202 215 +-0.375000 2.187500 -6.712797 189 202 215 +-0.312500 2.187500 -6.712797 189 202 214 +-0.250000 2.187500 -6.712797 191 203 215 +-0.187500 2.187500 -6.712797 192 203 215 +-0.125000 2.187500 -6.712797 192 203 215 +-0.062500 2.187500 -6.712797 191 203 215 +0.000000 2.187500 -6.712797 191 203 215 +0.226950 1.985816 -6.093887 190 204 217 +0.281690 1.971831 -6.050972 190 203 217 +0.375000 1.458333 -4.475198 192 205 217 +0.414508 1.450777 -4.452011 191 204 217 +3.125000 2.187500 -6.712797 179 197 211 +3.187500 2.187500 -6.712797 178 196 211 +3.250000 2.187500 -6.712797 178 196 212 +3.312500 2.187500 -6.712797 178 196 213 +3.375000 2.187500 -6.712797 176 196 211 +3.437500 2.187500 -6.712797 176 195 211 +3.500000 2.187500 -6.712797 175 194 210 +3.562500 2.187500 -6.712797 175 194 210 +3.625000 2.187500 -6.712797 175 194 210 +3.687500 2.187500 -6.712797 174 193 209 +3.750000 2.187500 -6.712797 174 192 209 +3.812500 2.187500 -6.712797 173 192 207 +3.875000 2.187500 -6.712797 174 191 207 +3.937500 2.187500 -6.712797 174 191 206 +4.000000 2.187500 -6.712797 175 191 206 +4.062500 2.187500 -6.712797 175 191 205 +4.125000 2.187500 -6.712797 175 190 205 +4.187500 2.187500 -6.712797 175 190 204 +4.250000 2.187500 -6.712797 174 189 203 +4.312500 2.187500 -6.712797 174 189 203 +4.375000 2.187500 -6.712797 173 189 203 +4.437500 2.187500 -6.712797 172 188 202 +4.500000 2.187500 -6.712797 172 187 202 +4.562500 2.187500 -6.712797 170 186 201 +4.625000 2.187500 -6.712797 170 186 200 +4.687500 2.187500 -6.712797 170 186 200 +4.750000 2.187500 -6.712797 169 184 199 +4.812500 2.187500 -6.712797 169 184 199 +4.875000 2.187500 -6.712797 168 184 198 +4.937500 2.187500 -6.712797 168 183 197 +5.000000 2.187500 -6.712797 168 183 197 +5.062500 2.187500 -6.712797 167 182 197 +5.125000 2.187500 -6.712797 167 182 196 +5.187500 2.187500 -6.712797 167 182 196 +5.250000 2.187500 -6.712797 165 180 195 +5.312500 2.187500 -6.712797 164 180 194 +5.375000 2.187500 -6.712797 162 178 193 +5.437500 2.187500 -6.712797 161 178 192 +5.500000 2.187500 -6.712797 160 176 192 +5.562500 2.187500 -6.712797 159 175 191 +5.625000 2.187500 -6.712797 158 175 190 +5.687500 2.187500 -6.712797 155 173 190 +5.750000 2.187500 -6.712797 155 173 189 +5.812500 2.187500 -6.712797 155 172 188 +5.875000 2.187500 -6.712797 154 171 187 +5.937500 2.187500 -6.712797 153 170 187 +6.000000 2.187500 -6.712797 152 170 186 +6.062500 2.187500 -6.712797 151 169 185 +6.125000 2.187500 -6.712797 151 169 184 +6.187500 2.187500 -6.712797 149 167 183 +6.250000 2.187500 -6.712797 149 167 183 +6.312500 2.187500 -6.712797 148 166 182 +6.375000 2.187500 -6.712797 148 165 181 +6.437500 2.187500 -6.712797 148 164 180 +6.500000 2.187500 -6.712797 146 163 179 +6.562500 2.187500 -6.712797 146 163 179 +6.625000 2.187500 -6.712797 145 162 178 +6.687500 2.187500 -6.712797 143 161 177 +6.750000 2.187500 -6.712797 142 160 176 +6.812500 2.187500 -6.712797 140 159 175 +6.875000 2.187500 -6.712797 139 158 175 +6.937500 2.187500 -6.712797 139 157 174 +7.000000 2.187500 -6.712797 137 156 173 +7.062500 2.187500 -6.712797 137 155 172 +7.125000 2.187500 -6.712797 135 154 170 +7.187500 2.187500 -6.712797 135 154 170 +7.250000 2.187500 -6.712797 135 153 170 +7.312500 2.187500 -6.712797 133 152 168 +7.375000 2.187500 -6.712797 133 151 167 +7.437500 2.187500 -6.712797 132 150 167 +7.500000 2.187500 -6.712797 131 149 166 +-0.687500 2.125000 -6.712797 190 203 216 +-0.625000 2.125000 -6.712797 190 203 216 +-0.562500 2.125000 -6.712797 190 203 216 +-0.500000 2.125000 -6.712797 190 203 216 +-0.437500 2.125000 -6.712797 189 203 215 +-0.375000 2.125000 -6.712797 190 203 215 +-0.312500 2.125000 -6.712797 191 203 215 +-0.250000 2.125000 -6.712797 192 203 215 +-0.187500 2.125000 -6.712797 192 203 215 +-0.125000 2.125000 -6.712797 192 203 215 +-0.062500 2.125000 -6.712797 192 203 215 +0.000000 2.125000 -6.712797 193 205 216 +0.062500 2.125000 -6.712797 193 205 216 +0.180451 2.045113 -6.460436 192 205 217 +0.225352 1.915493 -6.050972 192 205 217 +0.280702 1.908772 -6.029741 192 205 217 +0.335664 1.902098 -6.008657 192 205 217 +0.392982 1.908772 -6.029741 192 205 217 +0.447552 1.902098 -6.008657 192 205 217 +0.500000 1.888889 -5.966931 192 205 217 +0.555556 1.888889 -5.966931 192 205 217 +0.611111 1.888889 -5.966931 192 205 217 +3.125000 2.125000 -6.712797 181 197 212 +3.187500 2.125000 -6.712797 180 197 213 +3.250000 2.125000 -6.712797 179 197 212 +3.312500 2.125000 -6.712797 178 196 212 +3.375000 2.125000 -6.712797 178 196 211 +3.437500 2.125000 -6.712797 177 196 211 +3.500000 2.125000 -6.712797 178 195 210 +3.562500 2.125000 -6.712797 177 195 210 +3.625000 2.125000 -6.712797 176 194 209 +3.687500 2.125000 -6.712797 175 194 209 +3.750000 2.125000 -6.712797 175 193 208 +3.812500 2.125000 -6.712797 175 192 208 +3.875000 2.125000 -6.712797 176 193 208 +3.937500 2.125000 -6.712797 176 192 206 +4.000000 2.125000 -6.712797 176 192 206 +4.062500 2.125000 -6.712797 177 192 206 +4.125000 2.125000 -6.712797 178 192 205 +4.187500 2.125000 -6.712797 177 191 205 +4.250000 2.125000 -6.712797 176 190 204 +4.312500 2.125000 -6.712797 176 191 204 +4.375000 2.125000 -6.712797 175 190 203 +4.437500 2.125000 -6.712797 174 189 203 +4.500000 2.125000 -6.712797 173 188 202 +4.562500 2.125000 -6.712797 173 187 201 +4.625000 2.125000 -6.712797 173 188 201 +4.687500 2.125000 -6.712797 173 187 200 +4.750000 2.125000 -6.712797 173 187 200 +4.812500 2.125000 -6.712797 173 187 200 +4.875000 2.125000 -6.712797 172 186 199 +4.937500 2.125000 -6.712797 170 185 198 +5.000000 2.125000 -6.712797 170 185 198 +5.062500 2.125000 -6.712797 170 184 197 +5.125000 2.125000 -6.712797 169 183 196 +5.187500 2.125000 -6.712797 167 182 196 +5.250000 2.125000 -6.712797 166 181 196 +5.312500 2.125000 -6.712797 164 180 194 +5.375000 2.125000 -6.712797 162 178 194 +5.437500 2.125000 -6.712797 160 178 193 +5.500000 2.125000 -6.712797 160 177 192 +5.562500 2.125000 -6.712797 159 176 192 +5.625000 2.125000 -6.712797 158 175 191 +5.687500 2.125000 -6.712797 157 174 190 +5.750000 2.125000 -6.712797 156 174 189 +5.812500 2.125000 -6.712797 155 173 189 +5.875000 2.125000 -6.712797 155 173 188 +5.937500 2.125000 -6.712797 155 172 187 +6.000000 2.125000 -6.712797 155 171 186 +6.062500 2.125000 -6.712797 154 170 186 +6.125000 2.125000 -6.712797 153 170 185 +6.187500 2.125000 -6.712797 151 168 184 +6.250000 2.125000 -6.712797 151 168 183 +6.312500 2.125000 -6.712797 150 167 182 +6.375000 2.125000 -6.712797 150 167 182 +6.437500 2.125000 -6.712797 149 166 181 +6.500000 2.125000 -6.712797 148 164 180 +6.562500 2.125000 -6.712797 147 163 179 +6.625000 2.125000 -6.712797 146 163 178 +6.687500 2.125000 -6.712797 144 161 177 +6.750000 2.125000 -6.712797 143 160 177 +6.812500 2.125000 -6.712797 141 160 175 +6.875000 2.125000 -6.712797 140 159 175 +6.937500 2.125000 -6.712797 139 157 174 +7.000000 2.125000 -6.712797 138 157 173 +7.062500 2.125000 -6.712797 137 156 173 +7.125000 2.125000 -6.712797 136 155 171 +7.187500 2.125000 -6.712797 136 155 171 +7.250000 2.125000 -6.712797 135 154 170 +7.312500 2.125000 -6.712797 135 153 169 +7.375000 2.125000 -6.712797 133 151 167 +7.437500 2.125000 -6.712797 132 151 167 +7.500000 2.125000 -6.712797 131 149 166 +-0.687500 2.062500 -6.712797 191 204 216 +-0.625000 2.062500 -6.712797 191 204 217 +-0.562500 2.062500 -6.712797 191 204 217 +-0.500000 2.062500 -6.712797 190 203 216 +-0.437500 2.062500 -6.712797 190 203 216 +-0.375000 2.062500 -6.712797 190 203 215 +-0.312500 2.062500 -6.712797 192 203 215 +-0.250000 2.062500 -6.712797 192 203 215 +-0.187500 2.062500 -6.712797 193 204 215 +-0.125000 2.062500 -6.712797 193 204 215 +-0.062500 2.062500 -6.712797 192 204 215 +0.000000 2.062500 -6.712797 193 205 216 +0.062500 2.062500 -6.712797 193 205 216 +0.125000 2.062500 -6.712797 193 205 216 +0.180451 1.984962 -6.460436 193 205 217 +0.225352 1.859155 -6.050972 192 205 217 +0.280702 1.852632 -6.029741 193 205 217 +0.335664 1.846154 -6.008657 193 205 217 +0.391608 1.846154 -6.008657 192 205 217 +0.447552 1.846154 -6.008657 193 205 217 +0.500000 1.833333 -5.966931 193 206 218 +0.555556 1.833333 -5.966931 193 206 217 +0.611111 1.833333 -5.966931 193 206 217 +0.666667 1.833333 -5.966931 194 206 217 +0.719723 1.826990 -5.946284 194 206 217 +0.775087 1.826990 -5.946284 196 207 218 +0.830450 1.826990 -5.946284 195 206 218 +3.187500 2.062500 -6.712797 181 198 212 +3.250000 2.062500 -6.712797 180 197 212 +3.312500 2.062500 -6.712797 180 197 212 +3.375000 2.062500 -6.712797 179 197 211 +3.437500 2.062500 -6.712797 179 197 211 +3.500000 2.062500 -6.712797 178 196 210 +3.562500 2.062500 -6.712797 178 196 210 +3.625000 2.062500 -6.712797 178 195 210 +3.687500 2.062500 -6.712797 178 194 209 +3.750000 2.062500 -6.712797 178 194 209 +3.812500 2.062500 -6.712797 178 194 208 +3.875000 2.062500 -6.712797 177 193 207 +3.937500 2.062500 -6.712797 178 194 207 +4.000000 2.062500 -6.712797 178 193 206 +4.062500 2.062500 -6.712797 178 193 206 +4.125000 2.062500 -6.712797 178 193 206 +4.187500 2.062500 -6.712797 178 192 205 +4.250000 2.062500 -6.712797 178 192 205 +4.312500 2.062500 -6.712797 179 192 205 +4.375000 2.062500 -6.712797 178 191 203 +4.437500 2.062500 -6.712797 177 190 203 +4.500000 2.062500 -6.712797 176 190 203 +4.562500 2.062500 -6.712797 176 190 202 +4.625000 2.062500 -6.712797 176 190 202 +4.687500 2.062500 -6.712797 175 189 201 +4.750000 2.062500 -6.712797 175 189 200 +4.812500 2.062500 -6.712797 175 189 200 +4.875000 2.062500 -6.712797 174 187 200 +4.937500 2.062500 -6.712797 173 186 199 +5.000000 2.062500 -6.712797 173 186 199 +5.062500 2.062500 -6.712797 173 186 198 +5.125000 2.062500 -6.712797 172 185 197 +5.187500 2.062500 -6.712797 170 184 197 +5.250000 2.062500 -6.712797 168 182 196 +5.312500 2.062500 -6.712797 165 180 195 +5.375000 2.062500 -6.712797 163 179 194 +5.437500 2.062500 -6.712797 162 178 193 +5.500000 2.062500 -6.712797 161 178 192 +5.562500 2.062500 -6.712797 160 177 192 +5.625000 2.062500 -6.712797 159 176 191 +5.687500 2.062500 -6.712797 159 175 190 +5.750000 2.062500 -6.712797 159 175 190 +5.812500 2.062500 -6.712797 158 175 189 +5.875000 2.062500 -6.712797 157 173 189 +5.937500 2.062500 -6.712797 157 173 188 +6.000000 2.062500 -6.712797 156 173 187 +6.062500 2.062500 -6.712797 155 171 186 +6.125000 2.062500 -6.712797 155 171 185 +6.187500 2.062500 -6.712797 155 170 184 +6.250000 2.062500 -6.712797 154 170 184 +6.312500 2.062500 -6.712797 152 168 183 +6.375000 2.062500 -6.712797 151 167 182 +6.437500 2.062500 -6.712797 150 167 181 +6.500000 2.062500 -6.712797 149 166 180 +6.562500 2.062500 -6.712797 148 164 179 +6.625000 2.062500 -6.712797 147 163 178 +6.687500 2.062500 -6.712797 146 163 178 +6.750000 2.062500 -6.712797 144 161 177 +6.812500 2.062500 -6.712797 142 160 176 +6.875000 2.062500 -6.712797 141 160 175 +6.937500 2.062500 -6.712797 140 158 174 +7.000000 2.062500 -6.712797 139 158 174 +7.062500 2.062500 -6.712797 139 157 173 +7.125000 2.062500 -6.712797 138 156 172 +7.187500 2.062500 -6.712797 138 155 171 +7.250000 2.062500 -6.712797 137 155 170 +7.312500 2.062500 -6.712797 136 154 169 +7.375000 2.062500 -6.712797 134 153 168 +7.437500 2.062500 -6.712797 134 152 168 +-0.625000 2.000000 -6.712797 192 205 217 +-0.562500 2.000000 -6.712797 192 205 217 +-0.500000 2.000000 -6.712797 192 204 216 +-0.437500 2.000000 -6.712797 191 204 216 +-0.375000 2.000000 -6.712797 191 203 215 +-0.312500 2.000000 -6.712797 192 203 215 +-0.250000 2.000000 -6.712797 192 204 215 +-0.187500 2.000000 -6.712797 193 204 215 +-0.125000 2.000000 -6.712797 193 204 215 +-0.062500 2.000000 -6.712797 193 205 215 +0.000000 2.000000 -6.712797 193 205 216 +0.062500 2.000000 -6.712797 192 205 216 +0.125000 2.000000 -6.712797 193 205 217 +0.187500 2.000000 -6.712797 193 205 217 +0.240602 1.924812 -6.460436 193 205 217 +0.281690 1.802817 -6.050972 194 205 217 +0.335664 1.790210 -6.008657 194 205 217 +0.391608 1.790210 -6.008657 193 205 217 +0.445993 1.783972 -5.987721 193 205 217 +0.500000 1.777778 -5.966931 194 206 218 +0.555556 1.777778 -5.966931 194 206 217 +0.611111 1.777778 -5.966931 194 206 218 +0.666667 1.777778 -5.966931 194 206 218 +0.719723 1.771626 -5.946284 195 206 218 +0.775087 1.771626 -5.946284 196 207 218 +0.830450 1.771626 -5.946284 196 207 218 +0.888889 1.777778 -5.966931 197 208 218 +3.250000 2.000000 -6.712797 182 199 212 +3.312500 2.000000 -6.712797 181 197 211 +3.375000 2.000000 -6.712797 181 197 211 +3.437500 2.000000 -6.712797 180 197 211 +3.500000 2.000000 -6.712797 180 197 211 +3.562500 2.000000 -6.712797 180 197 210 +3.625000 2.000000 -6.712797 179 196 210 +3.687500 2.000000 -6.712797 179 196 209 +3.750000 2.000000 -6.712797 180 195 209 +3.812500 2.000000 -6.712797 180 195 208 +3.875000 2.000000 -6.712797 180 195 208 +3.937500 2.000000 -6.712797 180 194 208 +4.000000 2.000000 -6.712797 179 194 206 +4.062500 2.000000 -6.712797 180 194 206 +4.125000 2.000000 -6.712797 180 194 206 +4.187500 2.000000 -6.712797 180 194 205 +4.250000 2.000000 -6.712797 180 194 205 +4.312500 2.000000 -6.712797 181 194 205 +4.375000 2.000000 -6.712797 180 193 204 +4.437500 2.000000 -6.712797 180 193 204 +4.500000 2.000000 -6.712797 180 192 203 +4.562500 2.000000 -6.712797 180 192 203 +4.625000 2.000000 -6.712797 178 190 202 +4.687500 2.000000 -6.712797 177 190 202 +4.750000 2.000000 -6.712797 176 189 202 +4.812500 2.000000 -6.712797 175 189 200 +4.875000 2.000000 -6.712797 175 188 200 +4.937500 2.000000 -6.712797 174 187 199 +5.000000 2.000000 -6.712797 174 187 199 +5.062500 2.000000 -6.712797 173 186 198 +5.125000 2.000000 -6.712797 173 186 198 +5.187500 2.000000 -6.712797 171 184 197 +5.250000 2.000000 -6.712797 169 183 196 +5.312500 2.000000 -6.712797 167 182 195 +5.375000 2.000000 -6.712797 165 180 194 +5.437500 2.000000 -6.712797 164 180 194 +5.500000 2.000000 -6.712797 163 179 193 +5.562500 2.000000 -6.712797 163 178 192 +5.625000 2.000000 -6.712797 161 178 192 +5.687500 2.000000 -6.712797 161 177 191 +5.750000 2.000000 -6.712797 160 176 190 +5.812500 2.000000 -6.712797 160 175 190 +5.875000 2.000000 -6.712797 158 175 189 +5.937500 2.000000 -6.712797 157 173 188 +6.000000 2.000000 -6.712797 156 173 187 +6.062500 2.000000 -6.712797 156 173 186 +6.125000 2.000000 -6.712797 156 172 186 +6.187500 2.000000 -6.712797 155 171 184 +6.250000 2.000000 -6.712797 155 171 184 +6.312500 2.000000 -6.712797 154 170 183 +6.375000 2.000000 -6.712797 153 169 182 +6.437500 2.000000 -6.712797 151 167 182 +6.500000 2.000000 -6.712797 150 167 181 +6.562500 2.000000 -6.712797 149 166 180 +6.625000 2.000000 -6.712797 148 164 179 +6.687500 2.000000 -6.712797 147 164 178 +6.750000 2.000000 -6.712797 146 162 177 +6.812500 2.000000 -6.712797 146 162 177 +6.875000 2.000000 -6.712797 144 160 175 +6.937500 2.000000 -6.712797 143 160 175 +7.000000 2.000000 -6.712797 142 159 173 +7.062500 2.000000 -6.712797 141 158 173 +7.125000 2.000000 -6.712797 141 158 173 +7.187500 2.000000 -6.712797 140 157 172 +7.250000 2.000000 -6.712797 139 155 170 +7.312500 2.000000 -6.712797 137 155 169 +7.375000 2.000000 -6.712797 136 154 169 +7.437500 2.000000 -6.712797 135 153 168 +-0.562500 1.937500 -6.712797 192 205 216 +-0.500000 1.937500 -6.712797 192 205 216 +-0.437500 1.937500 -6.712797 192 205 216 +-0.375000 1.937500 -6.712797 192 204 215 +-0.312500 1.937500 -6.712797 192 204 215 +-0.250000 1.937500 -6.712797 192 204 215 +-0.187500 1.937500 -6.712797 192 204 215 +-0.125000 1.937500 -6.712797 193 205 215 +-0.062500 1.937500 -6.712797 193 205 215 +0.000000 1.937500 -6.712797 193 205 216 +0.062500 1.937500 -6.712797 193 205 216 +0.125000 1.937500 -6.712797 194 206 217 +0.187500 1.937500 -6.712797 194 206 217 +0.250000 1.937500 -6.712797 194 205 217 +0.300752 1.864662 -6.460436 193 205 216 +0.338028 1.746479 -6.050972 194 206 217 +0.392982 1.740351 -6.029741 194 206 217 +0.447552 1.734266 -6.008657 194 206 217 +0.503497 1.734266 -6.008657 194 206 217 +0.557491 1.728223 -5.987721 195 206 218 +0.613240 1.728223 -5.987721 195 207 218 +0.666667 1.722222 -5.966931 195 207 218 +0.722222 1.722222 -5.966931 195 207 218 +0.777778 1.722222 -5.966931 195 207 218 +0.833333 1.722222 -5.966931 195 207 218 +0.888889 1.722222 -5.966931 196 208 218 +3.250000 1.937500 -6.712797 183 199 213 +3.312500 1.937500 -6.712797 182 199 212 +3.375000 1.937500 -6.712797 182 199 211 +3.437500 1.937500 -6.712797 182 198 211 +3.500000 1.937500 -6.712797 182 197 211 +3.562500 1.937500 -6.712797 182 197 210 +3.625000 1.937500 -6.712797 181 197 210 +3.687500 1.937500 -6.712797 181 197 209 +3.750000 1.937500 -6.712797 181 197 209 +3.812500 1.937500 -6.712797 180 196 208 +3.875000 1.937500 -6.712797 180 196 208 +3.937500 1.937500 -6.712797 180 195 208 +4.000000 1.937500 -6.712797 180 195 207 +4.062500 1.937500 -6.712797 180 195 207 +4.125000 1.937500 -6.712797 180 194 206 +4.187500 1.937500 -6.712797 182 195 206 +4.250000 1.937500 -6.712797 182 195 206 +4.312500 1.937500 -6.712797 182 194 205 +4.375000 1.937500 -6.712797 181 194 205 +4.437500 1.937500 -6.712797 180 193 204 +4.500000 1.937500 -6.712797 181 193 204 +4.562500 1.937500 -6.712797 180 192 203 +4.625000 1.937500 -6.712797 179 192 203 +4.687500 1.937500 -6.712797 178 190 202 +4.750000 1.937500 -6.712797 177 190 201 +4.812500 1.937500 -6.712797 176 189 201 +4.875000 1.937500 -6.712797 175 188 200 +4.937500 1.937500 -6.712797 174 187 200 +5.000000 1.937500 -6.712797 173 186 199 +5.062500 1.937500 -6.712797 173 186 199 +5.125000 1.937500 -6.712797 171 185 198 +5.187500 1.937500 -6.712797 170 184 197 +5.250000 1.937500 -6.712797 168 183 196 +5.312500 1.937500 -6.712797 167 182 196 +5.375000 1.937500 -6.712797 167 182 195 +5.437500 1.937500 -6.712797 166 181 194 +5.500000 1.937500 -6.712797 165 180 194 +5.562500 1.937500 -6.712797 164 180 193 +5.625000 1.937500 -6.712797 163 179 192 +5.687500 1.937500 -6.712797 163 178 191 +5.750000 1.937500 -6.712797 162 178 191 +5.812500 1.937500 -6.712797 160 176 190 +5.875000 1.937500 -6.712797 160 175 189 +5.937500 1.937500 -6.712797 160 175 189 +6.000000 1.937500 -6.712797 159 174 187 +6.062500 1.937500 -6.712797 159 174 187 +6.125000 1.937500 -6.712797 159 173 186 +6.187500 1.937500 -6.712797 158 173 186 +6.250000 1.937500 -6.712797 156 172 185 +6.312500 1.937500 -6.712797 155 170 184 +6.375000 1.937500 -6.712797 155 170 183 +6.437500 1.937500 -6.712797 154 169 182 +6.500000 1.937500 -6.712797 153 168 181 +6.562500 1.937500 -6.712797 152 167 180 +6.625000 1.937500 -6.712797 151 167 180 +6.687500 1.937500 -6.712797 151 166 179 +6.750000 1.937500 -6.712797 149 164 178 +6.812500 1.937500 -6.712797 148 164 178 +6.875000 1.937500 -6.712797 148 163 176 +6.937500 1.937500 -6.712797 147 162 175 +7.000000 1.937500 -6.712797 145 160 174 +7.062500 1.937500 -6.712797 145 160 174 +7.125000 1.937500 -6.712797 143 159 173 +7.187500 1.937500 -6.712797 142 158 172 +7.250000 1.937500 -6.712797 140 157 171 +7.312500 1.937500 -6.712797 139 155 170 +7.375000 1.937500 -6.712797 137 155 169 +7.437500 1.937500 -6.712797 136 154 168 +-0.500000 1.875000 -6.712797 193 205 217 +-0.437500 1.875000 -6.712797 192 205 216 +-0.375000 1.875000 -6.712797 192 204 215 +-0.312500 1.875000 -6.712797 192 205 216 +-0.250000 1.875000 -6.712797 192 204 216 +-0.187500 1.875000 -6.712797 192 205 215 +-0.125000 1.875000 -6.712797 193 205 216 +-0.062500 1.875000 -6.712797 194 205 215 +0.000000 1.875000 -6.712797 194 205 216 +0.062500 1.875000 -6.712797 194 206 217 +0.125000 1.875000 -6.712797 194 206 217 +0.187500 1.875000 -6.712797 194 206 217 +0.250000 1.875000 -6.712797 194 206 217 +0.312500 1.875000 -6.712797 194 206 217 +0.354244 1.771218 -6.341240 194 205 216 +0.395760 1.696113 -6.072354 193 205 217 +0.449123 1.684211 -6.029741 193 206 217 +0.505263 1.684211 -6.029741 194 206 218 +0.559441 1.678322 -6.008657 195 206 218 +0.615385 1.678322 -6.008657 195 207 218 +0.668990 1.672474 -5.987721 195 207 218 +0.722222 1.666667 -5.966931 195 207 218 +0.777778 1.666667 -5.966931 195 207 218 +0.833333 1.666667 -5.966931 195 207 218 +0.888889 1.666667 -5.966931 196 208 219 +0.947735 1.672474 -5.987721 197 208 219 +3.250000 1.875000 -6.712797 184 200 213 +3.312500 1.875000 -6.712797 184 199 212 +3.375000 1.875000 -6.712797 184 199 211 +3.437500 1.875000 -6.712797 183 199 211 +3.500000 1.875000 -6.712797 183 198 211 +3.562500 1.875000 -6.712797 183 198 210 +3.625000 1.875000 -6.712797 183 198 210 +3.687500 1.875000 -6.712797 183 197 210 +3.750000 1.875000 -6.712797 182 197 209 +3.812500 1.875000 -6.712797 182 197 209 +3.875000 1.875000 -6.712797 182 196 208 +3.937500 1.875000 -6.712797 182 196 208 +4.000000 1.875000 -6.712797 181 196 207 +4.062500 1.875000 -6.712797 181 196 207 +4.125000 1.875000 -6.712797 181 194 206 +4.187500 1.875000 -6.712797 181 195 206 +4.250000 1.875000 -6.712797 181 194 206 +4.312500 1.875000 -6.712797 181 194 206 +4.375000 1.875000 -6.712797 180 194 205 +4.437500 1.875000 -6.712797 180 193 204 +4.500000 1.875000 -6.712797 179 192 203 +4.562500 1.875000 -6.712797 179 192 203 +4.625000 1.875000 -6.712797 178 192 203 +4.687500 1.875000 -6.712797 178 191 202 +4.750000 1.875000 -6.712797 177 190 202 +4.812500 1.875000 -6.712797 175 189 201 +4.875000 1.875000 -6.712797 175 189 200 +4.937500 1.875000 -6.712797 173 188 200 +5.000000 1.875000 -6.712797 173 186 199 +5.062500 1.875000 -6.712797 172 186 199 +5.125000 1.875000 -6.712797 171 186 198 +5.187500 1.875000 -6.712797 170 185 197 +5.250000 1.875000 -6.712797 170 184 197 +5.312500 1.875000 -6.712797 170 184 196 +5.375000 1.875000 -6.712797 169 183 196 +5.437500 1.875000 -6.712797 168 182 195 +5.500000 1.875000 -6.712797 167 182 194 +5.562500 1.875000 -6.712797 167 182 194 +5.625000 1.875000 -6.712797 167 181 193 +5.687500 1.875000 -6.712797 167 180 192 +5.750000 1.875000 -6.712797 166 180 192 +5.812500 1.875000 -6.712797 165 179 191 +5.875000 1.875000 -6.712797 163 178 190 +5.937500 1.875000 -6.712797 163 177 189 +6.000000 1.875000 -6.712797 161 176 189 +6.062500 1.875000 -6.712797 161 175 188 +6.125000 1.875000 -6.712797 160 175 187 +6.187500 1.875000 -6.712797 160 173 186 +6.250000 1.875000 -6.712797 158 173 185 +6.312500 1.875000 -6.712797 156 172 184 +6.375000 1.875000 -6.712797 155 171 183 +6.437500 1.875000 -6.712797 155 170 183 +6.500000 1.875000 -6.712797 155 170 182 +6.562500 1.875000 -6.712797 155 169 181 +6.625000 1.875000 -6.712797 155 169 181 +6.687500 1.875000 -6.712797 152 167 179 +6.750000 1.875000 -6.712797 151 166 178 +6.812500 1.875000 -6.712797 149 165 178 +6.875000 1.875000 -6.712797 148 163 176 +6.937500 1.875000 -6.712797 148 163 176 +7.000000 1.875000 -6.712797 146 161 175 +7.062500 1.875000 -6.712797 146 161 175 +7.125000 1.875000 -6.712797 144 160 173 +7.187500 1.875000 -6.712797 143 159 173 +7.250000 1.875000 -6.712797 141 157 171 +7.312500 1.875000 -6.712797 140 156 170 +7.375000 1.875000 -6.712797 139 155 170 +7.437500 1.875000 -6.712797 138 155 169 +-0.437500 1.812500 -6.712797 194 205 215 +-0.375000 1.812500 -6.712797 194 205 215 +-0.312500 1.812500 -6.712797 194 205 216 +-0.250000 1.812500 -6.712797 194 205 215 +-0.187500 1.812500 -6.712797 193 205 215 +-0.125000 1.812500 -6.712797 194 205 216 +-0.062500 1.812500 -6.712797 194 205 215 +0.000000 1.812500 -6.712797 194 205 216 +0.062500 1.812500 -6.712797 195 206 216 +0.125000 1.812500 -6.712797 196 206 217 +0.187500 1.812500 -6.712797 196 206 217 +0.250000 1.812500 -6.712797 195 206 216 +0.312500 1.812500 -6.712797 194 205 216 +0.359551 1.737828 -6.436240 194 206 216 +0.410256 1.699634 -6.294784 194 206 217 +0.455516 1.651246 -6.115573 194 206 217 +0.507042 1.633803 -6.050972 194 206 218 +0.561404 1.628070 -6.029741 195 207 218 +0.617544 1.628070 -6.029741 196 208 218 +0.671329 1.622378 -6.008657 196 208 218 +0.724739 1.616725 -5.987721 196 207 218 +0.780488 1.616725 -5.987721 196 208 218 +0.836237 1.616725 -5.987721 196 208 219 +0.891986 1.616725 -5.987721 197 208 219 +0.954386 1.628070 -6.029741 197 209 219 +3.187500 1.812500 -6.712797 186 202 213 +3.250000 1.812500 -6.712797 186 201 213 +3.312500 1.812500 -6.712797 186 201 213 +3.375000 1.812500 -6.712797 186 200 212 +3.437500 1.812500 -6.712797 185 200 211 +3.500000 1.812500 -6.712797 186 200 211 +3.562500 1.812500 -6.712797 185 199 211 +3.625000 1.812500 -6.712797 184 199 210 +3.687500 1.812500 -6.712797 184 198 210 +3.750000 1.812500 -6.712797 184 198 209 +3.812500 1.812500 -6.712797 184 197 209 +3.875000 1.812500 -6.712797 184 197 209 +3.937500 1.812500 -6.712797 184 197 208 +4.000000 1.812500 -6.712797 183 197 208 +4.062500 1.812500 -6.712797 182 196 208 +4.125000 1.812500 -6.712797 182 196 207 +4.187500 1.812500 -6.712797 182 196 207 +4.250000 1.812500 -6.712797 181 195 206 +4.312500 1.812500 -6.712797 181 195 206 +4.375000 1.812500 -6.712797 180 194 205 +4.437500 1.812500 -6.712797 180 193 205 +4.500000 1.812500 -6.712797 179 192 204 +4.562500 1.812500 -6.712797 178 192 203 +4.625000 1.812500 -6.712797 178 192 203 +4.687500 1.812500 -6.712797 178 191 203 +4.750000 1.812500 -6.712797 177 190 202 +4.812500 1.812500 -6.712797 176 190 201 +4.875000 1.812500 -6.712797 175 189 201 +4.937500 1.812500 -6.712797 174 188 200 +5.000000 1.812500 -6.712797 173 187 200 +5.062500 1.812500 -6.712797 173 186 199 +5.125000 1.812500 -6.712797 172 186 199 +5.187500 1.812500 -6.712797 172 186 198 +5.250000 1.812500 -6.712797 172 186 197 +5.312500 1.812500 -6.712797 172 185 197 +5.375000 1.812500 -6.712797 171 184 196 +5.437500 1.812500 -6.712797 172 184 196 +5.500000 1.812500 -6.712797 171 184 195 +5.562500 1.812500 -6.712797 172 184 195 +5.625000 1.812500 -6.712797 171 183 194 +5.687500 1.812500 -6.712797 170 183 193 +5.750000 1.812500 -6.712797 170 182 192 +5.812500 1.812500 -6.712797 168 181 192 +5.875000 1.812500 -6.712797 167 180 190 +5.937500 1.812500 -6.712797 166 179 190 +6.000000 1.812500 -6.712797 164 178 189 +6.062500 1.812500 -6.712797 163 176 189 +6.125000 1.812500 -6.712797 162 175 187 +6.187500 1.812500 -6.712797 160 175 186 +6.250000 1.812500 -6.712797 160 174 186 +6.312500 1.812500 -6.712797 160 173 185 +6.375000 1.812500 -6.712797 159 173 184 +6.437500 1.812500 -6.712797 158 172 183 +6.500000 1.812500 -6.712797 157 171 182 +6.562500 1.812500 -6.712797 155 169 181 +6.625000 1.812500 -6.712797 154 168 180 +6.687500 1.812500 -6.712797 152 167 179 +6.750000 1.812500 -6.712797 151 166 179 +6.812500 1.812500 -6.712797 149 165 178 +6.875000 1.812500 -6.712797 149 164 178 +6.937500 1.812500 -6.712797 148 163 176 +7.000000 1.812500 -6.712797 147 162 175 +7.062500 1.812500 -6.712797 147 162 175 +7.125000 1.812500 -6.712797 145 160 173 +7.187500 1.812500 -6.712797 143 160 173 +7.250000 1.812500 -6.712797 142 158 172 +7.312500 1.812500 -6.712797 141 157 170 +7.375000 1.812500 -6.712797 139 156 170 +7.437500 1.812500 -6.712797 138 155 169 +-0.375000 1.750000 -6.712797 197 206 215 +-0.312500 1.750000 -6.712797 196 206 215 +-0.250000 1.750000 -6.712797 196 206 215 +-0.187500 1.750000 -6.712797 195 205 215 +-0.125000 1.750000 -6.712797 195 206 215 +-0.062500 1.750000 -6.712797 195 206 216 +0.000000 1.750000 -6.712797 195 206 216 +0.062500 1.750000 -6.712797 196 206 217 +0.125000 1.750000 -6.712797 197 207 217 +0.187500 1.750000 -6.712797 196 206 217 +0.250000 1.750000 -6.712797 196 206 216 +0.312500 1.750000 -6.712797 195 206 216 +0.375000 1.750000 -6.712797 195 206 216 +0.419476 1.677903 -6.436240 194 206 217 +0.468864 1.641026 -6.294784 194 206 217 +0.517986 1.611511 -6.181569 195 206 217 +0.569395 1.594306 -6.115573 196 207 218 +0.621908 1.583039 -6.072354 196 208 218 +0.676056 1.577465 -6.050972 197 208 218 +0.732394 1.577465 -6.050972 197 208 218 +0.791519 1.583039 -6.072354 197 208 218 +0.848057 1.583039 -6.072354 197 208 218 +0.901408 1.577465 -6.050972 199 209 218 +0.978417 1.611511 -6.181569 200 210 218 +1.054945 1.641026 -6.294784 202 210 219 +3.125000 1.750000 -6.712797 189 202 214 +3.187500 1.750000 -6.712797 189 202 214 +3.250000 1.750000 -6.712797 188 202 213 +3.312500 1.750000 -6.712797 188 202 213 +3.375000 1.750000 -6.712797 188 202 213 +3.437500 1.750000 -6.712797 188 201 212 +3.500000 1.750000 -6.712797 187 201 211 +3.562500 1.750000 -6.712797 186 200 211 +3.625000 1.750000 -6.712797 186 200 210 +3.687500 1.750000 -6.712797 186 199 210 +3.750000 1.750000 -6.712797 186 199 210 +3.812500 1.750000 -6.712797 186 199 210 +3.875000 1.750000 -6.712797 185 198 209 +3.937500 1.750000 -6.712797 184 198 209 +4.000000 1.750000 -6.712797 184 197 208 +4.062500 1.750000 -6.712797 184 197 208 +4.125000 1.750000 -6.712797 184 197 208 +4.187500 1.750000 -6.712797 182 196 207 +4.250000 1.750000 -6.712797 182 195 206 +4.312500 1.750000 -6.712797 181 195 206 +4.375000 1.750000 -6.712797 180 194 205 +4.437500 1.750000 -6.712797 180 194 205 +4.500000 1.750000 -6.712797 180 194 205 +4.562500 1.750000 -6.712797 180 193 204 +4.625000 1.750000 -6.712797 179 192 203 +4.687500 1.750000 -6.712797 179 192 203 +4.750000 1.750000 -6.712797 178 191 202 +4.812500 1.750000 -6.712797 177 190 202 +4.875000 1.750000 -6.712797 176 190 201 +4.937500 1.750000 -6.712797 175 189 200 +5.000000 1.750000 -6.712797 175 189 200 +5.062500 1.750000 -6.712797 174 188 199 +5.125000 1.750000 -6.712797 174 187 199 +5.187500 1.750000 -6.712797 174 187 199 +5.250000 1.750000 -6.712797 173 186 198 +5.312500 1.750000 -6.712797 173 186 197 +5.375000 1.750000 -6.712797 173 186 197 +5.437500 1.750000 -6.712797 174 186 196 +5.500000 1.750000 -6.712797 173 185 196 +5.562500 1.750000 -6.712797 173 185 196 +5.625000 1.750000 -6.712797 173 184 194 +5.687500 1.750000 -6.712797 173 184 194 +5.750000 1.750000 -6.712797 172 183 193 +5.812500 1.750000 -6.712797 171 182 193 +5.875000 1.750000 -6.712797 169 181 192 +5.937500 1.750000 -6.712797 167 180 190 +6.000000 1.750000 -6.712797 165 178 189 +6.062500 1.750000 -6.712797 165 178 189 +6.125000 1.750000 -6.712797 163 177 188 +6.187500 1.750000 -6.712797 162 175 187 +6.250000 1.750000 -6.712797 161 175 186 +6.312500 1.750000 -6.712797 162 175 186 +6.375000 1.750000 -6.712797 161 174 185 +6.437500 1.750000 -6.712797 160 173 184 +6.500000 1.750000 -6.712797 159 172 183 +6.562500 1.750000 -6.712797 157 170 182 +6.625000 1.750000 -6.712797 155 169 181 +6.687500 1.750000 -6.712797 153 167 180 +6.750000 1.750000 -6.712797 150 166 179 +6.812500 1.750000 -6.712797 150 165 178 +6.875000 1.750000 -6.712797 150 165 178 +6.937500 1.750000 -6.712797 150 165 177 +7.000000 1.750000 -6.712797 148 163 175 +7.062500 1.750000 -6.712797 148 163 175 +7.125000 1.750000 -6.712797 145 160 174 +7.187500 1.750000 -6.712797 144 160 173 +7.250000 1.750000 -6.712797 142 158 172 +7.312500 1.750000 -6.712797 141 158 172 +7.375000 1.750000 -6.712797 139 156 170 +7.437500 1.750000 -6.712797 137 155 169 +-0.312500 1.687500 -6.712797 199 207 215 +-0.250000 1.687500 -6.712797 198 207 215 +-0.187500 1.687500 -6.712797 197 206 215 +-0.125000 1.687500 -6.712797 197 206 215 +-0.062500 1.687500 -6.712797 197 207 216 +0.000000 1.687500 -6.712797 196 206 216 +0.062500 1.687500 -6.712797 197 207 217 +0.125000 1.687500 -6.712797 197 208 217 +0.187500 1.687500 -6.712797 197 207 216 +0.250000 1.687500 -6.712797 197 207 216 +0.312500 1.687500 -6.712797 196 206 216 +0.375000 1.687500 -6.712797 196 206 215 +0.437500 1.687500 -6.712797 196 206 216 +0.479401 1.617977 -6.436240 196 206 217 +0.529412 1.588235 -6.317927 196 207 217 +0.588235 1.588235 -6.317927 196 207 217 +0.640000 1.570909 -6.249004 196 208 218 +0.693141 1.559567 -6.203885 197 208 218 +0.750903 1.559567 -6.203885 198 209 218 +0.820513 1.582418 -6.294784 199 209 218 +0.879121 1.582418 -6.294784 199 209 218 +0.920863 1.553957 -6.181569 201 210 219 +0.996337 1.582418 -6.294784 202 211 219 +1.125000 1.687500 -6.712797 204 212 219 +1.187500 1.687500 -6.712797 206 213 220 +1.250000 1.687500 -6.712797 206 213 220 +2.937500 1.687500 -6.712797 191 205 215 +3.000000 1.687500 -6.712797 190 204 215 +3.062500 1.687500 -6.712797 190 203 214 +3.125000 1.687500 -6.712797 190 203 214 +3.187500 1.687500 -6.712797 190 203 214 +3.250000 1.687500 -6.712797 189 203 213 +3.312500 1.687500 -6.712797 189 202 213 +3.375000 1.687500 -6.712797 189 202 213 +3.437500 1.687500 -6.712797 189 202 212 +3.500000 1.687500 -6.712797 189 202 211 +3.562500 1.687500 -6.712797 189 201 211 +3.625000 1.687500 -6.712797 188 201 211 +3.687500 1.687500 -6.712797 187 200 210 +3.750000 1.687500 -6.712797 187 200 210 +3.812500 1.687500 -6.712797 186 199 210 +3.875000 1.687500 -6.712797 186 199 210 +3.937500 1.687500 -6.712797 186 199 209 +4.000000 1.687500 -6.712797 186 198 208 +4.062500 1.687500 -6.712797 185 198 208 +4.125000 1.687500 -6.712797 184 197 208 +4.187500 1.687500 -6.712797 184 197 208 +4.250000 1.687500 -6.712797 183 196 207 +4.312500 1.687500 -6.712797 182 196 206 +4.375000 1.687500 -6.712797 182 195 206 +4.437500 1.687500 -6.712797 182 195 205 +4.500000 1.687500 -6.712797 181 194 205 +4.562500 1.687500 -6.712797 181 194 205 +4.625000 1.687500 -6.712797 180 193 204 +4.687500 1.687500 -6.712797 180 192 203 +4.750000 1.687500 -6.712797 180 192 203 +4.812500 1.687500 -6.712797 178 192 202 +4.875000 1.687500 -6.712797 177 190 202 +4.937500 1.687500 -6.712797 176 190 201 +5.000000 1.687500 -6.712797 175 189 200 +5.062500 1.687500 -6.712797 175 189 200 +5.125000 1.687500 -6.712797 174 188 199 +5.187500 1.687500 -6.712797 174 187 199 +5.250000 1.687500 -6.712797 173 186 198 +5.312500 1.687500 -6.712797 173 186 197 +5.375000 1.687500 -6.712797 173 186 197 +5.437500 1.687500 -6.712797 172 185 196 +5.500000 1.687500 -6.712797 172 184 196 +5.562500 1.687500 -6.712797 172 184 195 +5.625000 1.687500 -6.712797 171 184 194 +5.687500 1.687500 -6.712797 170 183 194 +5.750000 1.687500 -6.712797 170 182 193 +5.812500 1.687500 -6.712797 169 182 192 +5.875000 1.687500 -6.712797 167 180 191 +5.937500 1.687500 -6.712797 166 179 190 +6.000000 1.687500 -6.712797 165 178 190 +6.062500 1.687500 -6.712797 163 178 189 +6.125000 1.687500 -6.712797 162 176 188 +6.187500 1.687500 -6.712797 162 176 187 +6.250000 1.687500 -6.712797 161 175 186 +6.312500 1.687500 -6.712797 162 175 186 +6.375000 1.687500 -6.712797 160 174 185 +6.437500 1.687500 -6.712797 159 173 184 +6.500000 1.687500 -6.712797 159 173 183 +6.562500 1.687500 -6.712797 157 171 182 +6.625000 1.687500 -6.712797 155 169 181 +6.687500 1.687500 -6.712797 153 168 180 +6.750000 1.687500 -6.712797 151 167 180 +6.812500 1.687500 -6.712797 150 166 178 +6.875000 1.687500 -6.712797 149 164 178 +6.937500 1.687500 -6.712797 148 163 176 +7.000000 1.687500 -6.712797 147 163 176 +7.062500 1.687500 -6.712797 145 162 175 +7.125000 1.687500 -6.712797 144 160 174 +7.187500 1.687500 -6.712797 143 160 173 +7.250000 1.687500 -6.712797 141 159 173 +7.312500 1.687500 -6.712797 140 158 172 +7.375000 1.687500 -6.712797 139 157 171 +7.437500 1.687500 -6.712797 137 155 170 +-0.187500 1.625000 -6.712797 199 207 215 +-0.125000 1.625000 -6.712797 199 207 215 +-0.062500 1.625000 -6.712797 199 207 215 +0.000000 1.625000 -6.712797 198 208 216 +0.062500 1.625000 -6.712797 197 207 216 +0.125000 1.625000 -6.712797 198 208 217 +0.187500 1.625000 -6.712797 197 208 216 +0.250000 1.625000 -6.712797 198 208 217 +0.312500 1.625000 -6.712797 197 207 216 +0.375000 1.625000 -6.712797 197 206 215 +0.437500 1.625000 -6.712797 197 206 216 +0.500000 1.625000 -6.712797 197 207 216 +0.562500 1.625000 -6.712797 196 207 217 +0.625000 1.625000 -6.712797 196 208 218 +0.687500 1.625000 -6.712797 197 208 218 +0.750000 1.625000 -6.712797 197 208 218 +0.812500 1.625000 -6.712797 197 209 218 +0.875000 1.625000 -6.712797 199 209 218 +0.937500 1.625000 -6.712797 201 210 218 +1.000000 1.625000 -6.712797 203 211 219 +1.062500 1.625000 -6.712797 203 211 219 +1.125000 1.625000 -6.712797 205 212 219 +1.187500 1.625000 -6.712797 205 213 219 +1.250000 1.625000 -6.712797 206 214 220 +1.312500 1.625000 -6.712797 207 214 220 +2.625000 1.625000 -6.712797 192 207 218 +2.687500 1.625000 -6.712797 192 206 218 +2.750000 1.625000 -6.712797 192 206 217 +2.812500 1.625000 -6.712797 192 205 216 +2.875000 1.625000 -6.712797 192 205 216 +2.937500 1.625000 -6.712797 192 205 215 +3.000000 1.625000 -6.712797 192 205 215 +3.062500 1.625000 -6.712797 192 204 214 +3.125000 1.625000 -6.712797 192 204 215 +3.187500 1.625000 -6.712797 191 203 214 +3.250000 1.625000 -6.712797 190 203 213 +3.312500 1.625000 -6.712797 190 203 213 +3.375000 1.625000 -6.712797 190 203 213 +3.437500 1.625000 -6.712797 190 203 213 +3.500000 1.625000 -6.712797 190 202 212 +3.562500 1.625000 -6.712797 189 202 211 +3.625000 1.625000 -6.712797 189 202 211 +3.687500 1.625000 -6.712797 188 201 211 +3.750000 1.625000 -6.712797 189 201 210 +3.812500 1.625000 -6.712797 188 200 210 +3.875000 1.625000 -6.712797 188 200 210 +3.937500 1.625000 -6.712797 187 200 209 +4.000000 1.625000 -6.712797 187 199 209 +4.062500 1.625000 -6.712797 186 199 208 +4.125000 1.625000 -6.712797 186 198 208 +4.187500 1.625000 -6.712797 184 197 207 +4.250000 1.625000 -6.712797 184 197 207 +4.312500 1.625000 -6.712797 183 196 206 +4.375000 1.625000 -6.712797 182 196 206 +4.437500 1.625000 -6.712797 182 196 206 +4.500000 1.625000 -6.712797 182 195 205 +4.562500 1.625000 -6.712797 182 195 205 +4.625000 1.625000 -6.712797 181 194 204 +4.687500 1.625000 -6.712797 180 193 203 +4.750000 1.625000 -6.712797 180 193 203 +4.812500 1.625000 -6.712797 180 192 203 +4.875000 1.625000 -6.712797 178 191 202 +4.937500 1.625000 -6.712797 178 190 202 +5.000000 1.625000 -6.712797 176 190 200 +5.062500 1.625000 -6.712797 176 189 200 +5.125000 1.625000 -6.712797 175 189 200 +5.187500 1.625000 -6.712797 174 188 199 +5.250000 1.625000 -6.712797 174 187 199 +5.312500 1.625000 -6.712797 174 187 198 +5.375000 1.625000 -6.712797 173 186 197 +5.437500 1.625000 -6.712797 172 185 196 +5.500000 1.625000 -6.712797 172 185 196 +5.562500 1.625000 -6.712797 170 184 195 +5.625000 1.625000 -6.712797 170 183 194 +5.687500 1.625000 -6.712797 168 182 193 +5.750000 1.625000 -6.712797 167 182 192 +5.812500 1.625000 -6.712797 167 181 192 +5.875000 1.625000 -6.712797 166 180 191 +5.937500 1.625000 -6.712797 165 179 190 +6.000000 1.625000 -6.712797 163 178 190 +6.062500 1.625000 -6.712797 163 178 189 +6.125000 1.625000 -6.712797 162 177 189 +6.187500 1.625000 -6.712797 161 175 187 +6.250000 1.625000 -6.712797 160 175 186 +6.312500 1.625000 -6.712797 160 174 186 +6.375000 1.625000 -6.712797 160 174 185 +6.437500 1.625000 -6.712797 158 173 184 +6.500000 1.625000 -6.712797 157 172 183 +6.562500 1.625000 -6.712797 155 170 182 +6.625000 1.625000 -6.712797 154 169 181 +6.687500 1.625000 -6.712797 152 168 180 +6.750000 1.625000 -6.712797 150 167 180 +6.812500 1.625000 -6.712797 149 166 179 +6.875000 1.625000 -6.712797 148 164 178 +6.937500 1.625000 -6.712797 147 163 177 +7.000000 1.625000 -6.712797 146 163 176 +7.062500 1.625000 -6.712797 144 162 175 +7.125000 1.625000 -6.712797 143 160 175 +7.187500 1.625000 -6.712797 142 160 174 +7.250000 1.625000 -6.712797 141 159 173 +7.312500 1.625000 -6.712797 141 159 173 +7.375000 1.625000 -6.712797 140 157 171 +7.437500 1.625000 -6.712797 139 157 170 +0.437500 1.562500 -6.712797 198 207 216 +0.500000 1.562500 -6.712797 197 208 217 +0.562500 1.562500 -6.712797 196 207 217 +0.625000 1.562500 -6.712797 196 208 218 +0.687500 1.562500 -6.712797 197 208 218 +0.750000 1.562500 -6.712797 197 209 218 +0.812500 1.562500 -6.712797 199 209 219 +0.875000 1.562500 -6.712797 199 209 218 +0.937500 1.562500 -6.712797 202 211 219 +1.000000 1.562500 -6.712797 203 211 219 +1.062500 1.562500 -6.712797 203 211 219 +1.125000 1.562500 -6.712797 204 212 219 +1.187500 1.562500 -6.712797 206 213 220 +1.250000 1.562500 -6.712797 206 213 220 +1.312500 1.562500 -6.712797 206 213 220 +1.375000 1.562500 -6.712797 205 213 220 +2.562500 1.562500 -6.712797 194 208 218 +2.625000 1.562500 -6.712797 194 208 218 +2.687500 1.562500 -6.712797 193 207 217 +2.750000 1.562500 -6.712797 194 206 217 +2.812500 1.562500 -6.712797 194 206 217 +2.875000 1.562500 -6.712797 194 206 216 +2.937500 1.562500 -6.712797 194 206 216 +3.000000 1.562500 -6.712797 194 206 215 +3.062500 1.562500 -6.712797 194 205 215 +3.125000 1.562500 -6.712797 192 205 214 +3.187500 1.562500 -6.712797 192 205 214 +3.250000 1.562500 -6.712797 192 205 214 +3.312500 1.562500 -6.712797 192 203 213 +3.375000 1.562500 -6.712797 191 203 213 +3.437500 1.562500 -6.712797 191 203 213 +3.500000 1.562500 -6.712797 190 203 213 +3.562500 1.562500 -6.712797 190 202 212 +3.625000 1.562500 -6.712797 190 202 211 +3.687500 1.562500 -6.712797 189 202 211 +3.750000 1.562500 -6.712797 189 202 211 +3.812500 1.562500 -6.712797 189 201 210 +3.875000 1.562500 -6.712797 189 201 210 +3.937500 1.562500 -6.712797 189 200 210 +4.000000 1.562500 -6.712797 188 200 209 +4.062500 1.562500 -6.712797 187 200 209 +4.125000 1.562500 -6.712797 187 199 209 +4.187500 1.562500 -6.712797 185 198 208 +4.250000 1.562500 -6.712797 185 197 207 +4.312500 1.562500 -6.712797 184 197 207 +4.375000 1.562500 -6.712797 184 197 206 +4.437500 1.562500 -6.712797 184 196 206 +4.500000 1.562500 -6.712797 184 196 206 +4.562500 1.562500 -6.712797 183 195 205 +4.625000 1.562500 -6.712797 182 195 205 +4.687500 1.562500 -6.712797 182 194 204 +4.750000 1.562500 -6.712797 181 194 203 +4.812500 1.562500 -6.712797 180 192 203 +4.875000 1.562500 -6.712797 180 192 202 +4.937500 1.562500 -6.712797 179 192 202 +5.000000 1.562500 -6.712797 178 190 201 +5.062500 1.562500 -6.712797 178 190 200 +5.125000 1.562500 -6.712797 177 190 200 +5.187500 1.562500 -6.712797 177 189 200 +5.250000 1.562500 -6.712797 175 189 199 +5.312500 1.562500 -6.712797 175 188 198 +5.375000 1.562500 -6.712797 174 187 197 +5.437500 1.562500 -6.712797 173 186 197 +5.500000 1.562500 -6.712797 172 185 196 +5.562500 1.562500 -6.712797 171 184 195 +5.625000 1.562500 -6.712797 170 183 194 +5.687500 1.562500 -6.712797 168 182 194 +5.750000 1.562500 -6.712797 167 182 193 +5.812500 1.562500 -6.712797 167 181 192 +5.875000 1.562500 -6.712797 166 180 192 +5.937500 1.562500 -6.712797 165 180 191 +6.000000 1.562500 -6.712797 163 178 190 +6.062500 1.562500 -6.712797 162 177 189 +6.125000 1.562500 -6.712797 161 177 189 +6.187500 1.562500 -6.712797 160 175 187 +6.250000 1.562500 -6.712797 160 175 187 +6.312500 1.562500 -6.712797 159 174 186 +6.375000 1.562500 -6.712797 158 173 185 +6.437500 1.562500 -6.712797 157 173 184 +6.500000 1.562500 -6.712797 155 171 184 +6.562500 1.562500 -6.712797 155 170 183 +6.625000 1.562500 -6.712797 153 169 182 +6.687500 1.562500 -6.712797 152 168 181 +6.750000 1.562500 -6.712797 151 167 180 +6.812500 1.562500 -6.712797 150 167 180 +6.875000 1.562500 -6.712797 150 166 179 +6.937500 1.562500 -6.712797 148 165 178 +7.000000 1.562500 -6.712797 148 164 177 +7.062500 1.562500 -6.712797 147 163 176 +7.125000 1.562500 -6.712797 146 163 175 +7.187500 1.562500 -6.712797 145 161 174 +7.250000 1.562500 -6.712797 145 161 174 +7.312500 1.562500 -6.712797 143 160 173 +7.375000 1.562500 -6.712797 142 159 172 +7.437500 1.562500 -6.712797 141 158 170 +0.875000 1.500000 -6.712797 200 210 218 +0.937500 1.500000 -6.712797 203 211 219 +1.000000 1.500000 -6.712797 203 211 219 +1.062500 1.500000 -6.712797 203 211 219 +1.125000 1.500000 -6.712797 205 212 219 +1.187500 1.500000 -6.712797 205 213 219 +1.250000 1.500000 -6.712797 205 213 220 +1.312500 1.500000 -6.712797 205 213 220 +1.375000 1.500000 -6.712797 205 213 219 +1.437500 1.500000 -6.712797 205 213 219 +2.062500 1.500000 -6.712797 199 210 219 +2.125000 1.500000 -6.712797 199 209 218 +2.187500 1.500000 -6.712797 198 209 219 +2.500000 1.500000 -6.712797 197 208 218 +2.562500 1.500000 -6.712797 197 208 217 +2.625000 1.500000 -6.712797 196 208 217 +2.687500 1.500000 -6.712797 196 208 217 +2.750000 1.500000 -6.712797 196 207 217 +2.812500 1.500000 -6.712797 196 208 217 +2.875000 1.500000 -6.712797 196 207 216 +2.937500 1.500000 -6.712797 195 206 216 +3.000000 1.500000 -6.712797 195 206 216 +3.062500 1.500000 -6.712797 194 206 215 +3.125000 1.500000 -6.712797 194 205 215 +3.187500 1.500000 -6.712797 194 205 215 +3.250000 1.500000 -6.712797 193 205 214 +3.312500 1.500000 -6.712797 193 205 214 +3.375000 1.500000 -6.712797 192 204 214 +3.437500 1.500000 -6.712797 192 204 213 +3.500000 1.500000 -6.712797 192 203 213 +3.562500 1.500000 -6.712797 192 203 212 +3.625000 1.500000 -6.712797 192 203 212 +3.687500 1.500000 -6.712797 190 203 211 +3.750000 1.500000 -6.712797 190 202 211 +3.812500 1.500000 -6.712797 190 202 211 +3.875000 1.500000 -6.712797 190 202 210 +3.937500 1.500000 -6.712797 190 202 210 +4.000000 1.500000 -6.712797 190 201 210 +4.062500 1.500000 -6.712797 189 200 209 +4.125000 1.500000 -6.712797 187 199 208 +4.187500 1.500000 -6.712797 187 199 208 +4.250000 1.500000 -6.712797 186 198 208 +4.312500 1.500000 -6.712797 186 198 207 +4.375000 1.500000 -6.712797 186 197 206 +4.437500 1.500000 -6.712797 185 197 206 +4.500000 1.500000 -6.712797 184 197 206 +4.562500 1.500000 -6.712797 184 196 205 +4.625000 1.500000 -6.712797 184 196 205 +4.687500 1.500000 -6.712797 183 195 204 +4.750000 1.500000 -6.712797 182 195 204 +4.812500 1.500000 -6.712797 182 194 203 +4.875000 1.500000 -6.712797 181 193 203 +4.937500 1.500000 -6.712797 180 192 202 +5.000000 1.500000 -6.712797 180 192 202 +5.062500 1.500000 -6.712797 179 191 201 +5.125000 1.500000 -6.712797 178 190 200 +5.187500 1.500000 -6.712797 178 190 200 +5.250000 1.500000 -6.712797 175 189 199 +5.312500 1.500000 -6.712797 175 188 198 +5.375000 1.500000 -6.712797 174 187 197 +5.437500 1.500000 -6.712797 173 186 197 +5.500000 1.500000 -6.712797 172 185 196 +5.562500 1.500000 -6.712797 170 184 196 +5.625000 1.500000 -6.712797 170 184 195 +5.687500 1.500000 -6.712797 168 182 194 +5.750000 1.500000 -6.712797 167 182 193 +5.812500 1.500000 -6.712797 167 181 192 +5.875000 1.500000 -6.712797 165 180 191 +5.937500 1.500000 -6.712797 164 180 191 +6.000000 1.500000 -6.712797 163 179 190 +6.062500 1.500000 -6.712797 163 178 190 +6.125000 1.500000 -6.712797 162 177 189 +6.187500 1.500000 -6.712797 161 176 188 +6.250000 1.500000 -6.712797 160 175 187 +6.312500 1.500000 -6.712797 160 175 186 +6.375000 1.500000 -6.712797 159 174 186 +6.437500 1.500000 -6.712797 158 173 184 +6.500000 1.500000 -6.712797 157 173 184 +6.562500 1.500000 -6.712797 156 172 183 +6.625000 1.500000 -6.712797 155 170 182 +6.687500 1.500000 -6.712797 155 170 181 +6.750000 1.500000 -6.712797 154 169 180 +6.812500 1.500000 -6.712797 153 168 180 +6.875000 1.500000 -6.712797 152 167 179 +6.937500 1.500000 -6.712797 151 167 178 +7.000000 1.500000 -6.712797 151 166 178 +7.062500 1.500000 -6.712797 149 164 176 +7.125000 1.500000 -6.712797 149 164 176 +7.187500 1.500000 -6.712797 148 163 175 +7.250000 1.500000 -6.712797 147 161 173 +7.312500 1.500000 -6.712797 146 161 173 +7.375000 1.500000 -6.712797 145 160 172 +7.437500 1.500000 -6.712797 144 159 171 +1.125000 1.437500 -6.712797 206 214 220 +1.187500 1.437500 -6.712797 206 214 220 +1.250000 1.437500 -6.712797 206 214 220 +1.312500 1.437500 -6.712797 206 213 220 +1.375000 1.437500 -6.712797 205 213 219 +1.437500 1.437500 -6.712797 205 213 219 +1.500000 1.437500 -6.712797 205 213 219 +1.562500 1.437500 -6.712797 204 213 219 +1.625000 1.437500 -6.712797 204 212 219 +1.687500 1.437500 -6.712797 203 212 219 +1.750000 1.437500 -6.712797 203 212 219 +1.812500 1.437500 -6.712797 202 211 219 +1.875000 1.437500 -6.712797 202 211 219 +1.937500 1.437500 -6.712797 201 211 219 +2.000000 1.437500 -6.712797 200 211 219 +2.062500 1.437500 -6.712797 200 210 219 +2.125000 1.437500 -6.712797 199 210 219 +2.187500 1.437500 -6.712797 199 210 219 +2.250000 1.437500 -6.712797 198 209 219 +2.437500 1.437500 -6.712797 199 209 218 +2.500000 1.437500 -6.712797 199 210 218 +2.562500 1.437500 -6.712797 199 209 218 +2.625000 1.437500 -6.712797 198 209 217 +2.687500 1.437500 -6.712797 198 209 217 +2.750000 1.437500 -6.712797 197 208 217 +2.812500 1.437500 -6.712797 197 208 217 +2.875000 1.437500 -6.712797 197 208 216 +2.937500 1.437500 -6.712797 196 207 216 +3.000000 1.437500 -6.712797 196 207 216 +3.062500 1.437500 -6.712797 196 206 215 +3.125000 1.437500 -6.712797 195 206 215 +3.187500 1.437500 -6.712797 195 206 215 +3.250000 1.437500 -6.712797 195 206 215 +3.312500 1.437500 -6.712797 194 205 214 +3.375000 1.437500 -6.712797 194 205 214 +3.437500 1.437500 -6.712797 193 205 213 +3.500000 1.437500 -6.712797 193 204 213 +3.562500 1.437500 -6.712797 193 204 213 +3.625000 1.437500 -6.712797 192 203 212 +3.687500 1.437500 -6.712797 192 203 211 +3.750000 1.437500 -6.712797 190 202 211 +3.812500 1.437500 -6.712797 191 203 211 +3.875000 1.437500 -6.712797 192 202 210 +3.937500 1.437500 -6.712797 191 202 210 +4.000000 1.437500 -6.712797 190 201 210 +4.062500 1.437500 -6.712797 189 201 209 +4.125000 1.437500 -6.712797 189 200 209 +4.187500 1.437500 -6.712797 188 200 208 +4.250000 1.437500 -6.712797 188 199 208 +4.312500 1.437500 -6.712797 187 199 208 +4.375000 1.437500 -6.712797 186 198 207 +4.437500 1.437500 -6.712797 186 198 206 +4.500000 1.437500 -6.712797 186 197 206 +4.562500 1.437500 -6.712797 185 197 206 +4.625000 1.437500 -6.712797 185 197 205 +4.687500 1.437500 -6.712797 184 196 205 +4.750000 1.437500 -6.712797 183 195 204 +4.812500 1.437500 -6.712797 183 195 203 +4.875000 1.437500 -6.712797 182 194 203 +4.937500 1.437500 -6.712797 181 193 202 +5.000000 1.437500 -6.712797 180 192 202 +5.062500 1.437500 -6.712797 179 191 201 +5.125000 1.437500 -6.712797 178 190 200 +5.187500 1.437500 -6.712797 178 190 200 +5.250000 1.437500 -6.712797 176 189 199 +5.312500 1.437500 -6.712797 175 189 199 +5.375000 1.437500 -6.712797 174 187 197 +5.437500 1.437500 -6.712797 173 186 197 +5.500000 1.437500 -6.712797 173 186 196 +5.562500 1.437500 -6.712797 172 185 196 +5.625000 1.437500 -6.712797 170 184 195 +5.687500 1.437500 -6.712797 170 183 194 +5.750000 1.437500 -6.712797 170 183 194 +5.812500 1.437500 -6.712797 169 182 193 +5.875000 1.437500 -6.712797 167 182 192 +5.937500 1.437500 -6.712797 167 180 191 +6.000000 1.437500 -6.712797 166 180 190 +6.062500 1.437500 -6.712797 165 179 190 +6.125000 1.437500 -6.712797 164 178 189 +6.187500 1.437500 -6.712797 164 178 189 +6.250000 1.437500 -6.712797 163 177 188 +6.312500 1.437500 -6.712797 162 176 186 +6.375000 1.437500 -6.712797 161 175 186 +6.437500 1.437500 -6.712797 160 175 185 +6.500000 1.437500 -6.712797 160 174 184 +6.562500 1.437500 -6.712797 159 173 184 +6.625000 1.437500 -6.712797 158 172 182 +6.687500 1.437500 -6.712797 158 171 182 +6.750000 1.437500 -6.712797 157 170 181 +6.812500 1.437500 -6.712797 155 169 180 +6.875000 1.437500 -6.712797 155 169 180 +6.937500 1.437500 -6.712797 154 167 178 +7.000000 1.437500 -6.712797 153 167 178 +7.062500 1.437500 -6.712797 152 166 177 +7.125000 1.437500 -6.712797 151 165 175 +7.187500 1.437500 -6.712797 150 164 175 +7.250000 1.437500 -6.712797 149 163 174 +7.312500 1.437500 -6.712797 149 163 173 +7.375000 1.437500 -6.712797 148 161 172 +7.437500 1.437500 -6.712797 147 160 171 +1.437500 1.375000 -6.712797 205 213 220 +1.500000 1.375000 -6.712797 205 213 220 +1.562500 1.375000 -6.712797 205 213 219 +1.625000 1.375000 -6.712797 204 213 219 +1.687500 1.375000 -6.712797 204 213 219 +1.750000 1.375000 -6.712797 203 212 219 +1.812500 1.375000 -6.712797 202 211 219 +1.875000 1.375000 -6.712797 202 211 219 +1.937500 1.375000 -6.712797 200 211 219 +2.000000 1.375000 -6.712797 200 210 219 +2.062500 1.375000 -6.712797 200 210 219 +2.125000 1.375000 -6.712797 200 211 219 +2.187500 1.375000 -6.712797 199 210 219 +2.250000 1.375000 -6.712797 199 210 219 +2.312500 1.375000 -6.712797 200 210 219 +2.375000 1.375000 -6.712797 200 210 218 +2.437500 1.375000 -6.712797 200 210 218 +2.500000 1.375000 -6.712797 200 210 218 +2.562500 1.375000 -6.712797 199 210 218 +2.625000 1.375000 -6.712797 199 209 217 +2.687500 1.375000 -6.712797 199 209 217 +2.750000 1.375000 -6.712797 198 209 217 +2.812500 1.375000 -6.712797 198 208 217 +2.875000 1.375000 -6.712797 197 208 217 +2.937500 1.375000 -6.712797 197 208 217 +3.000000 1.375000 -6.712797 197 208 216 +3.062500 1.375000 -6.712797 197 207 215 +3.125000 1.375000 -6.712797 196 207 215 +3.187500 1.375000 -6.712797 196 207 215 +3.250000 1.375000 -6.712797 196 206 215 +3.312500 1.375000 -6.712797 196 206 214 +3.375000 1.375000 -6.712797 195 206 214 +3.437500 1.375000 -6.712797 194 205 214 +3.500000 1.375000 -6.712797 194 205 213 +3.562500 1.375000 -6.712797 194 205 213 +3.625000 1.375000 -6.712797 194 204 213 +3.687500 1.375000 -6.712797 194 204 213 +3.750000 1.375000 -6.712797 192 203 211 +3.812500 1.375000 -6.712797 192 203 211 +3.875000 1.375000 -6.712797 192 203 211 +3.937500 1.375000 -6.712797 192 203 210 +4.000000 1.375000 -6.712797 192 202 210 +4.062500 1.375000 -6.712797 191 202 210 +4.125000 1.375000 -6.712797 190 202 209 +4.187500 1.375000 -6.712797 189 200 209 +4.250000 1.375000 -6.712797 189 200 208 +4.312500 1.375000 -6.712797 189 200 208 +4.375000 1.375000 -6.712797 188 199 207 +4.437500 1.375000 -6.712797 187 199 207 +4.500000 1.375000 -6.712797 187 198 206 +4.562500 1.375000 -6.712797 187 198 206 +4.625000 1.375000 -6.712797 187 197 206 +4.687500 1.375000 -6.712797 186 197 205 +4.750000 1.375000 -6.712797 185 196 205 +4.812500 1.375000 -6.712797 184 196 203 +4.875000 1.375000 -6.712797 184 195 203 +4.937500 1.375000 -6.712797 183 194 203 +5.000000 1.375000 -6.712797 182 193 202 +5.062500 1.375000 -6.712797 180 192 201 +5.125000 1.375000 -6.712797 180 192 201 +5.187500 1.375000 -6.712797 179 190 200 +5.250000 1.375000 -6.712797 178 190 199 +5.312500 1.375000 -6.712797 177 189 199 +5.375000 1.375000 -6.712797 176 189 198 +5.437500 1.375000 -6.712797 175 188 197 +5.500000 1.375000 -6.712797 175 186 196 +5.562500 1.375000 -6.712797 174 186 196 +5.625000 1.375000 -6.712797 173 186 196 +5.687500 1.375000 -6.712797 173 185 194 +5.750000 1.375000 -6.712797 172 184 194 +5.812500 1.375000 -6.712797 172 184 194 +5.875000 1.375000 -6.712797 171 183 192 +5.937500 1.375000 -6.712797 170 182 192 +6.000000 1.375000 -6.712797 169 181 191 +6.062500 1.375000 -6.712797 168 180 190 +6.125000 1.375000 -6.712797 167 180 189 +6.187500 1.375000 -6.712797 167 179 189 +6.250000 1.375000 -6.712797 166 178 188 +6.312500 1.375000 -6.712797 165 178 187 +6.375000 1.375000 -6.712797 163 176 186 +6.437500 1.375000 -6.712797 163 175 185 +6.500000 1.375000 -6.712797 163 175 184 +6.562500 1.375000 -6.712797 162 174 184 +6.625000 1.375000 -6.712797 161 173 183 +6.687500 1.375000 -6.712797 160 173 182 +6.750000 1.375000 -6.712797 160 172 181 +6.812500 1.375000 -6.712797 159 171 180 +6.875000 1.375000 -6.712797 158 170 180 +6.937500 1.375000 -6.712797 157 169 179 +7.000000 1.375000 -6.712797 156 168 178 +7.062500 1.375000 -6.712797 156 167 177 +7.125000 1.375000 -6.712797 155 167 175 +7.187500 1.375000 -6.712797 154 166 175 +7.250000 1.375000 -6.712797 152 164 174 +7.312500 1.375000 -6.712797 151 163 173 +7.375000 1.375000 -6.712797 150 163 172 +1.687500 1.312500 -6.712797 202 212 219 +1.750000 1.312500 -6.712797 202 212 220 +1.812500 1.312500 -6.712797 201 211 219 +1.875000 1.312500 -6.712797 200 211 219 +1.937500 1.312500 -6.712797 200 211 219 +2.000000 1.312500 -6.712797 200 211 220 +2.062500 1.312500 -6.712797 200 211 220 +2.125000 1.312500 -6.712797 200 211 219 +2.187500 1.312500 -6.712797 200 211 219 +2.250000 1.312500 -6.712797 201 211 219 +2.312500 1.312500 -6.712797 201 211 219 +2.375000 1.312500 -6.712797 202 211 219 +2.437500 1.312500 -6.712797 200 210 218 +2.500000 1.312500 -6.712797 200 210 218 +2.562500 1.312500 -6.712797 199 210 218 +2.625000 1.312500 -6.712797 200 210 218 +2.687500 1.312500 -6.712797 199 209 218 +2.750000 1.312500 -6.712797 199 210 218 +2.812500 1.312500 -6.712797 199 209 217 +2.875000 1.312500 -6.712797 199 209 217 +2.937500 1.312500 -6.712797 198 209 217 +3.000000 1.312500 -6.712797 198 208 217 +3.062500 1.312500 -6.712797 198 208 216 +3.125000 1.312500 -6.712797 197 208 216 +3.187500 1.312500 -6.712797 197 208 215 +3.250000 1.312500 -6.712797 197 207 215 +3.312500 1.312500 -6.712797 197 207 215 +3.375000 1.312500 -6.712797 196 206 214 +3.437500 1.312500 -6.712797 196 206 214 +3.500000 1.312500 -6.712797 195 205 213 +3.562500 1.312500 -6.712797 194 205 213 +3.625000 1.312500 -6.712797 194 205 213 +3.687500 1.312500 -6.712797 194 204 212 +3.750000 1.312500 -6.712797 194 205 212 +3.812500 1.312500 -6.712797 194 205 212 +3.875000 1.312500 -6.712797 195 205 211 +3.937500 1.312500 -6.712797 194 204 211 +4.000000 1.312500 -6.712797 194 203 211 +4.062500 1.312500 -6.712797 193 203 210 +4.125000 1.312500 -6.712797 192 202 210 +4.187500 1.312500 -6.712797 192 202 209 +4.250000 1.312500 -6.712797 191 202 209 +4.312500 1.312500 -6.712797 191 201 208 +4.375000 1.312500 -6.712797 191 201 208 +4.437500 1.312500 -6.712797 190 200 207 +4.500000 1.312500 -6.712797 190 200 207 +4.562500 1.312500 -6.712797 190 199 206 +4.625000 1.312500 -6.712797 189 199 206 +4.687500 1.312500 -6.712797 189 198 205 +4.750000 1.312500 -6.712797 188 197 205 +4.812500 1.312500 -6.712797 187 197 204 +4.875000 1.312500 -6.712797 186 196 203 +4.937500 1.312500 -6.712797 185 196 203 +5.000000 1.312500 -6.712797 184 194 202 +5.062500 1.312500 -6.712797 184 194 202 +5.125000 1.312500 -6.712797 182 193 201 +5.187500 1.312500 -6.712797 182 192 200 +5.250000 1.312500 -6.712797 181 191 200 +5.312500 1.312500 -6.712797 180 191 199 +5.375000 1.312500 -6.712797 179 190 198 +5.437500 1.312500 -6.712797 179 190 198 +5.500000 1.312500 -6.712797 178 189 197 +5.562500 1.312500 -6.712797 178 189 197 +5.625000 1.312500 -6.712797 176 187 196 +5.687500 1.312500 -6.712797 176 186 195 +5.750000 1.312500 -6.712797 175 186 194 +5.812500 1.312500 -6.712797 174 185 194 +5.875000 1.312500 -6.712797 173 184 193 +5.937500 1.312500 -6.712797 172 183 192 +6.000000 1.312500 -6.712797 172 183 192 +6.062500 1.312500 -6.712797 170 182 190 +6.125000 1.312500 -6.712797 170 180 189 +6.187500 1.312500 -6.712797 168 180 189 +6.250000 1.312500 -6.712797 168 180 189 +6.312500 1.312500 -6.712797 167 178 187 +6.375000 1.312500 -6.712797 166 178 186 +6.437500 1.312500 -6.712797 165 177 186 +6.500000 1.312500 -6.712797 165 176 185 +6.562500 1.312500 -6.712797 163 175 184 +6.625000 1.312500 -6.712797 163 175 183 +6.687500 1.312500 -6.712797 162 174 182 +6.750000 1.312500 -6.712797 161 173 182 +6.812500 1.312500 -6.712797 160 172 181 +6.875000 1.312500 -6.712797 159 170 180 +6.937500 1.312500 -6.712797 158 170 178 +7.000000 1.312500 -6.712797 157 169 178 +7.062500 1.312500 -6.712797 155 167 177 +7.125000 1.312500 -6.712797 154 167 176 +7.187500 1.312500 -6.712797 152 165 175 +7.250000 1.312500 -6.712797 150 163 174 +7.312500 1.312500 -6.712797 149 163 173 +7.375000 1.312500 -6.712797 148 161 172 +1.875000 1.250000 -6.712797 202 212 220 +1.937500 1.250000 -6.712797 201 211 220 +2.000000 1.250000 -6.712797 201 211 219 +2.062500 1.250000 -6.712797 202 212 220 +2.125000 1.250000 -6.712797 202 211 219 +2.187500 1.250000 -6.712797 202 212 219 +2.250000 1.250000 -6.712797 203 213 219 +2.312500 1.250000 -6.712797 202 212 219 +2.375000 1.250000 -6.712797 202 211 219 +2.437500 1.250000 -6.712797 201 211 219 +2.500000 1.250000 -6.712797 200 210 218 +2.562500 1.250000 -6.712797 200 210 218 +2.625000 1.250000 -6.712797 200 210 218 +2.687500 1.250000 -6.712797 200 210 218 +2.750000 1.250000 -6.712797 200 210 218 +2.812500 1.250000 -6.712797 200 210 218 +2.875000 1.250000 -6.712797 200 210 217 +2.937500 1.250000 -6.712797 199 209 217 +3.000000 1.250000 -6.712797 199 209 217 +3.062500 1.250000 -6.712797 199 208 216 +3.125000 1.250000 -6.712797 198 208 215 +3.187500 1.250000 -6.712797 199 208 216 +3.250000 1.250000 -6.712797 198 208 215 +3.312500 1.250000 -6.712797 197 207 215 +3.375000 1.250000 -6.712797 197 207 214 +3.437500 1.250000 -6.712797 197 206 214 +3.500000 1.250000 -6.712797 196 206 214 +3.562500 1.250000 -6.712797 197 206 214 +3.625000 1.250000 -6.712797 197 206 213 +3.687500 1.250000 -6.712797 197 206 213 +3.750000 1.250000 -6.712797 197 206 213 +3.812500 1.250000 -6.712797 196 205 212 +3.875000 1.250000 -6.712797 197 205 212 +3.937500 1.250000 -6.712797 197 205 211 +4.000000 1.250000 -6.712797 196 205 211 +4.062500 1.250000 -6.712797 196 204 210 +4.125000 1.250000 -6.712797 196 204 210 +4.187500 1.250000 -6.712797 195 203 210 +4.250000 1.250000 -6.712797 194 203 209 +4.312500 1.250000 -6.712797 193 202 208 +4.375000 1.250000 -6.712797 192 202 208 +4.437500 1.250000 -6.712797 192 201 208 +4.500000 1.250000 -6.712797 191 200 207 +4.562500 1.250000 -6.712797 191 200 206 +4.625000 1.250000 -6.712797 190 199 206 +4.687500 1.250000 -6.712797 190 199 206 +4.750000 1.250000 -6.712797 190 199 205 +4.812500 1.250000 -6.712797 189 198 204 +4.875000 1.250000 -6.712797 188 197 204 +4.937500 1.250000 -6.712797 186 196 203 +5.000000 1.250000 -6.712797 185 195 202 +5.062500 1.250000 -6.712797 185 195 202 +5.125000 1.250000 -6.712797 183 194 201 +5.187500 1.250000 -6.712797 182 193 200 +5.250000 1.250000 -6.712797 181 192 200 +5.312500 1.250000 -6.712797 180 191 199 +5.375000 1.250000 -6.712797 180 190 199 +5.437500 1.250000 -6.712797 178 190 198 +5.500000 1.250000 -6.712797 178 189 197 +5.562500 1.250000 -6.712797 178 189 197 +5.625000 1.250000 -6.712797 177 188 196 +5.687500 1.250000 -6.712797 177 187 196 +5.750000 1.250000 -6.712797 176 186 195 +5.812500 1.250000 -6.712797 175 185 194 +5.875000 1.250000 -6.712797 174 185 193 +5.937500 1.250000 -6.712797 173 184 192 +6.000000 1.250000 -6.712797 172 183 191 +6.062500 1.250000 -6.712797 172 182 191 +6.125000 1.250000 -6.712797 171 182 190 +6.187500 1.250000 -6.712797 170 181 189 +6.250000 1.250000 -6.712797 168 180 188 +6.312500 1.250000 -6.712797 168 180 188 +6.375000 1.250000 -6.712797 168 179 187 +6.437500 1.250000 -6.712797 167 178 186 +6.500000 1.250000 -6.712797 167 177 185 +6.562500 1.250000 -6.712797 166 176 184 +6.625000 1.250000 -6.712797 163 175 183 +6.687500 1.250000 -6.712797 162 174 182 +6.750000 1.250000 -6.712797 161 173 182 +6.812500 1.250000 -6.712797 160 172 181 +6.875000 1.250000 -6.712797 158 171 180 +6.937500 1.250000 -6.712797 156 169 178 +7.000000 1.250000 -6.712797 155 168 178 +7.062500 1.250000 -6.712797 155 167 178 +7.125000 1.250000 -6.712797 152 166 175 +7.187500 1.250000 -6.712797 151 165 175 +7.250000 1.250000 -6.712797 150 164 175 +7.312500 1.250000 -6.712797 148 163 173 +2.000000 1.187500 -6.712797 203 213 220 +2.062500 1.187500 -6.712797 203 213 219 +2.125000 1.187500 -6.712797 204 213 220 +2.187500 1.187500 -6.712797 205 213 219 +2.250000 1.187500 -6.712797 203 213 219 +2.312500 1.187500 -6.712797 203 213 219 +2.375000 1.187500 -6.712797 203 212 219 +2.437500 1.187500 -6.712797 202 211 219 +2.500000 1.187500 -6.712797 202 211 219 +2.562500 1.187500 -6.712797 202 211 219 +2.625000 1.187500 -6.712797 202 211 218 +2.687500 1.187500 -6.712797 202 211 218 +2.750000 1.187500 -6.712797 202 211 218 +2.812500 1.187500 -6.712797 202 211 218 +2.875000 1.187500 -6.712797 202 210 218 +2.937500 1.187500 -6.712797 202 210 217 +3.000000 1.187500 -6.712797 202 210 217 +3.062500 1.187500 -6.712797 201 210 217 +3.125000 1.187500 -6.712797 200 209 216 +3.187500 1.187500 -6.712797 200 209 215 +3.250000 1.187500 -6.712797 200 209 215 +3.312500 1.187500 -6.712797 199 208 215 +3.375000 1.187500 -6.712797 199 208 215 +3.437500 1.187500 -6.712797 198 208 214 +3.500000 1.187500 -6.712797 199 208 214 +3.562500 1.187500 -6.712797 199 208 214 +3.625000 1.187500 -6.712797 199 208 214 +3.687500 1.187500 -6.712797 199 207 213 +3.750000 1.187500 -6.712797 199 206 213 +3.812500 1.187500 -6.712797 199 206 213 +3.875000 1.187500 -6.712797 199 207 213 +3.937500 1.187500 -6.712797 198 206 212 +4.000000 1.187500 -6.712797 197 206 211 +4.062500 1.187500 -6.712797 196 205 211 +4.125000 1.187500 -6.712797 196 204 210 +4.187500 1.187500 -6.712797 195 203 210 +4.250000 1.187500 -6.712797 194 203 209 +4.312500 1.187500 -6.712797 193 202 209 +4.375000 1.187500 -6.712797 193 202 208 +4.437500 1.187500 -6.712797 192 202 208 +4.500000 1.187500 -6.712797 192 201 207 +4.562500 1.187500 -6.712797 192 200 207 +4.625000 1.187500 -6.712797 191 200 206 +4.687500 1.187500 -6.712797 190 199 206 +4.750000 1.187500 -6.712797 190 199 205 +4.812500 1.187500 -6.712797 189 197 204 +4.875000 1.187500 -6.712797 188 197 204 +4.937500 1.187500 -6.712797 186 196 203 +5.000000 1.187500 -6.712797 184 195 202 +5.062500 1.187500 -6.712797 184 194 202 +5.125000 1.187500 -6.712797 182 193 201 +5.187500 1.187500 -6.712797 182 193 201 +5.250000 1.187500 -6.712797 180 192 200 +5.312500 1.187500 -6.712797 180 191 199 +5.375000 1.187500 -6.712797 179 190 199 +5.437500 1.187500 -6.712797 178 190 198 +5.500000 1.187500 -6.712797 178 189 197 +5.562500 1.187500 -6.712797 178 189 197 +5.625000 1.187500 -6.712797 177 188 196 +5.687500 1.187500 -6.712797 176 187 196 +5.750000 1.187500 -6.712797 175 186 194 +5.812500 1.187500 -6.712797 175 186 194 +5.875000 1.187500 -6.712797 174 185 193 +5.937500 1.187500 -6.712797 173 184 192 +6.000000 1.187500 -6.712797 173 184 192 +6.062500 1.187500 -6.712797 173 184 192 +6.125000 1.187500 -6.712797 173 183 191 +6.187500 1.187500 -6.712797 172 182 190 +6.250000 1.187500 -6.712797 170 181 189 +6.312500 1.187500 -6.712797 170 180 189 +6.375000 1.187500 -6.712797 170 180 188 +6.437500 1.187500 -6.712797 169 179 186 +6.500000 1.187500 -6.712797 168 178 186 +6.562500 1.187500 -6.712797 167 178 185 +6.625000 1.187500 -6.712797 164 176 184 +6.687500 1.187500 -6.712797 163 175 183 +6.750000 1.187500 -6.712797 160 173 182 +6.812500 1.187500 -6.712797 160 173 181 +6.875000 1.187500 -6.712797 159 171 180 +6.937500 1.187500 -6.712797 157 170 179 +7.000000 1.187500 -6.712797 157 170 178 +7.062500 1.187500 -6.712797 156 169 178 +7.125000 1.187500 -6.712797 155 167 177 +2.062500 1.125000 -6.712797 207 214 220 +2.125000 1.125000 -6.712797 207 214 220 +2.187500 1.125000 -6.712797 206 214 220 +2.250000 1.125000 -6.712797 206 214 220 +2.312500 1.125000 -6.712797 206 214 220 +2.375000 1.125000 -6.712797 205 213 219 +2.437500 1.125000 -6.712797 205 213 219 +2.500000 1.125000 -6.712797 204 213 219 +2.562500 1.125000 -6.712797 204 213 219 +2.625000 1.125000 -6.712797 204 213 219 +2.687500 1.125000 -6.712797 203 212 218 +2.750000 1.125000 -6.712797 203 211 218 +2.812500 1.125000 -6.712797 203 211 218 +2.875000 1.125000 -6.712797 203 211 218 +2.937500 1.125000 -6.712797 202 211 217 +3.000000 1.125000 -6.712797 202 211 217 +3.062500 1.125000 -6.712797 202 210 217 +3.125000 1.125000 -6.712797 202 210 217 +3.187500 1.125000 -6.712797 202 210 216 +3.250000 1.125000 -6.712797 202 210 215 +3.312500 1.125000 -6.712797 202 210 215 +3.375000 1.125000 -6.712797 202 210 215 +3.437500 1.125000 -6.712797 202 209 215 +3.500000 1.125000 -6.712797 201 209 215 +3.562500 1.125000 -6.712797 201 209 214 +3.625000 1.125000 -6.712797 200 208 214 +3.687500 1.125000 -6.712797 199 208 214 +3.750000 1.125000 -6.712797 200 208 213 +3.812500 1.125000 -6.712797 200 207 213 +3.875000 1.125000 -6.712797 199 207 213 +3.937500 1.125000 -6.712797 198 206 212 +4.000000 1.125000 -6.712797 197 205 211 +4.062500 1.125000 -6.712797 196 204 210 +4.125000 1.125000 -6.712797 196 205 210 +4.187500 1.125000 -6.712797 196 204 210 +4.250000 1.125000 -6.712797 195 203 210 +4.312500 1.125000 -6.712797 194 203 209 +4.375000 1.125000 -6.712797 194 202 209 +4.437500 1.125000 -6.712797 193 202 208 +4.500000 1.125000 -6.712797 192 202 208 +4.562500 1.125000 -6.712797 191 200 206 +4.625000 1.125000 -6.712797 190 200 206 +4.687500 1.125000 -6.712797 189 199 205 +4.750000 1.125000 -6.712797 189 199 205 +4.812500 1.125000 -6.712797 188 198 205 +4.875000 1.125000 -6.712797 187 197 204 +5.125000 1.125000 -6.712797 184 194 202 +5.187500 1.125000 -6.712797 182 193 201 +5.250000 1.125000 -6.712797 182 192 200 +5.312500 1.125000 -6.712797 180 192 199 +5.375000 1.125000 -6.712797 180 191 199 +5.437500 1.125000 -6.712797 180 191 199 +5.500000 1.125000 -6.712797 180 190 197 +5.562500 1.125000 -6.712797 180 190 197 +5.625000 1.125000 -6.712797 179 189 196 +5.687500 1.125000 -6.712797 179 189 196 +5.750000 1.125000 -6.712797 179 189 195 +5.812500 1.125000 -6.712797 178 188 194 +5.875000 1.125000 -6.712797 178 186 193 +5.937500 1.125000 -6.712797 177 186 192 +6.000000 1.125000 -6.712797 176 185 192 +6.062500 1.125000 -6.712797 176 185 192 +6.125000 1.125000 -6.712797 175 184 190 +6.187500 1.125000 -6.712797 174 183 190 +6.250000 1.125000 -6.712797 174 182 189 +6.312500 1.125000 -6.712797 174 182 189 +6.375000 1.125000 -6.712797 174 182 189 +6.437500 1.125000 -6.712797 173 181 187 +6.500000 1.125000 -6.712797 173 180 186 +6.562500 1.125000 -6.712797 172 180 186 +6.625000 1.125000 -6.712797 170 178 184 +6.687500 1.125000 -6.712797 168 177 183 +6.750000 1.125000 -6.712797 167 176 182 +6.812500 1.125000 -6.712797 167 175 181 +2.187500 1.062500 -6.712797 208 215 220 +2.250000 1.062500 -6.712797 208 215 220 +2.312500 1.062500 -6.712797 206 214 220 +2.375000 1.062500 -6.712797 205 214 220 +2.437500 1.062500 -6.712797 205 213 219 +2.500000 1.062500 -6.712797 205 213 219 +2.562500 1.062500 -6.712797 205 213 219 +2.625000 1.062500 -6.712797 205 213 219 +2.687500 1.062500 -6.712797 204 213 218 +2.750000 1.062500 -6.712797 204 213 218 +2.812500 1.062500 -6.712797 204 212 218 +2.875000 1.062500 -6.712797 204 211 218 +2.937500 1.062500 -6.712797 204 211 217 +3.000000 1.062500 -6.712797 204 211 217 +3.062500 1.062500 -6.712797 203 211 217 +3.125000 1.062500 -6.712797 203 211 217 +3.187500 1.062500 -6.712797 203 210 216 +3.250000 1.062500 -6.712797 203 210 216 +3.312500 1.062500 -6.712797 203 210 216 +3.375000 1.062500 -6.712797 203 210 216 +3.437500 1.062500 -6.712797 202 210 215 +3.500000 1.062500 -6.712797 202 209 215 +3.562500 1.062500 -6.712797 201 209 214 +3.625000 1.062500 -6.712797 201 209 214 +3.687500 1.062500 -6.712797 201 209 214 +3.750000 1.062500 -6.712797 201 208 214 +3.812500 1.062500 -6.712797 200 208 213 +3.875000 1.062500 -6.712797 199 207 213 +3.937500 1.062500 -6.712797 199 206 212 +4.000000 1.062500 -6.712797 198 206 211 +4.062500 1.062500 -6.712797 197 205 211 +4.125000 1.062500 -6.712797 198 206 211 +4.187500 1.062500 -6.712797 196 204 210 +4.250000 1.062500 -6.712797 196 204 209 +4.312500 1.062500 -6.712797 196 204 209 +4.375000 1.062500 -6.712797 195 203 209 +4.437500 1.062500 -6.712797 194 202 208 +4.500000 1.062500 -6.712797 194 202 207 +4.562500 1.062500 -6.712797 193 202 207 +4.625000 1.062500 -6.712797 192 201 206 +4.687500 1.062500 -6.712797 192 200 206 +4.750000 1.062500 -6.712797 192 200 205 +5.312500 1.062500 -6.712797 186 194 200 +5.375000 1.062500 -6.712797 186 194 200 +5.437500 1.062500 -6.712797 186 193 199 +5.500000 1.062500 -6.712797 185 192 198 +5.562500 1.062500 -6.712797 185 192 197 +5.625000 1.062500 -6.712797 185 192 197 +5.687500 1.062500 -6.712797 184 191 196 +5.750000 1.062500 -6.712797 184 191 196 +5.812500 1.062500 -6.712797 184 190 196 +5.875000 1.062500 -6.712797 183 189 194 +5.937500 1.062500 -6.712797 182 189 194 +6.000000 1.062500 -6.712797 181 187 192 +6.062500 1.062500 -6.712797 180 186 192 +6.125000 1.062500 -6.712797 180 186 191 +6.187500 1.062500 -6.712797 180 186 190 +6.250000 1.062500 -6.712797 179 185 190 +6.312500 1.062500 -6.712797 178 184 189 +6.375000 1.062500 -6.712797 178 184 189 +6.437500 1.062500 -6.712797 177 182 187 +6.500000 1.062500 -6.712797 176 182 187 +6.562500 1.062500 -6.712797 175 181 186 +2.312500 1.000000 -6.712797 208 215 220 +2.375000 1.000000 -6.712797 207 214 220 +2.437500 1.000000 -6.712797 208 215 219 +2.500000 1.000000 -6.712797 209 215 220 +2.562500 1.000000 -6.712797 209 215 219 +2.625000 1.000000 -6.712797 208 214 219 +2.687500 1.000000 -6.712797 208 214 219 +2.750000 1.000000 -6.712797 208 214 218 +2.812500 1.000000 -6.712797 208 214 218 +2.875000 1.000000 -6.712797 208 213 218 +2.937500 1.000000 -6.712797 208 214 218 +3.000000 1.000000 -6.712797 207 213 217 +3.062500 1.000000 -6.712797 206 213 217 +3.125000 1.000000 -6.712797 206 213 217 +3.187500 1.000000 -6.712797 206 211 217 +3.250000 1.000000 -6.712797 205 211 217 +3.312500 1.000000 -6.712797 206 211 217 +3.375000 1.000000 -6.712797 205 211 216 +3.437500 1.000000 -6.712797 205 211 215 +3.500000 1.000000 -6.712797 203 210 215 +3.562500 1.000000 -6.712797 203 210 215 +3.625000 1.000000 -6.712797 202 209 214 +3.687500 1.000000 -6.712797 203 209 214 +3.750000 1.000000 -6.712797 203 209 214 +3.812500 1.000000 -6.712797 202 209 213 +3.875000 1.000000 -6.712797 202 208 213 +3.937500 1.000000 -6.712797 201 208 212 +4.000000 1.000000 -6.712797 200 207 211 +4.062500 1.000000 -6.712797 199 206 211 +4.125000 1.000000 -6.712797 199 206 211 +4.187500 1.000000 -6.712797 199 205 210 +4.250000 1.000000 -6.712797 199 205 210 +4.312500 1.000000 -6.712797 199 205 209 +4.375000 1.000000 -6.712797 198 204 209 +4.437500 1.000000 -6.712797 197 203 208 +4.500000 1.000000 -6.712797 197 203 208 +4.562500 1.000000 -6.712797 197 203 207 +4.625000 1.000000 -6.712797 196 202 206 +4.687500 1.000000 -6.712797 196 202 206 +4.750000 1.000000 -6.712797 196 202 206 +5.375000 1.000000 -6.712797 190 196 200 +5.437500 1.000000 -6.712797 188 194 199 +5.500000 1.000000 -6.712797 187 194 199 +5.562500 1.000000 -6.712797 187 193 198 +5.625000 1.000000 -6.712797 185 192 197 +5.687500 1.000000 -6.712797 184 192 197 +5.750000 1.000000 -6.712797 184 191 196 +5.812500 1.000000 -6.712797 182 190 195 +5.875000 1.000000 -6.712797 182 189 194 +5.937500 1.000000 -6.712797 180 188 194 +6.000000 1.000000 -6.712797 180 187 193 +6.062500 1.000000 -6.712797 178 186 192 +6.125000 1.000000 -6.712797 178 185 191 +6.187500 1.000000 -6.712797 178 185 190 +6.250000 1.000000 -6.712797 178 185 190 +2.500000 0.937500 -6.712797 211 216 219 +2.562500 0.937500 -6.712797 211 216 219 +2.625000 0.937500 -6.712797 211 216 219 +2.687500 0.937500 -6.712797 211 216 219 +2.750000 0.937500 -6.712797 210 215 219 +2.812500 0.937500 -6.712797 210 215 218 +2.875000 0.937500 -6.712797 210 215 218 +2.937500 0.937500 -6.712797 210 215 218 +3.000000 0.937500 -6.712797 210 214 218 +3.062500 0.937500 -6.712797 209 214 217 +3.125000 0.937500 -6.712797 209 214 217 +3.187500 0.937500 -6.712797 207 213 217 +3.250000 0.937500 -6.712797 206 212 217 +3.312500 0.937500 -6.712797 205 211 216 +3.375000 0.937500 -6.712797 204 211 216 +3.437500 0.937500 -6.712797 204 211 215 +3.500000 0.937500 -6.712797 203 210 215 +3.562500 0.937500 -6.712797 203 210 215 +3.625000 0.937500 -6.712797 202 209 214 +3.687500 0.937500 -6.712797 203 209 214 +3.750000 0.937500 -6.712797 203 209 213 +3.812500 0.937500 -6.712797 202 209 213 +3.875000 0.937500 -6.712797 202 208 213 +3.937500 0.937500 -6.712797 201 208 212 +4.000000 0.937500 -6.712797 200 207 212 +4.062500 0.937500 -6.712797 199 206 211 +4.125000 0.937500 -6.712797 199 205 210 +4.187500 0.937500 -6.712797 199 206 210 +4.250000 0.937500 -6.712797 199 205 210 +4.312500 0.937500 -6.712797 198 205 209 +4.375000 0.937500 -6.712797 197 204 209 +4.437500 0.937500 -6.712797 197 203 208 +4.500000 0.937500 -6.712797 196 203 208 +4.562500 0.937500 -6.712797 195 202 207 +4.625000 0.937500 -6.712797 195 202 206 +4.687500 0.937500 -6.712797 194 202 206 +5.625000 0.937500 -6.712797 181 190 197 +5.687500 0.937500 -6.712797 180 190 196 +5.750000 0.937500 -6.712797 180 189 196 +5.812500 0.937500 -6.712797 180 189 195 +5.875000 0.937500 -6.712797 180 189 194 +5.937500 0.937500 -6.712797 180 188 194 +6.000000 0.937500 -6.712797 180 187 192 +6.062500 0.937500 -6.712797 179 186 192 +6.125000 0.937500 -6.712797 179 186 192 +2.750000 0.875000 -6.712797 211 216 219 +2.812500 0.875000 -6.712797 211 216 219 +2.875000 0.875000 -6.712797 210 215 218 +2.937500 0.875000 -6.712797 210 215 218 +3.000000 0.875000 -6.712797 209 214 218 +3.062500 0.875000 -6.712797 209 214 218 +3.125000 0.875000 -6.712797 208 213 217 +3.187500 0.875000 -6.712797 206 213 217 +3.250000 0.875000 -6.712797 205 212 217 +3.312500 0.875000 -6.712797 205 211 217 +3.375000 0.875000 -6.712797 204 211 215 +3.437500 0.875000 -6.712797 204 210 215 +3.500000 0.875000 -6.712797 203 210 215 +3.562500 0.875000 -6.712797 203 210 215 +3.625000 0.875000 -6.712797 202 209 214 +3.687500 0.875000 -6.712797 202 209 214 +3.750000 0.875000 -6.712797 201 208 213 +3.812500 0.875000 -6.712797 201 208 213 +3.875000 0.875000 -6.712797 200 208 213 +3.937500 0.875000 -6.712797 200 207 212 +4.000000 0.875000 -6.712797 199 206 211 +4.062500 0.875000 -6.712797 199 206 211 +4.125000 0.875000 -6.712797 198 206 210 +4.187500 0.875000 -6.712797 199 206 210 +4.250000 0.875000 -6.712797 198 205 210 +3.065263 0.471579 -3.617844 184 192 196 +3.000000 0.812500 -6.712797 207 213 218 +3.062500 0.812500 -6.712797 205 213 217 +3.125000 0.812500 -6.712797 205 212 217 +3.187500 0.812500 -6.712797 204 211 217 +3.250000 0.812500 -6.712797 203 211 216 +3.312500 0.812500 -6.712797 203 211 216 +3.375000 0.812500 -6.712797 203 211 216 +3.437500 0.812500 -6.712797 202 210 215 +3.500000 0.812500 -6.712797 202 210 215 +3.562500 0.812500 -6.712797 201 209 214 +3.625000 0.812500 -6.712797 201 209 214 +3.687500 0.812500 -6.712797 200 208 213 +3.750000 0.812500 -6.712797 200 208 213 +3.812500 0.812500 -6.712797 200 208 213 +3.875000 0.812500 -6.712797 200 208 213 +3.065263 0.437895 -3.617844 191 195 197 +3.125000 0.750000 -6.712797 204 212 217 +3.187500 0.750000 -6.712797 204 211 217 +3.250000 0.750000 -6.712797 203 211 216 +3.312500 0.750000 -6.712797 203 211 216 +3.375000 0.750000 -6.712797 203 211 216 +3.437500 0.750000 -6.712797 202 210 215 +3.500000 0.750000 -6.712797 202 210 215 +3.562500 0.750000 -6.712797 201 209 214 +3.625000 0.750000 -6.712797 200 209 214 +0.422688 -0.012432 -1.335257 138 129 113 +0.434783 -0.012422 -1.334221 144 134 117 +0.446512 -0.012403 -1.332152 141 131 116 +0.457850 -0.012374 -1.329061 142 132 116 +0.469498 -0.012355 -1.327009 139 131 116 +0.446259 -0.010884 -1.169031 136 128 114 +0.485502 -0.010789 -1.158784 143 133 117 +0.489362 -0.010638 -1.142604 148 136 120 +0.498013 -0.010596 -1.138064 146 134 119 +0.422688 -0.024864 -1.335257 172 160 145 +0.434109 -0.024806 -1.332152 173 162 145 +0.446166 -0.024787 -1.331120 165 153 137 +0.457143 -0.024710 -1.327009 170 158 142 +0.469498 -0.024710 -1.327009 176 164 148 +0.481481 -0.024691 -1.325985 176 165 148 +0.435374 -0.021769 -1.169031 179 167 151 +0.444746 -0.021695 -1.165069 181 169 153 +0.454976 -0.021666 -1.163491 185 173 156 +0.464551 -0.021607 -1.160348 187 174 157 +0.469960 -0.021362 -1.147180 191 177 161 +0.478723 -0.021277 -1.142604 191 177 161 +0.487417 -0.021192 -1.138064 189 176 159 +0.495063 -0.021066 -1.131321 187 174 158 +0.505263 -0.021053 -1.130576 186 173 157 +0.515112 -0.021025 -1.129091 184 172 155 +0.575540 -0.020929 -1.123922 179 166 149 +0.584856 -0.020888 -1.121721 178 165 147 +0.435120 -0.037296 -1.335257 200 189 172 +0.446166 -0.037180 -1.331120 191 179 163 +0.457143 -0.037066 -1.327009 195 183 167 +0.469498 -0.037066 -1.327009 197 185 168 +0.481481 -0.037037 -1.325985 199 187 171 +0.435374 -0.032653 -1.169031 199 186 170 +0.444144 -0.032498 -1.163491 198 185 170 +0.453441 -0.032389 -1.159565 197 184 167 +0.461745 -0.032215 -1.153340 196 183 167 +0.469333 -0.032000 -1.145651 196 183 167 +0.478405 -0.031894 -1.141845 195 182 164 +0.487417 -0.031788 -1.138064 194 181 164 +0.495063 -0.031600 -1.131321 193 180 163 +0.503937 -0.031496 -1.127609 192 179 161 +0.514436 -0.031496 -1.127609 190 177 160 +0.525624 -0.031537 -1.129091 190 176 159 +0.565075 -0.031393 -1.123922 186 173 155 +0.573664 -0.031291 -1.120258 186 173 155 +0.583333 -0.031250 -1.118800 186 172 155 +0.593750 -0.031250 -1.118800 186 172 154 +0.457143 -0.049421 -1.327009 205 195 178 +0.469498 -0.049421 -1.327009 203 193 176 +0.481853 -0.049421 -1.327009 202 191 174 +0.493827 -0.049383 -1.325985 202 190 173 +0.442645 -0.043185 -1.159565 200 189 172 +0.451007 -0.042953 -1.153340 200 188 170 +0.458973 -0.042695 -1.146415 199 186 170 +0.468708 -0.042610 -1.144125 198 185 168 +0.478405 -0.042525 -1.141845 197 184 167 +0.487740 -0.042412 -1.138818 196 183 167 +0.495715 -0.042189 -1.132812 196 182 166 +0.503937 -0.041995 -1.127609 194 182 164 +0.514436 -0.041995 -1.127609 194 181 163 +0.525624 -0.042050 -1.129091 194 181 164 +0.536137 -0.042050 -1.129091 194 181 164 +0.563601 -0.041748 -1.120989 192 178 161 +0.572917 -0.041667 -1.118800 189 176 159 +0.581064 -0.041505 -1.114446 188 174 156 +0.591440 -0.041505 -1.114446 186 173 155 +0.459706 -0.056062 -1.204258 197 184 166 +0.451007 -0.053691 -1.153340 196 183 165 +0.458667 -0.053333 -1.145651 195 182 164 +0.468708 -0.053262 -1.144125 196 183 165 +0.478405 -0.053156 -1.141845 195 182 164 +0.487740 -0.053015 -1.138818 195 183 165 +0.495715 -0.052736 -1.132812 196 183 165 +0.504268 -0.052528 -1.128349 196 183 166 +0.514436 -0.052493 -1.127609 195 183 166 +0.524934 -0.052493 -1.127609 195 183 167 +0.535433 -0.052493 -1.127609 195 183 166 +0.545932 -0.052493 -1.127609 194 182 164 +0.553164 -0.052185 -1.120989 193 181 163 +0.562500 -0.052083 -1.118800 192 180 162 +0.570687 -0.051881 -1.114446 190 178 160 +0.579560 -0.051746 -1.111563 190 178 160 +0.589909 -0.051746 -1.111563 189 177 159 +0.461322 -0.067511 -1.208492 206 194 178 +0.452221 -0.064603 -1.156444 205 193 177 +0.460817 -0.064300 -1.151022 203 191 175 +0.469021 -0.063957 -1.144887 203 190 174 +0.478405 -0.063787 -1.141845 203 190 174 +0.487740 -0.063618 -1.138818 202 190 173 +0.498013 -0.063576 -1.138064 201 189 173 +0.505596 -0.063199 -1.131321 201 189 173 +0.514436 -0.062992 -1.127609 200 189 173 +0.522876 -0.062745 -1.123187 200 189 173 +0.532290 -0.062622 -1.120989 200 189 173 +0.542727 -0.062622 -1.120989 200 188 172 +0.551724 -0.062459 -1.118072 200 188 171 +0.559948 -0.062216 -1.113724 198 186 168 +0.570318 -0.062216 -1.113724 196 184 167 +0.578811 -0.062016 -1.110127 194 182 164 +0.589147 -0.062016 -1.110127 192 179 161 +0.430595 -0.079320 -1.217051 206 195 179 +0.460674 -0.078652 -1.206795 207 196 180 +0.470918 -0.078486 -1.204258 206 194 178 +0.478775 -0.077940 -1.195878 205 194 178 +0.471534 -0.075017 -1.151022 205 194 178 +0.479680 -0.074617 -1.144887 205 194 177 +0.489037 -0.074419 -1.141845 204 192 176 +0.498674 -0.074271 -1.139573 203 192 175 +0.505596 -0.073733 -1.131321 205 192 175 +0.511416 -0.073059 -1.120989 203 192 175 +0.521853 -0.073059 -1.120989 202 190 172 +0.531250 -0.072917 -1.118800 202 189 172 +0.541314 -0.072869 -1.118072 202 189 172 +0.549579 -0.072586 -1.113724 202 190 172 +0.559948 -0.072586 -1.113724 202 190 173 +0.570318 -0.072586 -1.113724 202 190 173 +0.580687 -0.072586 -1.113724 202 189 172 +0.418670 -0.090523 -1.215330 198 185 167 +0.428773 -0.090268 -1.211901 199 186 168 +0.438819 -0.090014 -1.208492 203 190 173 +0.449438 -0.089888 -1.206795 206 194 178 +0.459706 -0.089699 -1.204258 203 190 173 +0.470259 -0.089573 -1.202572 201 189 172 +0.480783 -0.089448 -1.200892 204 192 175 +0.505596 -0.084266 -1.131321 199 186 168 +0.510417 -0.083333 -1.118800 201 189 170 +0.520833 -0.083333 -1.118800 197 183 164 +0.531250 -0.083333 -1.118800 195 181 161 +0.541314 -0.083279 -1.118072 194 180 161 +0.551724 -0.083279 -1.118072 196 182 163 +0.560311 -0.083009 -1.114446 198 184 164 +0.606548 -0.073521 -0.987063 187 173 153 +0.615738 -0.073521 -0.987063 184 170 150 +0.624928 -0.073521 -0.987063 180 167 147 +0.633027 -0.073394 -0.985365 180 166 146 +0.642202 -0.073394 -0.985365 181 168 148 +0.418670 -0.101839 -1.215330 197 183 164 +0.427868 -0.101337 -1.209343 199 185 166 +0.438819 -0.101266 -1.208492 195 181 163 +0.449438 -0.101124 -1.206795 197 183 164 +0.459706 -0.100911 -1.204258 193 179 160 +0.470918 -0.100911 -1.204258 190 176 157 +0.498013 -0.095364 -1.138064 189 175 155 +0.501305 -0.093995 -1.121721 189 175 156 +0.510417 -0.093750 -1.118800 190 176 155 +0.520833 -0.093750 -1.118800 189 175 155 +0.531250 -0.093750 -1.118800 189 175 155 +0.541314 -0.093689 -1.118072 188 174 154 +0.551724 -0.093689 -1.118072 190 176 156 +0.598389 -0.082854 -0.988766 182 168 147 +0.606548 -0.082711 -0.987063 182 167 146 +0.615738 -0.082711 -0.987063 181 167 146 +0.624569 -0.082664 -0.986496 180 167 145 +0.633027 -0.082569 -0.985365 180 164 142 +0.642202 -0.082569 -0.985365 180 166 144 +0.262522 -0.138169 -1.484004 192 180 162 +0.275624 -0.137812 -1.480169 194 181 163 +0.290155 -0.138169 -1.484004 195 182 164 +0.419561 -0.113395 -1.217914 192 178 160 +0.428773 -0.112835 -1.211901 190 176 158 +0.439127 -0.112597 -1.209343 188 173 155 +0.449438 -0.112360 -1.206795 186 173 154 +0.459062 -0.111966 -1.202572 187 173 154 +0.469274 -0.111732 -1.200053 189 175 156 +0.505263 -0.105263 -1.130576 189 174 155 +0.510085 -0.104099 -1.118072 187 173 154 +0.520494 -0.104099 -1.118072 186 171 152 +0.530559 -0.104031 -1.117345 182 169 148 +0.541314 -0.104099 -1.118072 182 168 148 +0.551724 -0.104099 -1.118072 180 167 147 +0.598389 -0.092060 -0.988766 167 153 132 +0.607246 -0.092007 -0.988198 167 152 131 +0.616446 -0.092007 -0.988198 168 154 132 +0.624928 -0.091901 -0.987063 168 154 131 +0.633754 -0.091848 -0.986496 170 155 132 +0.642939 -0.091848 -0.986496 168 153 131 +0.261843 -0.151593 -1.480169 200 189 171 +0.275150 -0.151333 -1.477623 200 188 171 +0.288412 -0.151073 -1.475087 200 188 170 +0.420455 -0.125000 -1.220509 191 178 160 +0.429986 -0.124470 -1.215330 191 178 159 +0.440056 -0.124118 -1.211901 192 178 159 +0.450070 -0.123769 -1.208492 189 175 156 +0.459384 -0.123249 -1.203415 189 175 156 +0.469274 -0.122905 -1.200053 189 175 156 +0.531250 -0.114583 -1.118800 189 175 155 +0.599424 -0.101441 -0.990476 170 155 134 +0.607246 -0.101208 -0.988198 169 155 133 +0.616446 -0.101208 -0.988198 170 155 134 +0.625647 -0.101208 -0.988198 170 155 134 +0.633754 -0.101033 -0.986496 170 156 134 +0.642939 -0.101033 -0.986496 170 155 134 +0.261618 -0.165232 -1.478895 201 190 173 +0.274678 -0.164807 -1.475087 200 189 173 +0.287671 -0.164384 -1.471298 199 188 171 +0.301370 -0.164384 -1.471298 200 189 171 +0.421352 -0.136655 -1.223115 197 184 167 +0.431206 -0.136170 -1.218777 189 176 158 +0.441301 -0.135785 -1.215330 192 179 160 +0.451340 -0.135402 -1.211901 197 184 164 +0.459384 -0.134454 -1.203415 198 184 165 +0.469274 -0.134078 -1.200053 199 185 165 +0.479777 -0.133891 -1.198379 204 190 172 +0.599424 -0.110663 -0.990476 173 159 136 +0.607595 -0.110472 -0.988766 171 157 136 +0.616446 -0.110408 -0.988198 174 159 138 +0.625647 -0.110408 -0.988198 178 163 143 +0.633391 -0.110155 -0.985930 180 166 146 +0.642570 -0.110155 -0.985930 179 165 143 +0.261618 -0.179002 -1.478895 196 184 167 +0.274678 -0.178541 -1.475087 197 186 170 +0.287671 -0.178082 -1.471298 198 187 171 +0.301370 -0.178082 -1.471298 197 186 169 +0.421352 -0.148043 -1.223115 190 177 159 +0.431206 -0.147518 -1.218777 191 178 161 +0.441301 -0.147100 -1.215330 187 175 157 +0.451340 -0.146685 -1.211901 184 172 154 +0.460351 -0.145965 -1.205948 185 173 155 +0.471579 -0.145965 -1.205948 186 173 155 +0.479777 -0.145049 -1.198379 189 178 160 +0.599424 -0.119885 -0.990476 170 158 138 +0.607595 -0.119678 -0.988766 172 159 139 +0.616446 -0.119609 -0.988198 175 161 141 +0.625287 -0.119540 -0.987630 176 163 142 +0.633391 -0.119334 -0.985930 177 163 143 +0.642570 -0.119334 -0.985930 175 160 140 +0.261393 -0.192605 -1.477623 198 188 172 +0.274207 -0.191945 -1.472559 197 186 170 +0.287425 -0.191617 -1.470039 196 185 168 +0.301112 -0.191617 -1.470039 196 185 168 +0.420753 -0.159204 -1.221376 189 176 160 +0.431206 -0.158865 -1.218777 188 177 159 +0.441301 -0.158416 -1.215330 185 174 156 +0.451340 -0.157969 -1.211901 189 176 158 +0.460351 -0.157193 -1.205948 192 180 161 +0.471579 -0.157193 -1.205948 192 180 161 +0.479777 -0.156206 -1.198379 189 177 158 +0.470274 -0.149633 -1.147947 189 177 158 +0.480962 -0.149633 -1.147947 193 180 162 +0.599769 -0.129181 -0.991047 182 168 149 +0.607595 -0.128884 -0.988766 178 166 145 +0.616092 -0.128736 -0.987630 178 164 143 +0.624569 -0.128588 -0.986496 177 163 142 +0.633391 -0.128514 -0.985930 179 165 143 +0.642570 -0.128514 -0.985930 179 164 143 +0.261168 -0.206186 -1.476354 192 180 163 +0.27397 \ No newline at end of file diff --git a/auto/python/pad_detection.py b/auto/python/pad_detection.py new file mode 100644 index 0000000000..8c63623699 --- /dev/null +++ b/auto/python/pad_detection.py @@ -0,0 +1,136 @@ +import rospy +from sensor_msgs.msg import Image,CameraInfo +from std_msgs.msg import Float32 +from tf2_msgs.msg import TFMessage +from geometry_msgs.msg import TransformStamped, Vector3, Quaternion, PoseWithCovarianceStamped, PoseStamped, Point +from cv_bridge import CvBridge +from airsim2ros.msg import Barometer, Distance +from nav_msgs.msg import Odometry +from multiprocessing import Process +from tf import TransformListener + +from cv_bridge import CvBridge, CvBridgeError + +from tf.transformations import quaternion_from_matrix, rotation_matrix, euler_from_quaternion, quaternion_multiply + +import setup_path +import airsim +import cv2 +import math +import numpy as np +import roslaunch +import tf2_ros +import tf2_geometry_msgs +from matplotlib import pyplot as plt +import threading + +MIN_MATCH_COUNT = 10 + +BOARD_SIZE = 1.00 + +class PadDetector(): + + # Define a callback for the Image message + def imageCallback(self, img_msg): + self.curr_time = img_msg.header.stamp + # Try to convert the ROS Image message to a CV2 Image + try: + self.curr_image = self.bridge.imgmsg_to_cv2(img_msg, "passthrough") + except CvBridgeError, e: + rospy.logerr("CvBridge Error: {0}".format(e)) + + def camInfoCallback(self, cam_info): + # Try to convert the ROS Image message to a CV2 Image + self.K = np.reshape(cam_info.K,(3,3)).astype(np.float32) + self.D = np.reshape(cam_info.D,(1,5)).astype(np.float32) + + def buildPoseMsg(self, rvec, tvec): + + pose_msg = PoseStamped() + pose_msg.header.stamp = self.curr_time + pose_msg.header.frame_id = 'Drone1' + #get 3x3 mat + rmat, _jac = cv2.Rodrigues(rvec) + + #transform board frame to camera frame + R = rmat.transpose() + t = -R.dot(tvec) + + #Switch x and y coordinates going from image to camera frame? + t = np.array([t[1]-BOARD_SIZE/2,-t[0]+BOARD_SIZE/2,-t[2]]) + P = np.hstack((R,t)) + + #Transform from camera frame to REP-103 + cam_2_ned = np.array( [[1,0,0], + [0,1,0], + [0,0,-1]]) + P = cam_2_ned.dot(P) + R = P[0:3,0:3] + R = np.vstack((R,[0,0,0])) + R = np.hstack((R,[[0],[0],[0],[1]])) + t = P[0:3,3] + #convert to quaternion and build msg + q = quaternion_from_matrix(R) + + pose_msg.pose.position = Point(t[0],t[1],t[2]) + pose_msg.pose.orientation = Quaternion(q[0],q[1],q[2],q[3]) + + return pose_msg + + # Override the run() function of Thread class + def run(self): + rate = rospy.Rate(30) # 30hz + # Load the predefined dictionary + dictionary = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250) + # Generate the board + board = cv2.aruco.GridBoard_create(2,2, 0.46, 0.08, dictionary) + # Initialize the detector parameters using default values + parameters = cv2.aruco.DetectorParameters_create() + while not rospy.is_shutdown(): + img1 = self.curr_image; + # Detect the markers in the image + markerCorners, markerIds, rejectedCandidates = cv2.aruco.detectMarkers(img1, dictionary, parameters=parameters) + try: + _ret, rvec, tvec = cv2.aruco.estimatePoseBoard(markerCorners, markerIds, board, self.K, self.D) + self.pose_pub.publish(self.buildPoseMsg(rvec,tvec)) + except: + continue + rate.sleep() + + def stopped(self): + return self._stop_event.is_set() + + def __init__(self): + rospy.init_node('autonomy', anonymous=True) + rospy.Subscriber('/airsim/Drone1/main/image_raw', Image, self.imageCallback) + rospy.Subscriber('/airsim/Drone1/main', CameraInfo, self.camInfoCallback) + self._stop_event = threading.Event() + + rate = rospy.Rate(30) # 30hz + self.curr_image = np.array([]) + self.curr_time = 0 + self.bridge = CvBridge() + self.K = np.array([]) + self.D = np.array([]) + self.transformer = TransformListener() + self.tfBuffer = tf2_ros.Buffer() + listener = tf2_ros.TransformListener(self.tfBuffer) + + self.pose_pub = rospy.Publisher('mavros/vision_pose/pose', PoseStamped, queue_size=10) + + if (len(self.curr_image) == 0): + rospy.wait_for_message('/airsim/Drone1/main/image_raw',Image) + print('got image') + + if (len(self.K) == 0): + rospy.wait_for_message('/airsim/Drone1/main',CameraInfo) + print('got camInfo') + + +if __name__ == "__main__": + pd = PadDetector() + pd.run() + + + + diff --git a/auto/python/pad_detection.pyc b/auto/python/pad_detection.pyc new file mode 100644 index 0000000000..8a4ff00043 Binary files /dev/null and b/auto/python/pad_detection.pyc differ diff --git a/auto/python/setup_path.py b/auto/python/setup_path.py new file mode 100644 index 0000000000..c547acd8ea --- /dev/null +++ b/auto/python/setup_path.py @@ -0,0 +1,52 @@ +# Import this module to automatically setup path to local airsim module +# This module first tries to see if airsim module is installed via pip +# If it does then we don't do anything else +# Else we look up grand-parent folder to see if it has airsim folder +# and if it does then we add that in sys.path + +import os,sys,logging + +#this class simply tries to see if airsim +class SetupPath: + @staticmethod + def getDirLevels(path): + path_norm = os.path.normpath(path) + return len(path_norm.split(os.sep)) + + @staticmethod + def getCurrentPath(): + cur_filepath = __file__ + return os.path.dirname(cur_filepath) + + @staticmethod + def getGrandParentDir(): + cur_path = SetupPath.getCurrentPath() + if SetupPath.getDirLevels(cur_path) >= 2: + return os.path.dirname(os.path.dirname(cur_path)) + return '' + + @staticmethod + def getParentDir(): + cur_path = SetupPath.getCurrentPath() + if SetupPath.getDirLevels(cur_path) >= 1: + return os.path.dirname(cur_path) + return '' + + @staticmethod + def addAirSimModulePath(): + # if airsim module is installed then don't do anything else + #import pkgutil + #airsim_loader = pkgutil.find_loader('airsim') + #if airsim_loader is not None: + # return + + parent = SetupPath.getParentDir() + if parent != '': + airsim_path = os.path.join(parent, 'airsim') + client_path = os.path.join(airsim_path, 'client.py') + if os.path.exists(client_path): + sys.path.insert(0, parent) + else: + logging.warning("airsim module not found in parent folder. Using installed package (pip install airsim).") + +SetupPath.addAirSimModulePath() diff --git a/auto/python/setup_path.pyc b/auto/python/setup_path.pyc new file mode 100644 index 0000000000..37d00da3d6 Binary files /dev/null and b/auto/python/setup_path.pyc differ diff --git a/auto/python/sfm.py b/auto/python/sfm.py new file mode 100644 index 0000000000..d6538c97d8 --- /dev/null +++ b/auto/python/sfm.py @@ -0,0 +1,254 @@ +import setup_path +import airsim +import numpy as np +import time +import math +import cv2 +import open3d as o3d #pip install open3d; conda install "tornado<4.5.3" + +from scipy.sparse import lil_matrix +from scipy.optimize import least_squares + + + +def getColors(pixel_coords, image): + color = [] + for i in range(len(pixel_coords)): + x, y = pixel_coords[i].ravel() + x = min(image.shape[1]-1,max(0,x)) + y = min(image.shape[0]-1,max(0,y)) + BGR = ((image[int(y),int(x)].astype(float))/255) + color.append([BGR[2],BGR[1],BGR[0]]) + color = np.reshape(color,(len(color),3)) + return color + +def triangulate(pts1, P0, pts2, P1, K, d): + #initialize lists + obj_pts = [] + mask = [] + total = 0 + count = 0 + + #convert to weird array format that undistort points needs (N, 1, 2) DEPENDS ON OPENCV version + pts1 = np.reshape(pts1,(len(pts1),1,2)) + pts2 = np.reshape(pts2,(len(pts2),1,2)) + + #Normalize and undistort, this will convert the pts from pixel coordinates to camera coordinates (i.e. [-1 > 0 < 1]) + pts1_norm = cv2.undistortPoints(pts1, K, d) + pts2_norm = cv2.undistortPoints(pts2, K, d) + pts1_norm = np.reshape(pts1_norm,(len(pts1),2)) + pts2_norm = np.reshape(pts2_norm,(len(pts2),2)) + #pull out Rotation and translation from P1 matrix for reprojection test + R = np.reshape(P1[:,0:3],(3,3)) + t = np.reshape(P1[:,3],(3,1)) + + #for all points + points3d = [] + for i in range(len(pts1_norm)): + #triangulate + point = triangulate_int(pts1_norm[i],P0,pts2_norm[i],P1) + points3d.append(point) + points3d = np.reshape(points3d,(len(points3d),3)) + #do a reprojection + reproj_pts, ret = cv2.projectPoints(points3d,R,t,K,d) + for i in range(len(reproj_pts)): + rx, ry = reproj_pts[i].ravel() + ox, oy = pts2[i].ravel() + #get distance between projected and original points + distance = math.sqrt((rx-ox)**2+(ry-oy)**2) + #test if distance is above threshold + if (distance < 5): + #add point to cloud + obj_pts.append(points3d[i]) + print(points3d[i]) + count = count + 1 + mask.append([1]) + #aggregate distance for average calculation + total = total + distance + else: + mask.append([0]) + + obj_pts = np.reshape(obj_pts,(len(obj_pts),3)) + mask = np.reshape(mask,(len(mask),1)) + error = total/(len(obj_pts)+1) + print('Triangulated points',count,'out of',len(pts1_norm),'=',int(count/len(pts1_norm)*100),'%') + print('Average reprojection error',int(error),'pixel(s)') + + return obj_pts, mask, error + +def triangulate_int(pix_pt1,P0,pix_pt2,P1): + #code taken from 'Mastering OpenCV with Practical Computer Vision Projects' Chapter 4 + #https://www.cs.ccu.edu.tw/~damon/photo/,OpenCV/,Mastering_OpenCV.pdf + #build A matrix + A = np.array([[pix_pt1[0]*P0[2,0]-P0[0,0],pix_pt1[0]*P0[2,1]-P0[0,1],pix_pt1[0]*P0[2,2]-P0[0,2]], + [pix_pt1[1]*P0[2,0]-P0[1,0],pix_pt1[1]*P0[2,1]-P0[1,1],pix_pt1[1]*P0[2,2]-P0[1,2]], + [pix_pt2[0]*P1[2,0]-P1[0,0], pix_pt2[0]*P1[2,1]-P1[0,1],pix_pt2[0]*P1[2,2]-P1[0,2]], + [pix_pt2[1]*P1[2,0]-P1[1,0], pix_pt2[1]*P1[2,1]-P1[1,1],pix_pt2[1]*P1[2,2]-P1[1,2]]]) + #build B vector + B = np.array([[-1*(pix_pt1[0]*P0[2,3]-P0[0,3])], + [-1*(pix_pt1[1]*P0[2,3]-P0[1,3])], + [-1*(pix_pt2[0]*P1[2,3]-P1[0,3])], + [-1*(pix_pt2[1]*P1[2,3]-P1[1,3])]]) + #solve for X + ret, X = cv2.solve(A,B,flags=cv2.DECOMP_SVD) + #print(X) + return X + +def drawTracks(orig, next, img, color): + # Draws the optical flow tracks + output = img.copy() + mask = np.zeros_like(output) + for i, (new, old) in enumerate(zip(next, orig)): + # Returns a contiguous flattened array as (x, y) coordinates for new point + a, b = new.ravel() + # Returns a contiguous flattened array as (x, y) coordinates for old point + c, d = old.ravel() + # Draws line between new and old position with green color and 2 thickness + mask = cv2.line(mask, (a, b), (c, d), color, 2) + # Draws filled circle (thickness of -1) at new position with green color and radius of 3 + output = cv2.circle(output, (a, b), 3, color, -1) + # Overlays the optical flow tracks on the original frame + output = cv2.add(output, mask) + return output + +def getTrackLength(orig,new): + num_samples = 10 + rand_pts = np.random.choice(len(new),num_samples,replace=False) + total = 0 + for i in range(num_samples): + j = rand_pts[i] + x1, y1 = orig[j].ravel() + x2, y2 = new[j].ravel() + #get distance between new and original points + distance = math.sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2)) + total = total + distance + avg_dist = total/num_samples + return avg_dist + +def getObjectPointsEssential(prev_pts,new_pts,P0,K,d): + #Get essential matrix to derive R, t, and F + E, mask_e = cv2.findEssentialMat(prev_pts,new_pts, K, method=cv2.RANSAC, prob=0.999, threshold = 1.0) + print ("Essential matrix: used ", np.sum(mask_e) ," of total ",len(new_pts),"matches") + prev_pts = prev_pts[mask_e[:,0]==1] + new_pts = new_pts[mask_e[:,0]==1] + + R1, R2, t1 = cv2.decomposeEssentialMat(E) + t2 = -1*t1 + + P1 = [] + #These are the four possible Projection Matrices, only one produces the most correct points + P1.append(np.hstack((R1,t1))*P0) + P1.append(np.hstack((R1,t2))*P0) + P1.append(np.hstack((R2,t1))*P0) + P1.append(np.hstack((R2,t2))*P0) + best_obj_pts = [] + best_error = 1000 + for i in range(4): + #Get 3d world points and associated pixel values for the second image + obj_pts, mask_tri, error = triangulate(prev_pts, P0, new_pts, P1[i], K, d) + if(len(obj_pts) > len(best_obj_pts)): + best_error = error + best_obj_pts = obj_pts + best_mask_tri = mask_tri + best_P1 = P1[i] + elif(len(obj_pts) == len(best_obj_pts) & (error < best_error)): + best_error = error + best_obj_pts = obj_pts + best_mask_tri = mask_tri + best_P1 = P1[i] + + prev_pts = prev_pts[best_mask_tri[:,0]==1] + new_pts = new_pts[best_mask_tri[:,0]==1] + + return best_obj_pts, prev_pts, new_pts, best_P1 + +def getObjectPointsPnP(obj_pts,new_pts,P0,K,d,rvec,tvec): + #SolvePNP + _, rvec, tvec, inliers = cv2.solvePnPRansac(obj_pts, pixel_pts, K, d, rvec, tvec, useExtrinsicGuess = True) + print('SolvePnP used',len(inliers),'points of total', len(pixel_pts),'=',int(len(inliers)/len(pixel_pts)*100),'%') + #convert pnp rotation to (3x3) with rodrigues method + R, _ = cv2.Rodrigues(rvec) + t = tvec + + #Create second projection matrix + #P1 = np.dot(K,np.hstack((R, t))) + P1 = np.hstack((R, t)) + + #Now we need to get the full set of matches to apply this projection matrix to + #Get 3d world points and associated pixel values for the second image + obj_pts, mask_tri = triangulate(pts1, P0, pts2, P1, K, d) + + return obj_pts, mask_tri, P1 + +def eliminateDuplicateObjects(prev_objs, new_features): + #get all objects and pixel coords from the last image + prev_objs = np.reshape(prev_objs,(-1,6)) + #a new mask to check for last images objects in the new features + mask_1 = np.full((len(new_features),4),-1) + #a new mask to remove these duplicates from the pre_objs_array + mask_2 = np.ones(len(prev_objs[:,0])) + count = 0 + for i in range(len(new_features)): + x,y = new_features[i].ravel() + for j in range(len(prev_objs)): + #checks distance between new features and existing object pixels + if(math.isclose(prev_objs[j,4],x,rel_tol=0.02) & math.isclose(prev_objs[j,5],y,rel_tol=0.03)): + if (mask_2[j] == 1): + count += 1 + mask_1[i] = prev_objs[j,0:4] + mask_2[j] = 0 + break + #eliminate duplicate objects contained in the pre_objs array + prev_objs = prev_objs[mask_2==1] + #create the overall mask + mask_3 = np.vstack((mask_1,prev_objs[:,0:4])) + #create a total list of pixel coords consisting of new features and previous pixel coordinates, + #must be the same length as the object index mask + new_features = new_features.reshape(len(new_features),2) + orig = np.vstack((new_features,prev_objs[:,4:6])).reshape(-1, 1, 2).astype(np.float32) + + return orig, mask_3 + +def baFun(params, n_cameras, n_points, camera_indices, points_2d): + """Compute residuals. + `params` contains camera parameters and 3-D coordinates. + """ + cam_list = [] + for i in range(n_cameras): + begin = (i*6) + end = (i*6+6) + cam_list.append(params[begin:end].ravel()) + + points_3d = [] + for i in range(n_points): + begin = i*3+(n_cameras*6) + end = i*3+3+(n_cameras*6) + points_3d.append(params[begin:end]) + + proj_list = [] + for i in range(n_points): + camera = cam_list[camera_indices[i]] + R = np.reshape(camera[0:3],(1,3)) + t = np.reshape(camera[3:6],(1,3)) + R, _ = cv2.Rodrigues(R) + point_proj, ret = cv2.projectPoints(points_3d[i],R,t,K,d) + proj_list.append(point_proj) + proj_list = np.reshape(proj_list,(-1,2)) + points_2d = np.reshape(points_2d,(-1,2)) + return (proj_list - points_2d).ravel() + +def bundle_adjustment_sparsity(n_cameras, n_points, camera_indices, point_indices): + m = camera_indices.size * 2 + n = n_cameras * 6 + n_points * 3 + A = lil_matrix((m, n), dtype=int) + + i = np.arange(camera_indices.size) + for s in range(6): + A[2 * i, camera_indices * 6 + s] = 1 + A[2 * i + 1, camera_indices * 6 + s] = 1 + + for s in range(3): + A[2 * i, n_cameras * 6 + point_indices * 3 + s] = 1 + A[2 * i + 1, n_cameras * 6 + point_indices * 3 + s] = 1 + + return A diff --git a/auto/python/sfm.pyc b/auto/python/sfm.pyc new file mode 100644 index 0000000000..aa7c2b5d75 Binary files /dev/null and b/auto/python/sfm.pyc differ diff --git a/auto/python/sfm_ros.py b/auto/python/sfm_ros.py new file mode 100644 index 0000000000..b93d4308f5 --- /dev/null +++ b/auto/python/sfm_ros.py @@ -0,0 +1,304 @@ +import rospy +import tf2_ros +import tf2_geometry_msgs +import cv2 +import numpy as np +import math +import struct + +from cv_bridge import CvBridge +from sensor_msgs.msg import Image, CameraInfo, PointCloud2, PointField +from sensor_msgs import point_cloud2 +from tf2_msgs.msg import TFMessage +from geometry_msgs.msg import PoseStamped, Quaternion, TransformStamped +from nav_msgs.msg import Odometry + +from sfm import getColors +from sfm import triangulate +from sfm import triangulate_int +from sfm import drawTracks +from sfm import getTrackLength +from sfm import getObjectPointsEssential +from sfm import eliminateDuplicateObjects +from sfm import baFun +from sfm import bundle_adjustment_sparsity + +from scipy.sparse import lil_matrix +from scipy.optimize import least_squares + +from tf.transformations import quaternion_matrix, euler_from_quaternion, quaternion_multiply, quaternion_from_matrix + +class mapping(): + + def __init__(self): + + rospy.init_node('mapping', anonymous=True) + + self.bridge = CvBridge() + self.tfBuffer = tf2_ros.Buffer() + listener = tf2_ros.TransformListener(self.tfBuffer) + + self.image = [] + self.pose = PoseStamped() + self.K = [] + self.d = [] + self.cam_width = [] + self.cam_height = [] + self.rotation = [] + self.translation = [] + self.tracking = False + + self.img_curr = [] + self.img_prev = [] + + self.features_orig = [] + self.features_prev = [] + + self.obj_mask = [] + + self.reset = True + + #INITIALIZE FEATURE MATCHING PARAMETERS# + self.maxCorners = 500 #Maximum number of corners to return. If there are more corners than are found, the strongest of them is returned + self.qualityLevel = 0.01 #For example, if best corner has measure = 1500, and qualityLevel=0.01 , then corners with quality<15 are rejected. + self.minDistance = 10 #Minimum possible Euclidean distance between the returned corners. + self.lk_params = dict(winSize = (15,15), maxLevel = 2, criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03)) + + #rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.poseCallback) + rospy.Subscriber('orb_slam2_mono/pose', PoseStamped, self.poseCallback) + rospy.Subscriber('/airsim/base_link/camera/image_raw', Image, self.imageCallback) + rospy.Subscriber('/airsim/base_link/camera', CameraInfo, self.camInfoCallback) + self.cloud_pub = rospy.Publisher("cloud", PointCloud2, queue_size=10) + self.test_pose = rospy.Publisher("test_pose", PoseStamped, queue_size=10) + + print('waiting on topics...') + rospy.wait_for_message('/airsim/base_link/camera/image_raw', Image) + + #self.cam_width = self.img_curr.shape[0] + #self.cam_height = self.img_curr.shape[1] + print('K: ', self.K) + print('D: ', self.D) + rospy.wait_for_message('/mavros/local_position/pose', PoseStamped) + print('connected') + print(float(self.pose.header.stamp.secs)+float(self.pose.header.stamp.nsecs)/1000000000.0,float(self.header.stamp.secs)+float(self.header.stamp.nsecs)/1000000000.0) + self.run() + + def poseCallback(self, data): + self.pose = data + #self.test_pose.publish(self.pose) + q = (self.pose.pose.orientation.x,self.pose.pose.orientation.y,self.pose.pose.orientation.z,self.pose.pose.orientation.w) + r = quaternion_matrix(q) + #print(r) + self.rotation = r[0:3,0:3] + self.translation = np.array(([self.pose.pose.position.x],[self.pose.pose.position.y],[self.pose.pose.position.z])) + #print(self.translation) + + def imageCallback(self, data): + self.header = data.header + self.img_curr = self.bridge.imgmsg_to_cv2(data, desired_encoding='passthrough') + + def run(self): + #TAKE NEW IMAGE# + rawImage2 = self.img_curr + if (rawImage2 == None): + print("Camera is not returning image, please check airsim for error messages") + sys.exit(0) + else: + img2 = self.img_curr + gray2 = cv2.cvtColor(img2, cv2.COLOR_RGB2GRAY) + + #GET NEW FEATURE LOCATIONS# + next, status, error = cv2.calcOpticalFlowPyrLK(gray, gray2, prev, None, **lk_params) + error = error[status[:,0] == 1] + #First, filter out the points with high error + new = next[status[:,0] == 1] + new = new[error[:,0] < 10] + #Update the original list of features to reflect pixels that have been lost in the flow + orig = orig[status[:,0] == 1] + orig = orig[error[:,0] < 10] + #update the object mask + obj_mask = obj_mask[status[:,0] == 1] + obj_mask = obj_mask[error[:,0] < 10] + + # Updates previous good feature points + prev = new.reshape(-1, 1, 2) + + #Optional visualization + output = drawTracks(orig.astype(int), new, img2, (0, 255, 0)) + cv2.imshow('Tracks',output) + gray = gray2 + + #IF SIGNIFICANT CHANGE FROM PREVIOUS IMAGE# + avg_track_len = getTrackLength(orig,new) + if (avg_track_len > (img.shape[0]/5) or (len(orig)/orig_count < 0.8)): + print('update, image #:',img_idx) + #Time to calculate SFM + + prev_pts = orig.reshape(len(orig),2) + new_pts = new.reshape(len(new),2) + mask_inf = np.zeros(len(new_pts)) + '''Lets check for infiniti points''' + for i in range(len(new_pts)): + x1, y1 = prev_pts[i].ravel() + x2, y2 = new_pts[i].ravel() + #get distance between new and original points + distance = math.sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2)) + if (distance >= 5): + mask_inf[i] = 1 + + new_pts = new_pts[mask_inf==1] + obj_mask = obj_mask[mask_inf==1] + print(len(prev_pts)-len(new_pts),'"infinite" eliminated') + prev_pts = prev_pts[mask_inf==1] + + #Is this the first calculation? + if (img_idx == 0): + #initial P0 + P0 = np.hstack((np.eye(3, 3), np.zeros((3, 1)))) + #get first set of objects from essential matrix + obj_pts, prev_pts, new_pts, P = getObjectPointsEssential(prev_pts,new_pts,P0,K,d) + + #Check quality of pts + if (len(obj_pts) > 15): + cam_objs = [] + for i in range(len(obj_pts)): + cam_objs.append(np.hstack((i,obj_pts[i].ravel(),prev_pts[i].ravel()))) + all_objs.append(cam_objs) + cam_objs = [] + for i in range(len(obj_pts)): + cam_objs.append(np.hstack((i,obj_pts[i].ravel(),new_pts[i].ravel()))) + obj_idx += 1 + all_objs.append(cam_objs) + img_idx += 1 + all_P_mats.append(P0) + all_P_mats.append(P) + P0 = P + #get a new set of features to track + new_features = cv2.goodFeaturesToTrack(gray, maxCorners, qualityLevel, minDistance) + orig, obj_mask = eliminateDuplicateObjects(all_objs[img_idx],new_features) + t = np.reshape(P0[:,3],(1,3)) + t = t * np.array([1,-1,1]) + #Get colors from detected pixels for coloring pointcloud + colors = getColors(new_pts,img2.copy()) + all_pts = obj_pts + all_colors = colors + all_pts = np.vstack((all_pts,np.reshape(t,(1,3)))) + all_colors = np.vstack((all_colors,np.array([0.,1.,0.]))) + else: + orig = cv2.goodFeaturesToTrack(gray, maxCorners, qualityLevel, minDistance) + + + + else:#get camera matrices from PNP + #SolvePNP + masked_objs = obj_mask[obj_mask[:,0]!=-1] + pixel_pts = new_pts[obj_mask[:,0]!=-1] + obj_pts = masked_objs[:,1:4] + + _, rvec, tvec, inliers = cv2.solvePnPRansac(obj_pts, pixel_pts, K, d, flags=cv2.SOLVEPNP_EPNP) + print('SolvePnP used',len(inliers),'points of total', len(pixel_pts),'=',int(len(inliers)/len(pixel_pts)*100),'%') + #convert pnp rotation to (3x3) with rodrigues method + P1 = np.hstack((cv2.Rodrigues(rvec)[0], tvec)) + + #Now we need to get the full set of matches to apply this projection matrix to + #Get 3d world points and associated pixel values for the second image + new_objs, mask_tri, error = triangulate(prev_pts, P0, new_pts, P1, K, d) + + obj_mask = obj_mask[mask_tri[:,0]==1] + prev_pts = prev_pts[mask_tri[:,0]==1] + new_pts = new_pts[mask_tri[:,0]==1] + + cam_objs = [] + for i in range(len(new_objs)): + if (obj_mask[i,0] != -1): + cam_objs.append(np.hstack((obj_mask[i,0],new_objs[i].ravel(),new_pts[i].ravel()))) + else: + cam_objs.append(np.hstack((obj_idx,new_objs[i].ravel(),new_pts[i].ravel()))) + obj_idx += 1 + all_objs.append(cam_objs) + img_idx += 1 + all_P_mats.append(P1) + P0 = P1 + #get a new set of features to track + new_features = cv2.goodFeaturesToTrack(gray, maxCorners, qualityLevel, minDistance) + orig, obj_mask = eliminateDuplicateObjects(all_objs[img_idx],new_features) + t = np.reshape(P0[:,3],(1,3)) + t = t * np.array([1,-1,1]) + #Get colors from detected pixels for coloring pointcloud + colors = getColors(new_pts,img2.copy()) + all_pts = np.vstack((all_pts,new_objs)) + all_colors = np.vstack((all_colors,colors)) + all_pts = np.vstack((all_pts,np.reshape(t,(1,3)))) + all_colors = np.vstack((all_colors,np.array([1.,0.,0.]))) + + print('colro\points',len(all_pts),len(all_colors)) + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(all_pts) + pcd.colors = o3d.utility.Vector3dVector(all_colors.astype(float)) + o3d.io.write_point_cloud("pointcloud.ply", pcd) + + frames = 3 + if (False): + #if ((img_idx > frames) & ((img_idx-1) % frames == 0)): + #Lets do bundle adjustment + + n_cameras = frames + + ba_objs = [] + camera_params = [] + for i in range(frames): + ba_objs.extend(np.reshape(all_objs[(img_idx-frames-1+i)],(-1,6))) + P = all_P_mats[(img_idx-frames-1+i)] + R, _ = cv2.Rodrigues(P[:,0:3]) + t = np.reshape(P[:,3],(1,3)) + camera_params.extend(np.hstack((R.ravel(),t.ravel()))) + ba_objs = np.reshape(ba_objs,(-1,6)) + camera_params = np.reshape(camera_params,(-1,6)) + print(camera_params) + + camera_indices = np.empty_like(ba_objs[:,0]).astype(int) + next = 0 + prev = 0 + for i in range(frames): + next += len(all_objs[(img_idx-frames-1+i)]) + camera_indices[prev:next] = i + prev = next + + points_3d = np.reshape(ba_objs[:,1:4],(-1,3)) + points_2d = np.reshape(ba_objs[:,4:6],(-1,2)) + point_indices = np.reshape(ba_objs[:,0],(-1,1)) + + n_points = len(points_3d) + + n = 6 * n_cameras + 3 * n_points + m = 2 * n_points + + print("n_cameras: {}".format(n_cameras)) + print("n_points: {}".format(n_points)) + print("Total number of parameters: {}".format(n)) + print("Total number of residuals: {}".format(m)) + + #ba_thread = myThread(1,"thread1",n_cameras,n_points, camera_indices, point_indices) + + x0 = np.hstack((camera_params.ravel(), points_3d.ravel())) + f0 = baFun(x0, n_cameras, n_points, camera_indices, points_2d) + + plt.plot(f0) + #plt.show() + + A = bundle_adjustment_sparsity(n_cameras, n_points, camera_indices, point_indices) + res = least_squares(baFun, x0, jac_sparsity=A, verbose=2, x_scale='jac', ftol=1e-4, method='trf', + args=(n_cameras, n_points, camera_indices, points_2d)) + + plt.plot(res.fun) + plt.show() + prev = orig + orig_count = len(orig) + + + key = cv2.waitKey(1) & 0xFF; + if (key == 27 or key == ord('q') or key == ord('x')): + break; + +if __name__ == '__main__': + mapping() diff --git a/auto/python/slam.py b/auto/python/slam.py new file mode 100644 index 0000000000..6f97a90efd --- /dev/null +++ b/auto/python/slam.py @@ -0,0 +1,280 @@ +import rospy +import glob +import json +import math +import os +import px4tools +import sys +import time +import actionlib +import roslaunch +import numpy as np +import tf2_ros +import tf2_geometry_msgs +import cv2 +import shutil +import pcl_ros +import tf2_sensor_msgs.tf2_sensor_msgs +from sensor_msgs import point_cloud2 + +from cv_bridge import CvBridge, CvBridgeError + +from mavros_msgs.msg import Mavlink, Waypoint, WaypointReached, GlobalPositionTarget, State, TakeoffAction, TakeoffGoal, LandAction, LandGoal, WaypointsAction, WaypointsGoal, HomePosition +from mavros_msgs.srv import CommandBool, SetMode, CommandTOL, WaypointPush, WaypointClear, CommandHome +from orb_slam2_ros.msg import KeyFrames, Observations +from sensor_msgs.msg import NavSatFix, Image, PointCloud2, PointField +from std_msgs.msg import Header, Float32 +from nav_msgs.msg import Odometry +from geometry_msgs.msg import Pose, PoseWithCovariance, PoseStamped, PoseWithCovarianceStamped, TransformStamped +from mavros_test_common import MavrosTestCommon +from pymavlink import mavutil +from tf.transformations import quaternion_from_matrix, quaternion_from_euler, euler_from_quaternion, quaternion_conjugate + +class KeyFrame(object): + empty = [] + + def __init__(self): + self.image_id = "" + self.latitude = 0.0 + self.longitude = 0.0 + self.altitude = 0.0 + self.orientation = [0.0,0.0,0.0,0.0] + self.image = "" + +class companion(): + + def __init__(self): + + #Initialize ROSLaunch + launch = roslaunch.scriptapi.ROSLaunch() + launch.start() + uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) + roslaunch.configure_logging(uuid) + + #Launch ORBSLAM + cli_args = ('orb_slam2_ros', 'orb_slam2_mono.launch') + roslaunch_file = [(roslaunch.rlutil.resolve_launch_arguments(cli_args)[0])] + orbslam = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_file) + orbslam.start() + + rospy.init_node('companion_node', anonymous=True) + + #clear old images + shutil.rmtree('img') + os.mkdir('img') + + self.local_pose = Pose() + self.slam_pose = PoseStamped() + self.raw_slam_pose = PoseStamped() + self.calib_init_slam = Pose() + self.calib_init_local = Pose() + self.slam_calibrated = False + self.slam_offset_x = 0 + self.slam_offset_y = 0 + self.slam_offset_z = 0 + self.slam_scale = 1 + self.state = "SYSTEM_NOT_READY" + self.last_state = "SYSTEM_NOT_READY" + + #States: + #"SYSTEM_NOT_READY" + #"NO_IMAGES_YET" + #"NOT_INITIALIZED" + #"OK" + #"LOST" + + self.keyframes = [] + self.image_buffer = [] + self.bridge = CvBridge() + + rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.local_pose_callback) + rospy.Subscriber('/orb_slam2_mono/pose', PoseStamped, self.slamCallback) + rospy.Subscriber('/orb_slam2_mono/state', Header, self.stateCallback) + rospy.Subscriber('/orb_slam2_mono/map_points', PointCloud2, self.pointcloudCallback) + rospy.Subscriber('/orb_slam2_mono/observations', Observations, self.observationsCallback) + rospy.Subscriber('/orb_slam2_mono/keyframes', KeyFrames, self.keyframesCallback) + rospy.Subscriber('/airsim/base_link/camera/image_raw', Image, self.imageBuffer) + #rospy.Subscriber('/orb_slam2_mono/keyframes', KeyFrames, self.keyframesCallback) + self.slam_pub = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=50) + self.cloud_pub = rospy.Publisher('/test/new_cloud', PointCloud2) + self.scale_pub = rospy.Publisher('/orb_slam2_mono/map_scale', Float32, queue_size=1) + self.tfBuffer = tf2_ros.Buffer() + listener = tf2_ros.TransformListener(self.tfBuffer) + + rospy.spin() + + #Code to scale and publish orbslam pose to px4 + + def local_pose_callback(self,data): + #TODO Ensure this data is up to date!! + self.local_pose = data.pose + + def stateCallback(self,data): + #Check for state change + self.state = data.frame_id + if (self.last_state != self.state): + self.slam_calibrated = False + if (self.state == "OK"): + self.calib_init_slam = self.raw_slam_pose.pose + self.calib_init_local = self.local_pose + self.last_state = self.state + + def slamCallback(self,data): + + self.raw_slam_pose = self.convertFRD(data) + #self.slam_pub.publish(self.raw_slam_pose) + #return + self.runCalibration() + if (self.slam_calibrated): + #transform SLAM pose + self.slam_pose.pose.position.x = self.raw_slam_pose.pose.position.x * self.slam_scale + self.slam_offset_x + self.slam_pose.pose.position.y = self.raw_slam_pose.pose.position.y * self.slam_scale + self.slam_offset_y + self.slam_pose.pose.position.z = self.raw_slam_pose.pose.position.z * self.slam_scale + self.slam_offset_z + self.slam_pose.pose.orientation = self.raw_slam_pose.pose.orientation + self.slam_pose.header = self.raw_slam_pose.header + + self.slam_pub.publish(self.slam_pose) + #else: + #if (self.state == "OK"): + + + + def runCalibration(self): + #check for sufficient calibration movement + x = self.local_pose.position.x - self.calib_init_local.position.x + y = self.local_pose.position.y - self.calib_init_local.position.y + x_slam = self.raw_slam_pose.pose.position.x - self.calib_init_slam.position.x + y_slam = self.raw_slam_pose.pose.position.y - self.calib_init_slam.position.y + if (math.sqrt(x*x+y*y) > 5.0 and x_slam != 0): + self.slam_scale = math.sqrt(x*x+y*y)/math.sqrt(x_slam*x_slam+y_slam*y_slam) + self.slam_offset_x = (self.calib_init_local.position.x-self.slam_scale*self.calib_init_slam.position.x) + self.slam_offset_y = (self.calib_init_local.position.y-self.slam_scale*self.calib_init_slam.position.y) + self.slam_offset_z = (self.calib_init_local.position.z-self.slam_scale*self.calib_init_slam.position.z) + #print('offset', self.slam_offset_x, self.slam_offset_y, self.slam_offset_z) + self.slam_calibrated = True + print(self.slam_scale) + + + def convertFRD(self, data): + + stage1 = PoseStamped() + stage2 = PoseStamped() + #first, apply camera gimbal rotation + t = TransformStamped() + t.header.frame_id = "camera_link" + t.header.stamp = rospy.Time.now() + t.child_frame_id = "map" + t.transform.translation.x = 0.0 + t.transform.translation.y = 0.0 + t.transform.translation.z = 0.0 + + q = quaternion_from_euler(0, 90*(math.pi/180), 90*(math.pi/180)) + t.transform.rotation.x = q[0] + t.transform.rotation.y = q[1] + t.transform.rotation.z = q[2] + t.transform.rotation.w = q[3] + + stage1 = tf2_geometry_msgs.do_transform_pose(data, t) + #second, convert from x-forward (orb-slam) to y-forward (drone) + + stage2.pose.position.x = stage1.pose.position.x + stage2.pose.position.y = stage1.pose.position.y + stage2.pose.position.z = stage1.pose.position.z + + stage2.pose.orientation = stage1.pose.orientation + stage2.header.frame_id = "map" + + return data + #convert to px4 coord system (this is working Dec 15th) + + #transform = self.tfBuffer.lookup_transform('cam_odometry', 'camera_link', rospy.Time(0),rospy.Duration(1.0)); + #print(transform) + #converted = tf2_geometry_msgs.do_transform_pose(data, transform) + #converted.header.frame_id = 'cam_odometry' + #return data + + def imageBuffer(self, data): + buffer_size = 50 + if (len(self.image_buffer) > buffer_size): + self.image_buffer.pop() + self.image_buffer.insert(0,data) + + def keyframesCallback(self,data): + #TODO not currently capturing all keyframes + #print(len(data.keyframes),len(self.keyframes)) + #check for new keyframes + data = list(data.keyframes) + if (len(self.keyframes) == 0): + for kf in data: + self.addKeyFrame(kf) + else: + #check for removed keyframes + for old_kf in self.keyframes: + found = False + for new_kf in data: + if (old_kf.image_id == new_kf.header.stamp.to_sec()): + found = True + break + #print(found) + if (not found): + self.removeKeyFrame(old_kf) + #Add new kfs + latest = self.keyframes[-1].image_id + for new_kf in data: + if (new_kf.header.stamp.to_sec() > latest): + self.addKeyFrame(new_kf) + + def removeKeyFrame(self, data): + #print('remove frame '+data.image) + self.keyframes.remove(data) + if os.path.exists(data.image): + os.remove(data.image) + + def addKeyFrame(self, data): + kf = KeyFrame() + found = False + for img in self.image_buffer: + #print(img.header.stamp, data.header.stamp) + if (img.header.stamp.to_sec() == data.header.stamp.to_sec()): + #print('add frame') + kf.image_id = data.header.stamp.to_sec() + cv_img = self.bridge.imgmsg_to_cv2(img, "passthrough") + kf.image = 'img/'+str(kf.image_id)+'.png' + cv2.imwrite(kf.image, cv_img) + self.keyframes.append(kf) + found = True + return + print('Image not found, consider increasing image buffer size') + + def pointcloudCallback(self, data): + self.pointcloud = data + + t = TransformStamped() + t.header.frame_id = "camera_link" + t.header.stamp = rospy.Time.now() + t.child_frame_id = "map" + t.transform.translation.x = self.slam_offset_x + t.transform.translation.y = self.slam_offset_y + t.transform.translation.z = 0.0 + + q = quaternion_from_euler(0, 90*(math.pi/180), 90*(math.pi/180)) + t.transform.rotation.x = q[0] + t.transform.rotation.y = q[1] + t.transform.rotation.z = q[2] + t.transform.rotation.w = q[3] + + cloud_out = tf2_sensor_msgs.do_transform_cloud(data, t) + cloud_out.header.frame_id = 'base_link' + + self.scale_pub.publish(self.slam_scale) + self.cloud_pub.publish(cloud_out) + + + def observationsCallback(self, data): + data = list(data.observations) + #print(self.pointcloud.width, len(data)) + + +if __name__ == '__main__': + companion() + diff --git a/auto/python/stereo.py b/auto/python/stereo.py new file mode 100644 index 0000000000..486858d908 --- /dev/null +++ b/auto/python/stereo.py @@ -0,0 +1,129 @@ +#!/usr/bin/env python + +# Python 2/3 compatibility +from __future__ import print_function + +import numpy as np +import cv2 + +import struct + +import rospy +from sensor_msgs import point_cloud2 +from sensor_msgs.msg import Image,CameraInfo, PointCloud2, PointField +from std_msgs.msg import Header + +from cv_bridge import CvBridge, CvBridgeError + +ply_header = '''ply +format ascii 1.0 +element vertex %(vert_num)d +property float x +property float y +property float z +property uchar red +property uchar green +property uchar blue +end_header +''' + +class Stereo(): + + def depthCallback(self,data): + try: + self.depth_image = self.bridge.imgmsg_to_cv2(data, "bgr8") + except CvBridgeError as e: + print(e) + + def colorCallback(self,data): + try: + self.color_image = self.bridge.imgmsg_to_cv2(data, "bgr8") + except CvBridgeError as e: + print(e) + + def infoCallback(self, data): + self.img_height = data.height + self.img_width = data.width + self.frame = data.header.frame_id + self.time = data.header.stamp + self.D = np.reshape(data.D,(1,5)) + self.K = np.reshape(data.K,(3,3)) + + def array2pointcloud(self, xyz_pts, colors, stamp=None, frame_id=None): + points = [] + for i in range(xyz_pts.shape[0]): + rgb = struct.unpack('I', struct.pack('BBBB', colors[i,0], colors[i,1], colors[i,2], 255))[0] + pt = [xyz_pts[i,0],xyz_pts[i,1],xyz_pts[i,2], rgb] + points.append(pt) + + fields = [PointField('x', 0, PointField.FLOAT32, 1), + PointField('y', 4, PointField.FLOAT32, 1), + PointField('z', 8, PointField.FLOAT32, 1), + # PointField('rgb', 12, PointField.UINT32, 1), + PointField('rgba', 12, PointField.UINT32, 1), + ] + + header = Header() + header.frame_id = frame_id + header.stamp = stamp + pc2 = point_cloud2.create_cloud(header, fields, points) + self.pc_pub.publish(pc2) + + + def __init__(self): + + rospy.init_node('stereo', anonymous=True) + + self.bridge = CvBridge() + self.color_sub = rospy.Subscriber("airsim/Drone1/color/image_raw",Image,self.colorCallback) + self.depth_sub = rospy.Subscriber("airsim/Drone1/depth/image_raw",Image,self.depthCallback) + self.info_sub = rospy.Subscriber("airsim/Drone1/depth",CameraInfo,self.infoCallback) + self.pc_pub = rospy.Publisher("airsim/Drone1/stereo", PointCloud2, queue_size=2) + self.color_image = np.array(0) + self.depth_image = np.array(0) + self.img_height = 0 + self.img_width = 0 + self.time = 0 + self.frame = "" + self.D = np.array(0) + self.K = np.array(0) + + rospy.wait_for_message('/airsim/Drone1/depth/image_raw',Image) + print('got depth image') + + rospy.wait_for_message('/airsim/Drone1/color/image_raw',Image) + print('got color image') + + rospy.wait_for_message('/airsim/Drone1/depth', CameraInfo) + print('got camera info') + + rate = rospy.Rate(10) # 30hz + h, w = self.img_height, self.img_width + f = self.K[0,0] #focal length + + Q = np.array([ [0,-1,0, -320], + [1,0,0, -240], + [0,0,1, 204], + [0,0,33, 0]]) + + while(True): + + disp = np.array(cv2.cvtColor(self.depth_image, cv2.COLOR_BGR2GRAY)).astype(np.float32) / 16.0 + points = cv2.reprojectImageTo3D(disp, Q) + colors = cv2.cvtColor(self.color_image, cv2.COLOR_BGR2RGB) + mask = disp > disp.min() + out_points = points[mask] + out_colors = colors[mask] + + self.array2pointcloud(out_points, out_colors, self.time, self.frame) + + #cv2.imshow('left', self.left_image) + cv2.imshow('disparity', (disp)) + cv2.waitKey(1) + rate.sleep() + + +if __name__ == '__main__': + Stereo() + exit() + diff --git a/docs/build_linux.md b/docs/build_linux.md index 37519a7daf..473d25cca2 100644 --- a/docs/build_linux.md +++ b/docs/build_linux.md @@ -95,6 +95,7 @@ Alternatively, you can use [APIs](apis.md) for programmatic control or use the s ``` * If this is the case then look for *.gch file(s) that follows after that message, delete them and try again. Here's [relevant thread](https://answers.unrealengine.com/questions/412349/linux-ue4-build-precompiled-header-fatal-error.html) on Unreal Engine forums. * If you see other compile errors in console then open up those source files and see if it is due to changes you made. If not, then report it as issue on GitHub. + * Additionally, if you are having trouble rebuilding the plugin in UE4 after making changes to the AirSim library, you may need to manually delete the binaries and intermediate folders in both your Unreal Project folder, and in the UnrealProject/Plugin/AirSim folder. This will force clang to recompile both the UE4 project and plugin and may resolve issues of "clang could not find file x" - Unreal crashed! How do I know what went wrong? * Go to the `MyUnrealProject/Saved/Crashes` folder and search for the file `MyProject.log` within its subdirectories. At the end of this file you will see the stack trace and messages. diff --git a/settings.json b/settings.json new file mode 100644 index 0000000000..3c23354e66 --- /dev/null +++ b/settings.json @@ -0,0 +1,109 @@ +{ + "SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md", + "SettingsVersion": 1.2, + "SimMode": "Multirotor", + "ViewMode": "SpringArmChase", + "ClockSpeed": 1.0, + "Vehicles": { + "base_link": { + "VehicleType": "PX4Multirotor", + "UseSerial": false, + "UseTcp": true, + "LocalHostIp": "127.0.0.1", + "TcpPort": 4560, + "ControlPort": 14580, + "EnableCollisions": true, + "AllowAPIAlways": true, + "RC": { + "RemoteControlID": 0, + "AllowAPIWhenDisconnected": false + }, + "X": 0, "Y": 0, "Z": 0.0, + "Sensors": { + "barometer" : { + "SensorType": 1, + "Enabled": true + }, + "imu" : { + "SensorType": 2, + "Enabled": true + }, + "gps" : { + "SensorType": 3, + "Enabled": true + }, + "magnetometer": { + "SensorType": 4, + "Enabled": true + }, + "distance": { + "SensorType": 5, + "Enabled" : true, + "Noise": 0.02, + "MaxDistance": 8.0, + "MinDistance": 0.17, + "X": -0.06, "Y": 0, "Z": -0.025, + "Roll": 0, "Pitch": 0, "Yaw":0 + } + }, + "Cameras": { + "camera": { + "CaptureSettings": [ + { + "PublishToRos": 1, + "ImageType": 0, + "Width": 640, + "Height": 480, + "FOV_Degrees": 100 + } + ], + "X": 0.33, "Y": 0, "Z": 0.07, + "Roll": 0, "Pitch": -90, "Yaw": 0 + } + } + } + }, + "OriginGeopoint": { + "Latitude": 40, + "Longitude": 100, + "Altitude": 122 + }, + "SubWindows": [ + {"WindowID": 0, "ImageType": 0, "CameraName": "main", "Visible": false}, + {"WindowID": 1, "ImageType": 0, "CameraName": "left", "Visible": false}, + {"WindowID": 2, "ImageType": 0, "CameraName": "right", "Visible": false} + ] +} + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +