Fix bug joint torque control device (#890)

ami-iit · Sep 23, 2024 · 5806285 · 5806285
1 parent 392b02b
commit 5806285
Show file tree

Hide file tree

Showing 4 changed files with 6 additions and 24 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -8,6 +8,7 @@ All notable changes to this project are documented in this file.
 ### Changed
 
 ### Fixed
+- Bug fix of `JointTorqueControlDevice` device (https://github.com/ami-iit/bipedal-locomotion-framework/pull/890)
 
 ## [0.19.0] - 2024-09-06
 ### Added

diff --git a/devices/JointTorqueControlDevice/include/BipedalLocomotion/JointTorqueControlDevice.h b/devices/JointTorqueControlDevice/include/BipedalLocomotion/JointTorqueControlDevice.h
@@ -85,8 +85,6 @@ struct PINNParameters
 {
  std::string modelPath; /**< PINN model path */
  int threadNumber; /**< number of threads */
- int historyLength; /**< history length */
- int inputNumber; /**< number of inputs */
 
  /**
  * Reset the parameters
@@ -95,8 +93,6 @@ struct PINNParameters
  {
  modelPath = "";
  threadNumber = 0;
- historyLength = 0;
- inputNumber = 0;
  }
 };
 

diff --git a/devices/JointTorqueControlDevice/src/JointTorqueControlDevice.cpp b/devices/JointTorqueControlDevice/src/JointTorqueControlDevice.cpp
@@ -520,9 +520,6 @@ bool JointTorqueControlDevice::loadCouplingMatrix(Searchable& config,
  }
  }
 
- log()->error("{} LoadCouplingMatrix DEBUG: loaded kinematic coupling matrix from group {}", logPrefix, group_name);
- log()->error("{}", coupling_matrix.fromJointVelocitiesToMotorVelocities);
-
  coupling_matrix.fromJointTorquesToMotorTorques
  = coupling_matrix.fromJointVelocitiesToMotorVelocities.transpose();
  coupling_matrix.fromMotorTorquesToJointTorques
@@ -648,18 +645,7 @@ bool JointTorqueControlDevice::loadFrictionParams(
  log()->error("{} Parameter `model` not found", logPrefix);
  return false;
  }
- Eigen::VectorXi historySize;
- if (!frictionGroup->getParameter("history_size", historySize))
- {
- log()->error("{} Parameter `history_size` not found", logPrefix);
- return false;
- }
- Eigen::VectorXi inputNumber;
- if (!frictionGroup->getParameter("number_of_inputs", inputNumber))
- {
- log()->error("{} Parameter `number_of_inputs` not found", logPrefix);
- return false;
- }
+
  int threads;
  if (!frictionGroup->getParameter("thread_number", threads))
  {
@@ -670,8 +656,6 @@ bool JointTorqueControlDevice::loadFrictionParams(
  for (int i = 0; i < models.size(); i++)
  {
  pinnParameters[i].modelPath = models[i];
- pinnParameters[i].historyLength = historySize[i];
- pinnParameters[i].inputNumber = inputNumber[i];
  pinnParameters[i].threadNumber = threads;
  }
  }
@@ -767,8 +751,7 @@ bool JointTorqueControlDevice::open(yarp::os::Searchable& config)
 
  for (int i = 0; i < kt.size(); i++)
  {
- if (motorTorqueCurrentParameters[i].frictionModel == "FRICTION_PINN"
- && motorTorqueCurrentParameters[i].kfc > 0.0)
+ if (motorTorqueCurrentParameters[i].frictionModel == "FRICTION_PINN")
  {
  frictionEstimators[i] = std::make_unique<PINNFrictionEstimator>();
 

diff --git a/devices/JointTorqueControlDevice/src/PINNFrictionEstimator.cpp b/devices/JointTorqueControlDevice/src/PINNFrictionEstimator.cpp
@@ -63,6 +63,7 @@ bool PINNFrictionEstimator::initialize(const std::string& networkModelPath,
 {
  std::basic_string<ORTCHAR_T> networkModelPathAsOrtString(networkModelPath.begin(),
  networkModelPath.end());
+
  Ort::SessionOptions sessionOptions;
 
  // Set the number of intra-op threads
@@ -87,8 +88,9 @@ bool PINNFrictionEstimator::initialize(const std::string& networkModelPath,
  }
 
  // Get model input size
- int inputCount = m_pimpl->session->GetInputCount();
+ std::vector<int64_t> inputShape = m_pimpl->session->GetInputTypeInfo(0).GetTensorTypeAndShapeInfo().GetShape();
 
+ const std::size_t inputCount = inputShape[1];
  m_pimpl->historyLength = inputCount / 2;
 
  // format the input