[TorqueController] Pass qRefIn without checking range of motion when …

…motor torque contorller is disabled
snozawa · Dec 25, 2015 · 65f20fb · 65f20fb
1 parent 445c619
commit 65f20fb
Showing 1 changed file with 7 additions and 2 deletions.
diff --git a/rtc/TorqueController/TorqueController.cpp b/rtc/TorqueController/TorqueController.cpp
@@ -284,10 +284,15 @@ RTC::ReturnCode_t TorqueController::onExecute(RTC::UniqueId ec_id)
  // calculate dq by torque controller
  executeTorqueControl(dq);
 
- // output restricted qRef
+ // check range of motion and insert to output 
  for (int i = 0; i < m_robot->numJoints(); i++) {
- m_qRefOut.data[i] = std::min(std::max(m_qRefIn.data[i] + dq[i], m_robot->joint(i)->llimit), m_robot->joint(i)->ulimit);
+ if (m_motorTorqueControllers[i].isEnabled()) {
+ m_qRefOut.data[i] = std::min(std::max(m_qRefIn.data[i] + dq[i], m_robot->joint(i)->llimit), m_robot->joint(i)->ulimit);
+ } else {
+ m_qRefOut.data[i] = m_qRefIn.data[i]; // pass joint angle when controller is disabled
+ }
  }
+
  } else {
  if (isDebug()) {
  std::cerr << "[" << m_profile.instance_name << "]" << "TorqueController input is not correct" << std::endl;