Feature: add incremental encoder

march_hardware/include/march_hardware/Joint.h

@@ -79,7 +79,6 @@ class Joint
 
   void setName(const std::string& name);
   void setAllowActuation(bool allowActuation);
-  void setIMotionCube(const IMotionCube& iMotionCube);
   void setTemperatureGes(const TemperatureGES& temperatureGes);
   void setNetNumber(int netNumber);
 };

march_hardware/include/march_hardware/encoder/IncrementalEncoder.h

@@ -11,7 +11,7 @@ namespace march
 class IncrementalEncoder : public Encoder
 {
 public:
-  explicit IncrementalEncoder(size_t number_of_bits);
+  IncrementalEncoder(size_t number_of_bits, double transmission);
 
   double getAngleRad(uint8_t byte_offset);
 
@@ -28,6 +28,9 @@ class IncrementalEncoder : public Encoder
     return os << "slaveIndex: " << encoder.getSlaveIndex() << ", "
               << "totalPositions: " << encoder.getTotalPositions();
   }
+
+private:
+  const double transmission_;
 };
 }  // namespace march
 

march_hardware/src/Joint.cpp

@@ -150,10 +150,7 @@ void Joint::setAllowActuation(bool allowActuation)
 {
   Joint::allowActuation = allowActuation;
 }
