Merge branch 'develop' into feature/PM-325-ethercat-interface

Author: Olav de Haas

march_hardware/include/march_hardware/IMotionCube.h

@@ -49,6 +49,10 @@ class IMotionCube : public Slave
   int16_t getTorque();
   int32_t getAngleIUAbsolute();
   int32_t getAngleIUIncremental();
+  double getVelocityIUAbsolute();
+  double getVelocityIUIncremental();
+  virtual double getVelocityRadAbsolute();
+  virtual double getVelocityRadIncremental();
 
   uint16_t getStatusWord();
   uint16_t getMotionError();

march_hardware/include/march_hardware/IMotionCubeState.h

@@ -134,6 +134,8 @@ struct IMotionCubeState
   float IMCVoltage;
   int absoluteEncoderValue;
   int incrementalEncoderValue;
+  double absoluteVelocity;
+  double incrementalVelocity;
 };
 
 }  // namespace march

march_hardware/include/march_hardware/Joint.h

@@ -60,6 +60,8 @@ class Joint
   int16_t getTorque();
   int32_t getAngleIUAbsolute();
   int32_t getAngleIUIncremental();
+  double getVelocityIUAbsolute();
+  double getVelocityIUIncremental();
   float getTemperature();
   IMotionCubeState getIMotionCubeState();
 
@@ -127,6 +129,8 @@ class Joint
   float previous_motor_current_ = 0.0;
   double previous_absolute_position_ = 0.0;
   double previous_incremental_position_ = 0.0;
+  double previous_absolute_velocity_ = 0.0;
+  double previous_incremental_velocity_ = 0.0;
 
   double position_ = 0.0;
   double incremental_position_ = 0.0;

march_hardware/include/march_hardware/PDOmap.h

