diff --git a/rtc/SoftErrorLimiter/SoftErrorLimiter.cpp b/rtc/SoftErrorLimiter/SoftErrorLimiter.cpp index d6d2467269b..2721eadd448 100644 --- a/rtc/SoftErrorLimiter/SoftErrorLimiter.cpp +++ b/rtc/SoftErrorLimiter/SoftErrorLimiter.cpp @@ -230,7 +230,8 @@ RTC::ReturnCode_t SoftErrorLimiter::onExecute(RTC::UniqueId ec_id) double qvel = (m_qRef.data[i] - prev_angle[i]) / dt; double lvlimit = m_robot->joint(i)->lvlimit + 0.000175; // 0.01 deg / sec double uvlimit = m_robot->joint(i)->uvlimit - 0.000175; - if ( servo_state[i] == 1 && ((lvlimit > qvel) || (uvlimit < qvel)) ) { + // fixed joint has ulimit = vlimit + if ( servo_state[i] == 1 && (uvlimit < uvlimit) && ((lvlimit > qvel) || (uvlimit < qvel)) ) { if (loop % debug_print_freq == 0 || debug_print_velocity_first ) { std::cerr << "velocity limit over " << m_robot->joint(i)->name << "(" << i << "), qvel=" << qvel << ", lvlimit =" << lvlimit @@ -270,7 +271,8 @@ RTC::ReturnCode_t SoftErrorLimiter::onExecute(RTC::UniqueId ec_id) llimit = it->second.getLlimit(m_qRef.data[it->second.getTargetJointId()]); ulimit = it->second.getUlimit(m_qRef.data[it->second.getTargetJointId()]); } - bool servo_limit_state = ((llimit > m_qRef.data[i]) || (ulimit < m_qRef.data[i])); + // fixed joint have vlimit = ulimit + bool servo_limit_state = (llimit < ulimit) && ((llimit > m_qRef.data[i]) || (ulimit < m_qRef.data[i])); if ( servo_state[i] == 1 && servo_limit_state ) { if (loop % debug_print_freq == 0 || debug_print_position_first) { std::cerr << "position limit over " << m_robot->joint(i)->name << "(" << i << "), qRef=" << m_qRef.data[i]