-void Joint::setIMotionCube(const IMotionCube& iMotionCube)
-{
-  Joint::iMotionCube = iMotionCube;
-}
+
 void Joint::setTemperatureGes(const TemperatureGES& temperatureGes)
 {
   temperatureGES = temperatureGes;

march_hardware/src/encoder/IncrementalEncoder.cpp

@@ -5,7 +5,8 @@
 
 namespace march
 {
-IncrementalEncoder::IncrementalEncoder(size_t number_of_bits) : Encoder(number_of_bits)
+IncrementalEncoder::IncrementalEncoder(size_t number_of_bits, double transmission)
+  : Encoder(number_of_bits), transmission_(transmission)
 {
 }
 
@@ -16,6 +17,6 @@ double IncrementalEncoder::getAngleRad(uint8_t byte_offset)
 
 double IncrementalEncoder::toRad(int32_t iu)
 {
-  return iu * 2 * M_PI / Encoder::getTotalPositions();
+  return iu * this->transmission_ * 2 * M_PI / Encoder::getTotalPositions();
 }
 }  //  namespace march

march_hardware/test/encoder/TestIncrementalEncoder.cpp

@@ -9,7 +9,8 @@ class TestIncrementalEncoder : public testing::Test
 {
 protected:
   const size_t resolution = 12;
-  march::IncrementalEncoder encoder = march::IncrementalEncoder(this->resolution);
+  const double transmission = 100;
+  march::IncrementalEncoder encoder = march::IncrementalEncoder(this->resolution, this->transmission);
 };
 
 TEST_F(TestIncrementalEncoder, ZeroIUToRad)
@@ -20,6 +21,6 @@ TEST_F(TestIncrementalEncoder, ZeroIUToRad)
 TEST_F(TestIncrementalEncoder, CorrectToRad)
 {
   const int32_t iu = 1000;
-  const double expected = iu * 2.0 * M_PI / std::pow(2, this->resolution);
+  const double expected = iu * this->transmission * 2.0 * M_PI / std::pow(2, this->resolution);
   ASSERT_EQ(expected, this->encoder.toRad(iu));
 }

march_hardware/test/mocks/MockIncrementalEncoder.cpp

@@ -5,7 +5,7 @@
 class MockIncrementalEncoder : public march::IncrementalEncoder
 {
 public:
-  MockIncrementalEncoder() : IncrementalEncoder(10)
+  MockIncrementalEncoder() : IncrementalEncoder(10, 100.0)
   {
   }
 

march_hardware_builder/robots/march4.yaml

@@ -15,6 +15,7 @@ march4:
             maxPositionIU: 4091
           incrementalEncoder:
             resolution: 13
+            transmission: 200
 
     - left_hip_aa:
         actuationMode: torque
@@ -29,6 +30,7 @@ march4:
             maxPositionIU: 1426
           incrementalEncoder:
             resolution: 13
+            transmission: 200
 
     - right_hip_fe:
         actuationMode: torque
@@ -43,6 +45,7 @@ march4:
             maxPositionIU: 45617
           incrementalEncoder:
             resolution: 12
+            transmission: 101
 
     - left_hip_fe:
         actuationMode: torque
@@ -57,6 +60,7 @@ march4:
             maxPositionIU: 40270
           incrementalEncoder:
             resolution: 12
+            transmission: 101
 
     - right_knee:
         actuationMode: torque
@@ -71,6 +75,7 @@ march4:
             maxPositionIU: 68874
           incrementalEncoder:
             resolution: 12
+            transmission: 101
 
     - left_knee:
         actuationMode: torque
@@ -85,6 +90,7 @@ march4:
             maxPositionIU: 63543
           incrementalEncoder:
             resolution: 12
+            transmission: 101
 
     - right_ankle:
         actuationMode: torque
@@ -99,6 +105,7 @@ march4:
             maxPositionIU: 1210
           incrementalEncoder:
             resolution: 13
+            transmission: 200
 
     - left_ankle:
         actuationMode: torque
@@ -113,3 +120,4 @@ march4:
             maxPositionIU: 4082
           incrementalEncoder:
             resolution: 13
+            transmission: 200

march_hardware_builder/robots/test_joint_linear.yaml

@@ -13,3 +13,4 @@ testjoint_linear:
            maxPositionIU: 445
          incrementalEncoder:
            resolution: 13
+           transmission: 200

march_hardware_builder/robots/test_joint_rotational.yaml

@@ -13,3 +13,4 @@ testsetup:
             maxPositionIU: 102499
           incrementalEncoder:
             resolution: 12
+            transmission: 101

march_hardware_builder/src/hardware_builder.cpp

@@ -12,7 +12,7 @@ const std::vector<std::string> HardwareBuilder::ABSOLUTE_ENCODER_REQUIRED_KEYS =
     {
         "resolution", "minPositionIU", "maxPositionIU"
     };
-const std::vector<std::string> HardwareBuilder::INCREMENTAL_ENCODER_REQUIRED_KEYS = { "resolution" };
+const std::vector<std::string> HardwareBuilder::INCREMENTAL_ENCODER_REQUIRED_KEYS = { "resolution", "transmission" };
 const std::vector<std::string> HardwareBuilder::IMOTIONCUBE_REQUIRED_KEYS =
     {
         "slaveIndex", "incrementalEncoder", "absoluteEncoder"
@@ -168,7 +168,8 @@ march::IncrementalEncoder HardwareBuilder::createIncrementalEncoder(const YAML::
                                              HardwareBuilder::INCREMENTAL_ENCODER_REQUIRED_KEYS, "incrementalEncoder");
 
   auto resolution = incremental_encoder_config["resolution"].as<size_t>();
-  return march::IncrementalEncoder(resolution);
+  auto transmission = incremental_encoder_config["transmission"].as<double>();
+  return { resolution, transmission };
 }
 
 march::TemperatureGES HardwareBuilder::createTemperatureGES(const YAML::Node& temperature_ges_config)

march_hardware_builder/test/imc_builder_test.cpp

@@ -45,7 +45,7 @@ TEST_F(IMotionCubeTest, ValidIMotionCubeHip)
   march::AbsoluteEncoder absolute_encoder =
       march::AbsoluteEncoder(16, 22134, 43436, this->joint->limits->lower, this->joint->limits->upper,
                              this->joint->safety->soft_lower_limit, this->joint->safety->soft_upper_limit);
-  march::IncrementalEncoder incremental_encoder = march::IncrementalEncoder(12);
+  march::IncrementalEncoder incremental_encoder = march::IncrementalEncoder(12, 101.0);
   march::IMotionCube expected =
       march::IMotionCube(2, absolute_encoder, incremental_encoder, march::ActuationMode::unknown);
 

march_hardware_builder/test/incremental_encoder_builder_test.cpp

@@ -29,14 +29,21 @@ TEST_F(TestIncrementalEncoderBuilder, ValidIncrementalEncoder)
 {
   YAML::Node config = this->loadTestYaml("/incremental_encoder_correct.yaml");
 
-  march::IncrementalEncoder expected = march::IncrementalEncoder(12);
+  march::IncrementalEncoder expected = march::IncrementalEncoder(12, 45.5);
   march::IncrementalEncoder created = HardwareBuilder::createIncrementalEncoder(config);
   ASSERT_EQ(expected, created);
 }
 
 TEST_F(TestIncrementalEncoderBuilder, NoResolution)
 {
-  YAML::Node config;
+  YAML::Node config = this->loadTestYaml("/incremental_encoder_no_resolution.yaml");
+
+  ASSERT_THROW(HardwareBuilder::createIncrementalEncoder(config), MissingKeyException);
+}
+
+TEST_F(TestIncrementalEncoderBuilder, NoTransmission)
+{
+  YAML::Node config = this->loadTestYaml("/incremental_encoder_no_transmission.yaml");
 
   ASSERT_THROW(HardwareBuilder::createIncrementalEncoder(config), MissingKeyException);
 }

march_hardware_builder/test/joint_builder_test.cpp

@@ -45,7 +45,7 @@ TEST_F(JointTest, ValidJointHip)
   march::AbsoluteEncoder absolute_encoder =
       march::AbsoluteEncoder(16, 22134, 43436, this->joint->limits->lower, this->joint->limits->upper,
                              this->joint->safety->soft_lower_limit, this->joint->safety->soft_upper_limit);
-  march::IncrementalEncoder incremental_encoder = march::IncrementalEncoder(12);
+  march::IncrementalEncoder incremental_encoder = march::IncrementalEncoder(12, 50.0);
   march::IMotionCube imc = march::IMotionCube(2, absolute_encoder, incremental_encoder, march::ActuationMode::unknown);
   march::TemperatureGES ges = march::TemperatureGES(1, 2);
   march::Joint expected(imc);
@@ -70,7 +70,7 @@ TEST_F(JointTest, ValidNotActuated)
   march::AbsoluteEncoder absolute_encoder =
       march::AbsoluteEncoder(16, 22134, 43436, this->joint->limits->lower, this->joint->limits->upper,
                              this->joint->safety->soft_lower_limit, this->joint->safety->soft_upper_limit);
-  march::IncrementalEncoder incremental_encoder = march::IncrementalEncoder(12);
+  march::IncrementalEncoder incremental_encoder = march::IncrementalEncoder(12, 50.0);
   march::IMotionCube imc = march::IMotionCube(2, absolute_encoder, incremental_encoder, march::ActuationMode::unknown);
   march::TemperatureGES ges = march::TemperatureGES(1, 2);
   march::Joint expected(imc);
@@ -122,7 +122,7 @@ TEST_F(JointTest, ValidActuationMode)
       1,
       march::AbsoluteEncoder(16, 22134, 43436, this->joint->limits->lower, this->joint->limits->upper,
                              this->joint->safety->soft_lower_limit, this->joint->safety->soft_upper_limit),
-      march::IncrementalEncoder(12), march::ActuationMode::position));
+      march::IncrementalEncoder(12, 50.0), march::ActuationMode::position));
   expected.setName("test_joint_hip");
 
   ASSERT_EQ("test_joint_hip", expected.getName());

