Skip to content

[SoftErrorLimiter] fix priority among vel, err, pos #1294

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

[SoftErrorLimiter] fix priority among vel, err, pos #1294

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
93 changes: 37 additions & 56 deletions rtc/SoftErrorLimiter/SoftErrorLimiter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -284,38 +284,11 @@ RTC::ReturnCode_t SoftErrorLimiter::onExecute(RTC::UniqueId ec_id)
// Velocity limitation for reference joint angles
for ( unsigned int i = 0; i < m_qRef.data.length(); i++ ){
if(m_joint_mask[i]) continue;
// Determin total upper-lower limit considering velocity, position, and error limits.
// e.g.,
// total lower limit = max (vel, pos, err) <= severest lower limit
// total upper limit = min (vel, pos, err) <= severest upper limit
double total_upper_limit = std::numeric_limits<double>::max(), total_lower_limit = -std::numeric_limits<double>::max();
{
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;
// fixed joint has ulimit = vlimit
if ( servo_state[i] == 1 && (lvlimit < uvlimit) && ((lvlimit > qvel) || (uvlimit < qvel)) ) {
if (loop % debug_print_freq == 0 || debug_print_velocity_first ) {
std::cerr << "[" << m_profile.instance_name<< "] [" << m_qRef.tm
<< "] velocity limit over " << m_robot->joint(i)->name << "(" << i << "), qvel=" << qvel
<< ", lvlimit =" << lvlimit
<< ", uvlimit =" << uvlimit
<< ", servo_state = " << ( servo_state[i] ? "ON" : "OFF");
}
double limited;
// fix joint angle
if ( lvlimit > qvel ) {
limited = total_lower_limit = std::max(prev_angle[i] + lvlimit * dt, total_lower_limit);
}
if ( uvlimit < qvel ) {
limited = total_upper_limit = std::min(prev_angle[i] + uvlimit * dt, total_upper_limit);
}
if (loop % debug_print_freq == 0 || debug_print_velocity_first ) {
std::cerr << ", q(limited) = " << limited << std::endl;
}
velocity_limit_error = true;
}
}
// Output angle should satisfy following limits.
// 1. Velocity Limit (highest priority)
// 2. Error Limit
// 3. Position Limit (lowest priority)
// If these limits conflict with each other, the one with the higher priority takes precedence.

// Position limitation for reference joint angles
{
Expand All @@ -337,53 +310,61 @@ RTC::ReturnCode_t SoftErrorLimiter::onExecute(RTC::UniqueId ec_id)
<< ", servo_state = " << ( servo_state[i] ? "ON" : "OFF")
<< ", prev_angle = " << prev_angle[i];
}
double limited;
// fix joint angle
if ( llimit > m_qRef.data[i] && prev_angle[i] > m_qRef.data[i] ) { // ref < llimit and prev < ref -> OK
limited = total_lower_limit = std::max(llimit, total_lower_limit);
}
if ( ulimit < m_qRef.data[i] && prev_angle[i] < m_qRef.data[i] ) { // ulimit < ref and ref < prev -> OK
limited = total_upper_limit = std::min(ulimit, total_upper_limit);
}
// fix joint angles
m_qRef.data[i] = std::min(ulimit, std::max(llimit, m_qRef.data[i]));
if (loop % debug_print_freq == 0 || debug_print_position_first ) {
std::cerr << ", q(limited) = " << limited << std::endl;
std::cerr << ", q(limited) = " << m_qRef.data[i] << std::endl;
}
m_servoState.data[i][0] |= (0x200 << OpenHRP::RobotHardwareService::SERVO_ALARM_SHIFT);
position_limit_error = true;
}
}

// Servo error limitation between reference joint angles and actual joint angles
// pos_vel_limited_angle is current output joint angle which arleady finished position limit and velocity limit.
// Check and limit error between pos_vel_limited_angle and current actual joint angle.
{
double pos_vel_limited_angle = std::min(total_upper_limit, std::max(total_lower_limit, m_qRef.data[i]));
double limit = m_robot->m_servoErrorLimit[i];
double error = pos_vel_limited_angle - m_qCurrent.data[i];
double limit = std::max(0.0, m_robot->m_servoErrorLimit[i]); // limit should be greater than or equal to zero.
double error = m_qRef.data[i] - m_qCurrent.data[i];
if ( servo_state[i] == 1 && fabs(error) > limit ) {
if (loop % debug_print_freq == 0 || debug_print_error_first ) {
std::cerr << "[" << m_profile.instance_name<< "] [" << m_qRef.tm
<< "] error limit over " << m_robot->joint(i)->name << "(" << i << "), qRef=" << pos_vel_limited_angle
<< "] error limit over " << m_robot->joint(i)->name << "(" << i << "), qRef=" << m_qRef.data[i]
<< ", qCurrent=" << m_qCurrent.data[i] << " "
<< ", Error=" << error << " > " << limit << " (limit)"
<< ", servo_state = " << ( 1 ? "ON" : "OFF");
}
double limited;
if ( error > limit ) {
limited = total_upper_limit = std::min(m_qCurrent.data[i] + limit, total_upper_limit);
} else {
limited = total_lower_limit = std::max(m_qCurrent.data[i] - limit, total_lower_limit);
}
m_qRef.data[i] = std::max(m_qCurrent.data[i] - limit, std::min(m_qCurrent.data[i] + limit, m_qRef.data[i]));
if (loop % debug_print_freq == 0 || debug_print_error_first ) {
std::cerr << ", q(limited) = " << limited << std::endl;
std::cerr << ", q(limited) = " << m_qRef.data[i] << std::endl;
}
m_servoState.data[i][0] |= (0x040 << OpenHRP::RobotHardwareService::SERVO_ALARM_SHIFT);
soft_limit_error = true;
}
}

// Limitation of current output considering total upper and lower limits
prev_angle[i] = m_qRef.data[i] = std::min(total_upper_limit, std::max(total_lower_limit, m_qRef.data[i]));
// Velocity limitation
{
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;
// fixed joint has ulimit = vlimit
if ( servo_state[i] == 1 && (lvlimit < uvlimit) && ((lvlimit > qvel) || (uvlimit < qvel)) ) {
if (loop % debug_print_freq == 0 || debug_print_velocity_first ) {
std::cerr << "[" << m_profile.instance_name<< "] [" << m_qRef.tm
<< "] velocity limit over " << m_robot->joint(i)->name << "(" << i << "), qvel=" << qvel
<< ", lvlimit =" << lvlimit
<< ", uvlimit =" << uvlimit
<< ", servo_state = " << ( servo_state[i] ? "ON" : "OFF");
}
// fix joint angle
m_qRef.data[i] = std::min(prev_angle[i] + uvlimit * dt, std::max(prev_angle[i] + lvlimit * dt, m_qRef.data[i]));
if (loop % debug_print_freq == 0 || debug_print_velocity_first ) {
std::cerr << ", q(limited) = " << m_qRef.data[i] << std::endl;
}
velocity_limit_error = true;
}
}

prev_angle[i] = m_qRef.data[i];
}
// display error info if no error found
debug_print_velocity_first = !velocity_limit_error;
Expand Down