Fix first round of merge conflicts

Author: jpanikulam

CMakeLists.txt

@@ -9,6 +9,7 @@ FIND_PACKAGE(crossguid REQUIRED)
 
 # CMakeLists
 find_package(OpenCV REQUIRED)
+include_directories(${OpenCV_INCLUDE_DIRS})
 
 SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/run)
 
@@ -29,7 +30,7 @@ if(ret EQUAL "1")
 endif()
 
 set(CMAKE_CXX_COMPILER "g++-7")
-set(CMAKE_CXX_FLAGS "--std=c++17 -g -O1 -fno-omit-frame-pointer -Wall -Wno-deprecated-declarations ${CMAKE_CXX_FLAGS}")
+set(CMAKE_CXX_FLAGS "--std=c++17 -g -O1 -fno-omit-frame-pointer -Wall -Wno-deprecated-declarations -Wno-psabi ${CMAKE_CXX_FLAGS}")
 
 
 # Wait! Don't edit this file! Use pymake of updating this cmake.

config/fiducial_map/read_fiducial_map.cc

@@ -3,14 +3,13 @@
 
 namespace jet {
 
-
-fiducial_pose get_fiducial_pose() {
+FiducialDescription get_fiducial_pose() {
   YAML::Node config = YAML::LoadFile("/jet/config/fiducial_map/fiducial_map.yaml");
   auto const node = config["fiducial_5x5"];
 
-  fiducial_pose result;
+  FiducialDescription result;
   const auto log_translation_tag_from_world = node["log_translation_tag_from_world"].as<std::vector<double>>();
-  const auto log_rotation_tag_from_world =    node["log_rotation_tag_from_world"].as<std::vector<double>>();
+  const auto log_rotation_tag_from_world = node["log_rotation_tag_from_world"].as<std::vector<double>>();
 
   result.tag_from_world =
       SE3(SO3::exp(jcc::Vec3(
@@ -24,18 +23,18 @@ fiducial_pose get_fiducial_pose() {
   return result;
 }
 
-
-camera_extrinsics_struct get_camera_extrinsics() {
+CameraExtrinsics get_camera_extrinsics() {
   YAML::Node node = YAML::LoadFile("/jet/config/camera_extrinsics.yaml");
 
-  camera_extrinsics_struct result;
+  CameraExtrinsics result;
   const auto log_translation_camera_from_frame = node["log_translation_camera_from_frame"].as<std::vector<double>>();
-  const auto log_rotation_camera_from_frame =    node["log_rotation_camera_from_frame"].as<std::vector<double>>();
+  const auto log_rotation_camera_from_frame = node["log_rotation_camera_from_frame"].as<std::vector<double>>();
   result.camera_from_frame =
       SE3(SO3::exp(jcc::Vec3(
               log_rotation_camera_from_frame[0], log_rotation_camera_from_frame[1], log_rotation_camera_from_frame[2])),
-          jcc::Vec3(
-              log_translation_camera_from_frame[0], log_translation_camera_from_frame[1], log_translation_camera_from_frame[2]));
+          jcc::Vec3(log_translation_camera_from_frame[0],
+                    log_translation_camera_from_frame[1],
+                    log_translation_camera_from_frame[2]));
   return result;
 }
 

config/fiducial_map/read_fiducial_map.hh

@@ -8,21 +8,21 @@
 namespace jet {
 
 
-struct fiducial_pose
+struct FiducialDescription
 {
     SE3 tag_from_world;
     int tag_size_squares;
     double arcode_width_mm;
     double arcode_gap_mm;
 };
 
-struct camera_extrinsics_struct
+struct CameraExtrinsics
 {
     SE3 camera_from_frame;
 };
 
-fiducial_pose get_fiducial_pose();
-camera_extrinsics_struct get_camera_extrinsics();
+FiducialDescription get_fiducial_pose();
+CameraExtrinsics get_camera_extrinsics();
 
 }  //  namespace jet
 

config/hoverjet/controller.yaml

@@ -0,0 +1,9 @@
+k_proportional:
+  - 0.2
+  - 0.2
+  - 0.4
+
+k_derivative:
+  - 0.2
+  - 0.2
+  - 0.4

config/hoverjet/filter.yaml

@@ -0,0 +1,106 @@
+use_accelerometer: false
+use_gyro: true
+use_imus:
+  - 78
+use_fiducial: true
+max_fiducial_latency_s: 0.4
+
+transforms:
+  - source: imu_36
+    destination: camera
+    source_from_destination:
+      log_R_source_from_destination:
+        - -1.616591025757221
+        - 0.05134578843446101
+        - 0.05091124055477719
+      translation_source_from_destination:
+        - -0.001292535846636506
+        - -0.0009267741933531443
+        - -0.0199366607203838
+  - source_from_destination:
+      log_R_source_from_destination:
+        - -2.207252187700393
+        - 2.221627059568664
+        - 0.0729343244927283
+      translation_source_from_destination:
+        - -0.1457315425013999
+        - 0.07004447260546187
+        - -0.2286505835933628
+    destination: camera
+    source: imu_78
+  - destination: camera
+    source_from_destination:
+      log_R_source_from_destination:
+        - 1.209199576156145
+        - 1.209199576156145
+        - 1.209199576156145
+      translation_source_from_destination:
+        - 0.11
+        - 0
+        - 0.32
+    source: vehicle
+imus:
+  36:
+    source_log: /jet/logs/calibration-log-jul6-7
+    accelerometer_gains_p0:
+      - 0
+      - 0
+      - 0
+    magnetometer_gains_p0:
+      - -79.29922702237944
+      - -4.476413645049656
+      - -46.83316633469963
+    id: 36
+    magnetometer_gains_scaling:
+      - 32.1877202815169
+      - 3.408923758371102
+      - 10.62688468310214
+      - 0
+      - 33.17611158621461
+      - -13.05820640963059
+      - 0
+      - 0
+      - 34.80217509591158
+    name: imu_36
+    accelerometer_gains_scaling:
+      - 9.480392053156724
+      - -0.367804276102152
+      - 0.1097718806953435
+      - 0
+      - 9.392748712511086
+      - -0.223199776102899
+      - 0
+      - 0
+      - 9.674406404535416
+  78:
+    magnetometer_gains_scaling:
+      - 27.28940423457934
+      - -8.791590927283378
+      - 2.517804983200733
+      - 0
+      - 32.00283422687422
+      - 2.359513709031368
+      - 0
+      - 0
+      - 27.18480244848202
+    name: imu_78
+    accelerometer_gains_p0:
+      - 0
+      - 0
+      - 0
+    id: 78
+    accelerometer_gains_scaling:
+      - 9.699410372939981
+      - -1.712699829531295
+      - -0.3186110085605555
+      - 0
+      - 9.844189129580382
+      - -0.1136357460553914
+      - 0
+      - 0
+      - 8.810237581896597
+    magnetometer_gains_p0:
+      - 45.59041987760968
+      - 53.1135550856215
+      - 21.82576956097864
+    source_log: /jet/logs/calibration-log-jul6-7

config/supervisor/supervisor.conf

@@ -76,14 +76,14 @@ stopsignal=INT
 priority=2
 
 [program:pose_filter]
-command=/jet/bin/run/filter_balsaq_main
+command=/jet/bin/run/filter_balsaq_main /jet/config/hoverjet/filter.yaml
 stderr_logfile = /var/log/supervisord/filter-stderr.log
 stdout_logfile = /var/log/supervisord/filter-stdout.log
 stopsignal=INT
 priority=1
 
 [program:controller]
-command=/jet/bin/run/controller_balsaq_main
+command=/jet/bin/run/controller_balsaq_main /jet/config/hoverjet/controller.yaml
 stderr_logfile = /var/log/supervisord/controller-stderr.log
 stdout_logfile = /var/log/supervisord/controller-stdout.log
 stopsignal=INT

control/controller_balsaq.cc

@@ -7,35 +7,38 @@
 #include "control/control_utilities.hh"
 #include "control/jet_vane_mapper.hh"
 #include "control/servo_interface.hh"
+
 #include "filtering/pose_message.hh"
+#include "filtering/yaml_matrix.hh"
+
 #include "infrastructure/balsa_queue/bq_main_macro.hh"
 #include "infrastructure/engine/turbine_state_message.hh"
 
-#include "third_party/experiments/estimation/time_point.hh"
-
 namespace jet {
 namespace control {
 
 namespace {
 
-estimation::TimePoint to_time_point(const Timestamp& ts) {
-  const auto epoch_offset = std::chrono::nanoseconds(uint64_t(ts));
-  const estimation::TimePoint time_point = estimation::TimePoint{} + epoch_offset;
-  return time_point;
-}
-
+/*
 jcc::Vec3 sigmoid(const jcc::Vec3& v) {
   const double v_nrm = v.norm();
 
   const double interp_value = (1.0 / (1.0 + std::exp(-v_nrm)));
 
   return interp_value * v.normalized();
 }
+*/
 
-QuadraframeStatus generate_control(const SO3& world_from_target, const Pose& pose, const JetStatus& jet_status) {
+QuadraframeStatus generate_control(const SO3& world_from_target,
+                                   const Pose& pose,
+                                   const JetStatus& jet_status,
+                                   const GainConfig& cfg) {
   JetVaneMapper mapper_;
 
-  const MatNd<3, 3> K = jcc::Vec3(0.3, 0.3, 0.4).asDiagonal();
+  const jcc::Vec3 w_jet_frame = pose.world_from_jet.inverse() * pose.w_world_frame;
+
+  const MatNd<3, 3> Kp = cfg.k_proportional.asDiagonal();
+  const MatNd<3, 3> Kd = cfg.k_derivative.asDiagonal();
 
   //
   // Compute the current expected jet force (All servos zero'd)
@@ -53,7 +56,7 @@ QuadraframeStatus generate_control(const SO3& world_from_target, const Pose& pos
   const jcc::Vec3 desired_force_jet_frame = wrench_for_zero.force_N;
 
   const SO3 target_from_jet = world_from_target.inverse() * pose.world_from_jet.so3();
-  const jcc::Vec3 desired_torque_jet_frame = -K * target_from_jet.log();
+  const jcc::Vec3 desired_torque_jet_frame = -(Kp * target_from_jet.log()) + -(Kd * w_jet_frame);
 
   Wrench target_wrench;
   target_wrench.torque_Nm = desired_torque_jet_frame;
@@ -99,6 +102,9 @@ void ControllerBq::init(const Config& config) {
   std::cout << "Subscribing Turbine State" << std::endl;
   turbine_state_sub_ = make_subscriber("turbine_state");
 
+  read_matrix(config["k_proportional"], gain_cfg_.k_proportional);
+  read_matrix(config["k_derivative"], gain_cfg_.k_derivative);
+
   std::cout << "Controller starting" << std::endl;
 }
 
@@ -125,7 +131,7 @@ void ControllerBq::loop() {
 
   if (got_pose_msg && got_turbine_status_) {
     const Pose pose = pose_msg.to_pose();
-    const auto target_qframe_status = generate_control(target_from_world, pose, jet_status_);
+    const auto target_qframe_status = generate_control(target_from_world, pose, jet_status_, gain_cfg_);
     SetServoMessage servo_message = create_servo_command(target_qframe_status);
     servo_pub_->publish(servo_message);
   }

control/controller_balsaq.hh

@@ -8,6 +8,11 @@
 namespace jet {
 namespace control {
 
+struct GainConfig {
+  jcc::Vec3 k_proportional;
+  jcc::Vec3 k_derivative;
+};
+
 class ControllerBq : public BalsaQ {
  public:
   ControllerBq();
@@ -19,6 +24,8 @@ class ControllerBq : public BalsaQ {
   const static uint loop_delay_microseconds = 10000;
 
  private:
+  GainConfig gain_cfg_;
+
   SubscriberPtr roll_sub_;
   SubscriberPtr pitch_sub_;
   SubscriberPtr yaw_sub_;

control/jet_vane_model.hh

@@ -24,7 +24,8 @@ struct VaneConfiguration {
   SE3 T_vane_unit_from_vane_default = detail::compute_T_vane_unit_from_vane_default();
   double offset_vane_from_servo_rad = 0.0;
 
-  double gear_ratio_servo_from_vane = 5.33;
+  // double gear_ratio_servo_from_vane = 5.33;
+  double gear_ratio_servo_from_vane = 1.0;
   double max_servo_angle_rad = 1.116;
   double min_servo_angle_rad = -1.116;
 

filtering/calibrate_camera_from_log.cc

@@ -81,7 +81,9 @@ void calibrate_camera(const std::string& log_path) {
       break;
     }
 
-    const auto obj_pt_associations = obj_points_img_points_from_image(image->image);
+
+    const auto ids_corners = get_ids_and_corners(image->image);
+    const auto obj_pt_associations = obj_points_img_points_from_image(ids_corners);
 
     serial_number = image->serial_number;
     cols = image->image.cols;
@@ -130,13 +132,14 @@ void calibrate_camera(const std::string& log_path) {
   jcc::Warning() << "Calibrating camera, this might take a while..." << std::endl;
   int flag = 0;
   const auto t0 = jcc::now();
-  cv::calibrateCamera(object_points, image_points, cv::Size(480, 270), K, D, rvecs, tvecs, flag);
+  cv::Size resolution(480, 270);
+  cv::calibrateCamera(object_points, image_points, resolution, K, D, rvecs, tvecs, flag);
   const auto t1 = jcc::now();
   jcc::Success() << "Done. Took: " << estimation::to_seconds(t1 - t0) << std::endl;
   jcc::Success() << "Generating yaml..." << std::endl;
 
-  YAML::Node node;                        // starts out as null
-  node["serial_number"] = serial_number;  // it now is a map node
+  YAML::Node node;
+  node["serial_number"] = serial_number;
 
   {
     std::stringstream ss;