Fix some bugs

Author: Roelemans

march_hardware/config/march_odrive.json

@@ -6,7 +6,7 @@
     },
     {
         "name":"axis0.encoder.config.cpr",
-        "type":"uint32",
+        "type":"int32",
         "value":4096
     },
     {
@@ -21,7 +21,7 @@
     },
     {
         "name":"axis0.motor.config.pole_pairs",
-        "type":"uint32",
+        "type":"int32",
         "value":21
     },
     {
@@ -57,7 +57,7 @@
     {
         "name":"axis0.motor.config.direction",
         "type":"int32",
-        "value":1
+        "value":0
     },
     {
         "name":"axis0.sensorless_estimator.config.pm_flux_linkage",

march_hardware/include/march_hardware/joint.h

@@ -50,7 +50,7 @@ class Joint
   void prepareActuation();
 
   void actuateRad(double target_position);
-  void actuateTorque(int16_t target_torque);
+  void actuateTorque(double target_torque);
   void readEncoders(const ros::Duration& elapsed_time);
 
   double getPosition() const;

march_hardware/src/joint.cpp

@@ -141,7 +141,7 @@ double Joint::getAbsolutePosition() const
   return this->absolute_position_;
 }
 
-void Joint::actuateTorque(int16_t target_torque)
+void Joint::actuateTorque(double target_torque)
 {
   if (!this->canActuate())
   {

march_hardware/src/odrive/odrive_motor.cpp

@@ -111,10 +111,6 @@ void OdriveMotor::actuateTorque(double target_torque_ampere)
   float target_torque_ampere_float = (float)target_torque_ampere;
   ROS_INFO("target torque: %f", target_torque_ampere_float);
 
-  if (target_torque_ampere_float < 3.0)
-  {
-    target_torque_ampere_float = 3.0;
-  }
   std::string command_name_ = this->create_command(O_PM_DESIRED_MOTOR_CURRENT);
   if (this->write(command_name_, target_torque_ampere_float) == 1)
   {
@@ -219,6 +215,8 @@ float OdriveMotor::getMotorCurrent()
     return ODRIVE_ERROR;
   }
 
+    ROS_INFO("actual torque: %f", motor_current);
+
   return motor_current;
 }
 
@@ -272,13 +270,12 @@ int OdriveMotor::getAngleCountsIncremental()
     ROS_ERROR("Could not retrieve incremental position of the encoder");
     return ODRIVE_ERROR;
   }
-  ROS_WARN("encoder position: %f", iu_position);
   return iu_position;
 }
 
 double OdriveMotor::getAngleRadIncremental()
 {
-  double angle_rad = this->getAngleCountsIncremental() * PI_2 / std::pow(2, 12);
+  double angle_rad = this->getAngleCountsIncremental() * PI_2 / (std::pow(2, 12) * 101);
   return angle_rad;
 }
 
@@ -292,7 +289,7 @@ double OdriveMotor::getVelocityRadIncremental()
     return ODRIVE_ERROR;
   }
 
-  double angle_rad = iu_velocity * PI_2 / std::pow(2, 12);
+  double angle_rad = iu_velocity * PI_2 / (std::pow(2, 12) * 101);
   return angle_rad;
 }
 

march_hardware_builder/include/march_hardware_builder/allowed_robot.h

@@ -67,7 +67,7 @@ class AllowedRobot
     }
     else if (this->value == AllowedRobot::test_joint_rotational)
     {
-      return base_path.append("/robots/test_joint_rotational.yaml");
+      return base_path.append("/robots/odrive_test_joint_rotational.yaml");
     }
     else if (this->value == AllowedRobot::odrive_test_joint_rotational)
     {

march_hardware_builder/robots/odrive_test_joint_rotational.yaml

@@ -1,5 +1,4 @@
 testsetup:
-  ifName: enp2s0
   cycleTime: 4
   slaveTimeout: 50
   joints:

march_hardware_interface/src/march_hardware_interface.cpp

@@ -277,7 +277,7 @@ void MarchHardwareInterface::write(const ros::Time& /* time */, const ros::Durat
       }
       else if (joint.getActuationMode() == march::ActuationMode::torque)
       {
-        joint.actuateTorque(std::round(joint_effort_command_[i]));
+        joint.actuateTorque(joint_effort_command_[i]));
       }
     }
   }