Add the possibility to control the a joint in Idle mode in IRobotCont…

…rol interface
ami-iit · Feb 4, 2021 · 5c2b03f · 5c2b03f
1 parent 3bc8fa6
commit 5c2b03f
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 2 deletions.
diff --git a/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp b/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp
@@ -97,6 +97,9 @@ struct YarpRobotControl::Impl
  case VOCAB_CM_TORQUE:
  return IRobotControl::ControlMode::Torque;
 
+ case VOCAB_CM_IDLE:
+ return IRobotControl::ControlMode::Idle;
+
  default:
  return IRobotControl::ControlMode::Unknown;
  }
@@ -119,6 +122,9 @@ struct YarpRobotControl::Impl
  case IRobotControl::ControlMode::Torque:
  return VOCAB_CM_TORQUE;
 
+ case IRobotControl::ControlMode::Idle:
+ return VOCAB_CM_IDLE;
+
  default:
  return VOCAB_CM_UNKNOWN;
  }
@@ -171,6 +177,7 @@ struct YarpRobotControl::Impl
  this->desiredJointValuesAndMode.index[IRobotControl::ControlMode::PositionDirect].clear();
  this->desiredJointValuesAndMode.index[IRobotControl::ControlMode::Velocity].clear();
  this->desiredJointValuesAndMode.index[IRobotControl::ControlMode::Torque].clear();
+ this->desiredJointValuesAndMode.index[IRobotControl::ControlMode::Idle].clear();
 
  for (std::size_t i = 0; i < this->actuatedDOFs; i++)
  {
@@ -367,6 +374,7 @@ struct YarpRobotControl::Impl
  control(const IRobotControl::ControlMode& mode)
  {
  assert(mode != IRobotControl::ControlMode::Unknown);
+ assert(mode != IRobotControl::ControlMode::Idle);
 
  switch (mode)
  {
@@ -425,7 +433,10 @@ struct YarpRobotControl::Impl
  if (indeces.empty())
  continue;
 
- if (mode == IRobotControl::ControlMode::Unknown)
+ if (mode == IRobotControl::ControlMode::Idle)
+ continue;
+
+ else if (mode == IRobotControl::ControlMode::Unknown)
  {
  std::string error = " The following joints does not have a specified control "
  "mode: ";
@@ -544,7 +555,7 @@ bool YarpRobotControl::initialize(std::weak_ptr<ParametersHandler::IParametersHa
 }
 
 bool YarpRobotControl::setReferences(Eigen::Ref<const Eigen::VectorXd> jointValues,
-  const std::vector<IRobotControl::ControlMode>& controlModes)
+ const std::vector<IRobotControl::ControlMode>& controlModes)
 {
  if (controlModes != m_pimpl->controlModes)
  {

diff --git a/src/RobotInterface/include/BipedalLocomotion/RobotInterface/IRobotControl.h b/src/RobotInterface/include/BipedalLocomotion/RobotInterface/IRobotControl.h
@@ -34,6 +34,7 @@ class IRobotControl
  PositionDirect,
  Velocity,
  Torque,
+ Idle,
  Unknown
  };