march_hardware_builder/test/yaml/encoder/incremental_encoder_correct.yaml

@@ -1 +1,2 @@
-resolution: 12
+resolution: 12
+transmission: 45.5

march_hardware_builder/test/yaml/encoder/incremental_encoder_no_resolution.yaml

@@ -0,0 +1 @@
+transmission: 45.5

march_hardware_builder/test/yaml/encoder/incremental_encoder_no_transmission.yaml

@@ -0,0 +1 @@
+resolution: 13

march_hardware_builder/test/yaml/imotioncube/imotioncube_correct.yaml

@@ -7,3 +7,4 @@ absoluteEncoder:
   safetyMarginRad: 0.05
 incrementalEncoder:
   resolution: 12
+  transmission: 101

march_hardware_builder/test/yaml/imotioncube/imotioncube_no_absolute_encoder.yaml

@@ -1,3 +1,4 @@
 slaveIndex: 2
 incrementalEncoder:
   resolution: 12
+  transmission: 100

march_hardware_builder/test/yaml/imotioncube/imotioncube_no_slave_index.yaml

@@ -4,3 +4,4 @@ absoluteEncoder:
   maxPositionIU: 43436
 incrementalEncoder:
   resolution: 12
+  transmission: 75.1

march_hardware_builder/test/yaml/joint/joint_correct.yaml

@@ -7,6 +7,7 @@ imotioncube:
     maxPositionIU: 43436
   incrementalEncoder:
     resolution: 12
+    transmission: 50.0
 temperatureges:
   slaveIndex: 1
   byteOffset: 2

march_hardware_builder/test/yaml/joint/joint_correct_not_actuated.yaml

@@ -7,6 +7,7 @@ imotioncube:
     maxPositionIU: 43436
   incrementalEncoder:
     resolution: 12
+    transmission: 50.0
 temperatureges:
   slaveIndex: 1
   byteOffset: 2

march_hardware_builder/test/yaml/joint/joint_correct_position_mode.yaml

@@ -8,3 +8,4 @@ imotioncube:
     maxPositionIU: 43436
   incrementalEncoder:
     resolution: 12
+    transmission: 50.0

march_hardware_builder/test/yaml/joint/joint_no_actuate.yaml

@@ -6,6 +6,7 @@ imotioncube:
     maxPositionIU: 43436
   incrementalEncoder:
     resolution: 12
+    transmission: 14.0
 temperatureges:
   slaveIndex: 1
   byteOffset: 2

march_hardware_builder/test/yaml/joint/joint_no_temperature_ges.yaml

@@ -7,3 +7,4 @@ imotioncube:
     maxPositionIU: 40000
   incrementalEncoder:
     resolution: 12
+    transmission: 45.0