@@ -42,13 +42,15 @@ enum class IMCObjectName
 {
   StatusWord,
   ActualPosition,
+  ActualVelocity,
   MotionErrorRegister,
   DetailedErrorRegister,
   DCLinkVoltage,
   DriveTemperature,
   ActualTorque,
   CurrentLimit,
   MotorPosition,
+  MotorVelocity,
   ControlWord,
   TargetPosition,
   TargetTorque,

march_hardware/include/march_hardware/encoder/Encoder.h

@@ -33,6 +33,22 @@ class Encoder
    */
   double getAngleRad(const PdoSlaveInterface& pdo, uint8_t byte_offset) const;
 
+  /**
+   * Reads out the velocity of the encoder from the slave and returns the value in Internal Units per second (IU/s).
+   * @param pdo PDO interface to use for reading
+   * @param byte_offset the byte offset in the slave register for the IU velocity
+   * @returns The current velocity measured by the encoder in Internal Units per second (IU/s)
+   */
+  double getVelocityIU(const PdoSlaveInterface& pdo, uint8_t byte_offset) const;
+
+  /**
+   * Reads out the velocity of the encoder from the slave and transforms the result to radians per second.
+   * @param pdo PDO interface to use for reading
+   * @param byte_offset the byte offset in the slave register for the IU velocity
+   * @returns The current velocity measured by the encoder in radians per second
+   */
+  double getVelocityRad(const PdoSlaveInterface& pdo, uint8_t byte_offset) const;
+
   /**
    * Converts encoder Internal Units (IU) to radians.
    * This is a pure virtual function and must be implemented by subclasses,
@@ -52,6 +68,12 @@ class Encoder
   static const size_t MIN_RESOLUTION = 1;
   static const size_t MAX_RESOLUTION = 32;
 
+  // constant used for converting a fixed point 16.16 bit number to a double, which is done by dividing by 2^16
+  static constexpr double FIXED_POINT_TO_FLOAT_CONVERSION = 1 << 16;
+
+  // iMOTIONCUBE setting (slow loop sample period)
+  static constexpr double TIME_PER_VELOCITY_SAMPLE = 0.004;
+
   static constexpr double PI_2 = 2 * M_PI;
 
 private:

march_hardware/src/IMotionCube.cpp

@@ -63,6 +63,8 @@ void IMotionCube::mapMisoPDOs(SdoSlaveInterface& sdo)
   map_miso.addObject(IMCObjectName::DetailedErrorRegister);
   map_miso.addObject(IMCObjectName::DCLinkVoltage);
   map_miso.addObject(IMCObjectName::MotorPosition);
+  map_miso.addObject(IMCObjectName::MotorVelocity);
+  map_miso.addObject(IMCObjectName::ActualVelocity);
   this->miso_byte_offsets_ = map_miso.map(sdo, DataDirection::MISO);
 }
 
@@ -237,6 +239,26 @@ int IMotionCube::getAngleIUIncremental()
   return this->incremental_encoder_->getAngleIU(*this, this->miso_byte_offsets_.at(IMCObjectName::MotorPosition));
 }
 
+double IMotionCube::getVelocityIUAbsolute()
+{
+  return this->absolute_encoder_->getVelocityIU(*this, this->miso_byte_offsets_.at(IMCObjectName::ActualVelocity));
+}
+
+double IMotionCube::getVelocityIUIncremental()
+{
+  return this->incremental_encoder_->getVelocityIU(*this, this->miso_byte_offsets_.at(IMCObjectName::MotorVelocity));
+}
+
+double IMotionCube::getVelocityRadAbsolute()
+{
+  return this->absolute_encoder_->getVelocityRad(*this, this->miso_byte_offsets_.at(IMCObjectName::ActualVelocity));
+}
+
+double IMotionCube::getVelocityRadIncremental()
+{
+  return this->incremental_encoder_->getVelocityRad(*this, this->miso_byte_offsets_.at(IMCObjectName::MotorVelocity));
+}
+
 uint16_t IMotionCube::getStatusWord()
 {
   return this->read16(this->miso_byte_offsets_.at(IMCObjectName::StatusWord)).ui;

march_hardware/src/Joint.cpp

@@ -96,27 +96,21 @@ void Joint::readEncoders(const ros::Duration& elapsed_time)
 
   if (this->receivedDataUpdate())
   {
-    const double new_incremental_position = this->imc_->getAngleRadIncremental();
-    const double new_absolute_position = this->imc_->getAngleRadAbsolute();
+    const double incremental_position_change = this->imc_->getAngleRadIncremental() - this->incremental_position_;
 
-    // Get velocity from encoder position
-    double best_displacement;
-
-    // Take the velocity with the highest resolution.
+    // Take the velocity and position from the encoder with the highest resolution.
     if (this->imc_->getIncrementalRadPerBit() < this->imc_->getAbsoluteRadPerBit())
     {
-      best_displacement = new_incremental_position - this->incremental_position_;
+      this->velocity_ = this->imc_->getVelocityRadIncremental();
+      this->position_ += incremental_position_change;
     }
     else
     {
-      best_displacement = new_absolute_position - this->absolute_position_;
+      this->velocity_ = this->imc_->getVelocityRadAbsolute();
+      this->position_ = this->imc_->getAngleRadAbsolute();
     }
-
-    // Update position with the most accurate velocity
-    this->position_ += best_displacement;
-    this->velocity_ = best_displacement / elapsed_time.toSec();
-    this->incremental_position_ = new_incremental_position;
-    this->absolute_position_ = new_absolute_position;
+    this->incremental_position_ += incremental_position_change;
+    this->absolute_position_ = this->imc_->getAngleRadAbsolute();
   }
   else
   {
@@ -187,6 +181,26 @@ int32_t Joint::getAngleIUIncremental()
   return this->imc_->getAngleIUIncremental();
 }
 
+double Joint::getVelocityIUAbsolute()
+{
+  if (!this->hasIMotionCube())
+  {
+    ROS_WARN("[%s] Has no iMotionCube", this->name_.c_str());
+    return -1;
+  }
+  return this->imc_->getVelocityIUAbsolute();
+}
+
+double Joint::getVelocityIUIncremental()
+{
+  if (!this->hasIMotionCube())
+  {
+    ROS_WARN("[%s] Has no iMotionCube", this->name_.c_str());
+    return -1;
+  }
+  return this->imc_->getVelocityIUIncremental();
+}
+
 float Joint::getTemperature()
 {
   if (!this->hasTemperatureGES())
@@ -217,6 +231,8 @@ IMotionCubeState Joint::getIMotionCubeState()
 
   states.absoluteEncoderValue = this->imc_->getAngleIUAbsolute();
   states.incrementalEncoderValue = this->imc_->getAngleIUIncremental();
+  states.absoluteVelocity = this->imc_->getVelocityIUAbsolute();
+  states.incrementalVelocity = this->imc_->getVelocityIUIncremental();
 
   return states;
 }
@@ -275,19 +291,27 @@ bool Joint::receivedDataUpdate()
   {
     return false;
   }
-  // If imc voltage, motor current, and both encoders positions did not change,
+  // If imc voltage, motor current, and both encoders positions and velocities did not change,
   // we probably did not receive an update for this joint.
   float new_imc_volt = this->imc_->getIMCVoltage();
   float new_motor_current = this->imc_->getMotorCurrent();
   double new_absolute_position = this->imc_->getAngleRadAbsolute();
   double new_incremental_position = this->imc_->getAngleRadIncremental();
+  double new_absolute_velocity = this->imc_->getVelocityRadAbsolute();
+  double new_incremental_velocity = this->imc_->getVelocityRadIncremental();
+
   bool data_updated = (new_imc_volt != this->previous_imc_volt_ || new_motor_current != this->previous_motor_current_ ||
                        new_absolute_position != this->previous_absolute_position_ ||
-                       new_incremental_position != this->previous_incremental_position_);
+                       new_incremental_position != this->previous_incremental_position_ ||
+                       new_absolute_velocity != this->previous_absolute_velocity_ ||
+                       new_incremental_velocity != this->previous_incremental_velocity_);
+
   this->previous_imc_volt_ = new_imc_volt;
   this->previous_motor_current_ = new_motor_current;
   this->previous_absolute_position_ = new_absolute_position;
   this->previous_incremental_position_ = new_incremental_position;
+  this->previous_absolute_velocity_ = new_absolute_velocity;
+  this->previous_incremental_velocity_ = new_incremental_velocity;
   return data_updated;
 }
 

march_hardware/src/PDOmap.cpp

@@ -12,13 +12,15 @@ namespace march
 std::unordered_map<IMCObjectName, IMCObject> PDOmap::all_objects = {
   { IMCObjectName::StatusWord, IMCObject(0x6041, 16) },
   { IMCObjectName::ActualPosition, IMCObject(0x6064, 32) },
+  { IMCObjectName::ActualVelocity, IMCObject(0x6069, 32) },
   { IMCObjectName::MotionErrorRegister, IMCObject(0x2000, 16) },
   { IMCObjectName::DetailedErrorRegister, IMCObject(0x2002, 16) },
   { IMCObjectName::DCLinkVoltage, IMCObject(0x2055, 16) },
   { IMCObjectName::DriveTemperature, IMCObject(0x2058, 16) },
   { IMCObjectName::ActualTorque, IMCObject(0x6077, 16) },
   { IMCObjectName::CurrentLimit, IMCObject(0x207F, 16) },
   { IMCObjectName::MotorPosition, IMCObject(0x2088, 32) },
+  { IMCObjectName::MotorVelocity, IMCObject(0x2087, 32) },
   { IMCObjectName::ControlWord, IMCObject(0x6040, 16) },
   { IMCObjectName::TargetPosition, IMCObject(0x607A, 32) },
   { IMCObjectName::TargetTorque, IMCObject(0x6071, 16) },

march_hardware/src/encoder/Encoder.cpp

@@ -20,6 +20,17 @@ double Encoder::getAngleRad(const PdoSlaveInterface& pdo, uint8_t byte_offset) c
   return this->toRad(this->getAngleIU(pdo, byte_offset));
 }
 
+double Encoder::getVelocityIU(const PdoSlaveInterface& pdo, uint8_t byte_offset) const
+{
+  bit32 return_byte = pdo.read32(byte_offset);
+  return return_byte.i / (TIME_PER_VELOCITY_SAMPLE * FIXED_POINT_TO_FLOAT_CONVERSION);
+}
+
+double Encoder::getVelocityRad(const PdoSlaveInterface& pdo, uint8_t byte_offset) const
+{
+  return this->getVelocityIU(pdo, byte_offset) * this->getRadPerBit();
+}
+
 size_t Encoder::getTotalPositions() const
 {
   return this->total_positions_;

march_hardware/test/TestJoint.cpp

@@ -203,12 +203,13 @@ TEST_F(JointTest, TestReadEncodersOnce)
 {
   ros::Duration elapsed_time(0.2);
   double velocity = 0.5;
-  double absolute_noise = -2 * this->imc->getAbsoluteRadPerBit();
+  double velocity_with_noise = velocity - 2 * this->imc->getAbsoluteRadPerBit() / elapsed_time.toSec();
 
   double initial_incremental_position = 5;
   double initial_absolute_position = 3;
+
   double new_incremental_position = initial_incremental_position + velocity * elapsed_time.toSec();
-  double new_absolute_position = initial_absolute_position + velocity * elapsed_time.toSec() + absolute_noise;
+  double new_absolute_position = initial_absolute_position + velocity_with_noise * elapsed_time.toSec();
 
   EXPECT_CALL(*this->imc, getIMCVoltage()).WillOnce(Return(48));
   EXPECT_CALL(*this->imc, getMotorCurrent()).WillOnce(Return(5));
@@ -221,14 +222,17 @@ TEST_F(JointTest, TestReadEncodersOnce)
       .WillOnce(Return(new_absolute_position))
       .WillOnce(Return(new_absolute_position));
 
+  EXPECT_CALL(*this->imc, getVelocityRadIncremental()).WillOnce(Return(velocity)).WillOnce(Return(velocity));
+  EXPECT_CALL(*this->imc, getVelocityRadAbsolute()).WillOnce(Return(velocity_with_noise));
+
   march::Joint joint("actuate_true", 0, true, std::move(this->imc));
   joint.prepareActuation();
 
   joint.readEncoders(elapsed_time);
 
   ASSERT_DOUBLE_EQ(joint.getPosition(), initial_absolute_position + velocity * elapsed_time.toSec());
-  ASSERT_DOUBLE_EQ(joint.getVelocity(),
-                   (new_incremental_position - initial_incremental_position) / elapsed_time.toSec());
+  ASSERT_NEAR(joint.getVelocity(), (new_incremental_position - initial_incremental_position) / elapsed_time.toSec(),
+              0.0000001);
 }
 
 TEST_F(JointTest, TestReadEncodersTwice)
@@ -238,13 +242,15 @@ TEST_F(JointTest, TestReadEncodersTwice)
   double second_velocity = 0.8;
 
   double absolute_noise = -this->imc->getAbsoluteRadPerBit();
+  double first_velocity_with_noise = first_velocity + absolute_noise / elapsed_time.toSec();
+  double second_velocity_with_noise = second_velocity + absolute_noise / elapsed_time.toSec();
 
   double initial_incremental_position = 5;
   double initial_absolute_position = 3;
   double second_incremental_position = initial_incremental_position + first_velocity * elapsed_time.toSec();
-  double second_absolute_position = initial_absolute_position + first_velocity * elapsed_time.toSec() + absolute_noise;
+  double second_absolute_position = initial_absolute_position + first_velocity * elapsed_time.toSec();
   double third_incremental_position = second_incremental_position + second_velocity * elapsed_time.toSec();
-  double third_absolute_position = second_absolute_position + second_velocity * elapsed_time.toSec() + absolute_noise;
+  double third_absolute_position = second_absolute_position + second_velocity_with_noise * elapsed_time.toSec();
 
   EXPECT_CALL(*this->imc, getIMCVoltage()).WillOnce(Return(48)).WillOnce(Return(48.01));
   EXPECT_CALL(*this->imc, getAngleRadIncremental())
@@ -259,6 +265,14 @@ TEST_F(JointTest, TestReadEncodersTwice)
       .WillOnce(Return(second_absolute_position))
       .WillOnce(Return(third_absolute_position))
       .WillOnce(Return(third_absolute_position));
+  EXPECT_CALL(*this->imc, getVelocityRadIncremental())
+      .WillOnce(Return(first_velocity))
+      .WillOnce(Return(first_velocity))
+      .WillOnce(Return(second_velocity))
+      .WillOnce(Return(second_velocity));
+  EXPECT_CALL(*this->imc, getVelocityRadAbsolute())
+      .WillOnce(Return(first_velocity_with_noise))
+      .WillOnce(Return(second_velocity_with_noise));
 
   march::Joint joint("actuate_true", 0, true, std::move(this->imc));
   joint.prepareActuation();
@@ -268,21 +282,22 @@ TEST_F(JointTest, TestReadEncodersTwice)
 
   ASSERT_DOUBLE_EQ(joint.getPosition(),
                    initial_absolute_position + (first_velocity + second_velocity) * elapsed_time.toSec());
-  ASSERT_DOUBLE_EQ(joint.getVelocity(),
-                   (third_incremental_position - second_incremental_position) / elapsed_time.toSec());
+  ASSERT_NEAR(joint.getVelocity(), (third_incremental_position - second_incremental_position) / elapsed_time.toSec(),
+              0.0000001);
 }
 
 TEST_F(JointTest, TestReadEncodersNoUpdate)
 {
   ros::Duration elapsed_time(0.2);
-  double first_velocity = 0.5;
+
+  double velocity = 0.5;
 
   double absolute_noise = -this->imc->getAbsoluteRadPerBit();
 
   double initial_incremental_position = 5;
   double initial_absolute_position = 3;
-  double second_incremental_position = initial_incremental_position + first_velocity * elapsed_time.toSec();
-  double second_absolute_position = initial_absolute_position + first_velocity * elapsed_time.toSec() + absolute_noise;
+  double second_incremental_position = initial_incremental_position + velocity * elapsed_time.toSec();
+  double second_absolute_position = initial_absolute_position + velocity * elapsed_time.toSec() + absolute_noise;
 
   EXPECT_CALL(*this->imc, getIMCVoltage()).WillRepeatedly(Return(48));
   EXPECT_CALL(*this->imc, getAngleRadIncremental())
@@ -291,14 +306,15 @@ TEST_F(JointTest, TestReadEncodersNoUpdate)
   EXPECT_CALL(*this->imc, getAngleRadAbsolute())
       .WillOnce(Return(initial_absolute_position))
       .WillRepeatedly(Return(second_absolute_position));
+  EXPECT_CALL(*this->imc, getVelocityRadIncremental()).WillRepeatedly(Return(velocity));
+  EXPECT_CALL(*this->imc, getVelocityRadAbsolute()).WillRepeatedly(Return(velocity));
 
   march::Joint joint("actuate_true", 0, true, std::move(this->imc));
   joint.prepareActuation();
 
   joint.readEncoders(elapsed_time);
   joint.readEncoders(elapsed_time);
 
-  ASSERT_DOUBLE_EQ(joint.getPosition(), initial_absolute_position + 2 * first_velocity * elapsed_time.toSec());
-  ASSERT_DOUBLE_EQ(joint.getVelocity(),
-                   (second_incremental_position - initial_incremental_position) / elapsed_time.toSec());
+  ASSERT_DOUBLE_EQ(joint.getPosition(), initial_absolute_position + 2 * velocity * elapsed_time.toSec());
+  ASSERT_DOUBLE_EQ(joint.getVelocity(), velocity);
 }