From b6e41234ab920d5127d56215536de17d16d21974 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Wed, 11 Feb 2015 01:25:51 +0900 Subject: [PATCH 01/10] Add beep sound for CollisionDetector --- rtc/CollisionDetector/CollisionDetector.cpp | 23 +++++++++++++++++++++ rtc/CollisionDetector/CollisionDetector.h | 1 + 2 files changed, 24 insertions(+) diff --git a/rtc/CollisionDetector/CollisionDetector.cpp b/rtc/CollisionDetector/CollisionDetector.cpp index 2762c0a0365..3654eb9fc2d 100644 --- a/rtc/CollisionDetector/CollisionDetector.cpp +++ b/rtc/CollisionDetector/CollisionDetector.cpp @@ -22,6 +22,7 @@ #include "RobotHardwareService.hh" #include "CollisionDetector.h" +#include "../SoftErrorLimiter/beep.h" #define deg2rad(x) ((x)*M_PI/180) #define rad2deg(x) ((x)*180/M_PI) @@ -68,16 +69,20 @@ CollisionDetector::CollisionDetector(RTC::Manager* manager) #endif // USE_HRPSYSUTIL m_debugLevel(0), m_enable(true), + collision_beep_count(0), dummy(0) { m_service0.collision(this); #ifdef USE_HRPSYSUTIL m_log.enableRingBuffer(1); #endif // USE_HRPSYSUTIL + init_beep(); + start_beep(3136); } CollisionDetector::~CollisionDetector() { + quit_beep(); } @@ -236,6 +241,8 @@ RTC::ReturnCode_t CollisionDetector::onInitialize() status |= 0<< OpenHRP::RobotHardwareService::DRIVER_TEMP_SHIFT; m_servoState.data[i][0] = status; } + + collision_beep_freq = static_cast(1.0/(3.0*m_dt)); // 3 times / 1[s] return RTC::RTC_OK; } @@ -425,6 +432,22 @@ RTC::ReturnCode_t CollisionDetector::onExecute(RTC::UniqueId ec_id) // m_qOut.write(); + // beep sound for collision alert + // check servo for collision beep sound + bool has_servoOn = false; + for (int i = 0; i < m_robot->numJoints(); i++ ){ + int servo_state = (m_servoState.data[i][0] & OpenHRP::RobotHardwareService::SERVO_STATE_MASK) >> OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT; + has_servoOn = has_servoOn || (servo_state == 1); + } + // beep + if ( !m_safe_posture && has_servoOn ) { // If collided and some joint is servoOn + if ( collision_beep_count % collision_beep_freq == 0 && collision_beep_count % (collision_beep_freq * 3) != 0 ) start_beep(2352, collision_beep_freq*0.7); + else stop_beep(); + collision_beep_count++; + } else { + collision_beep_count = 0; + } + if ( ++m_loop_for_check >= m_collision_loop ) m_loop_for_check = 0; tp.posture.resize(m_qRef.data.length()); for (size_t i=0; i Date: Sat, 14 Feb 2015 19:38:36 +0900 Subject: [PATCH 02/10] Add 'some' argument for isServoOn and update documentation --- python/hrpsys_config.py | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/python/hrpsys_config.py b/python/hrpsys_config.py index 8f38c956c43..2e06c129d0d 100755 --- a/python/hrpsys_config.py +++ b/python/hrpsys_config.py @@ -1460,6 +1460,8 @@ def isServoOn(self, jname='any'): Check whether servo control has been turned on. @param jname str: Name of a link (that can be obtained by "hiro.Groups" as lists of groups). + When jname = 'all' or 'any' => If all joints are servoOn, return True, otherwise, return False. + When jname = 'some' => If some joint is servoOn, return True, otherwise return False. @return bool: True if servo is on ''' if self.simulation_mode: @@ -1471,13 +1473,20 @@ def isServoOn(self, jname='any'): # print self.configurator_name, 's = ', s if (s[0] & 2) == 0: return False + return True + elif jname.lower() == 'some': + for s in s_s: + # print self.configurator_name, 's = ', s + if (s[0] & 2) != 0: + return True + return False else: jid = eval('self.' + jname) print self.configurator_name, s_s[jid] if s_s[jid][0] & 1 == 0: return False - return True - return False + else: + return True def flat2Groups(self, flatList): '''!@brief From 352781bcd1cc4a4f45dfb7f4dbac31e1fedc92a6 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Sat, 14 Feb 2015 23:54:48 +0900 Subject: [PATCH 03/10] Add graspless manip mode, in which foot steps are modified based on hand modification --- idl/AutoBalancerService.idl | 5 ++ rtc/AutoBalancer/AutoBalancer.cpp | 89 +++++++++++++++++++++++++++++++ rtc/AutoBalancer/AutoBalancer.h | 5 ++ 3 files changed, 99 insertions(+) diff --git a/idl/AutoBalancerService.idl b/idl/AutoBalancerService.idl index fffba481ad9..00b04d6a2d0 100644 --- a/idl/AutoBalancerService.idl +++ b/idl/AutoBalancerService.idl @@ -127,6 +127,11 @@ module OpenHRP sequence default_zmp_offsets; double move_base_gain; ControllerMode controller_mode; + boolean graspless_manip_mode; + string graspless_manip_arm; + DblArray3 graspless_manip_p_gain; + DblArray3 graspless_manip_reference_trans_pos; + DblArray4 graspless_manip_reference_trans_rot; }; /** diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp index 615cd9a1ea7..0f5d2b8271b 100644 --- a/rtc/AutoBalancer/AutoBalancer.cpp +++ b/rtc/AutoBalancer/AutoBalancer.cpp @@ -254,6 +254,10 @@ RTC::ReturnCode_t AutoBalancer::onInitialize() leg_names.push_back("rleg"); leg_names.push_back("lleg"); + graspless_manip_mode = false; + graspless_manip_arm = "arms"; + graspless_manip_p_gain = hrp::Vector3::Zero(); + return RTC::RTC_OK; } @@ -590,6 +594,20 @@ void AutoBalancer::getTargetParameters() } tmp_foot_mid_pos *= 0.5; + // + { + if ( gg_is_walking && gg->get_gp_count() == static_cast(gg->get_default_step_time()/(2*m_dt))-1) { + hrp::Vector3 vel_htc(calc_vel_from_hand_error(tmp_fix_coords)); + gg->set_offset_velocity_param(vel_htc(0), vel_htc(1) ,vel_htc(2)); + }// else { + // if ( gg_is_walking && gg->get_gp_count() == static_cast(gg->get_default_step_time()/(2*m_dt))-1) { + // gg->set_offset_velocity_param(0,0,0); + // } + // } + } + + // + hrp::Vector3 tmp_ref_cog(m_robot->calcCM()); if (gg_is_walking) { ref_cog = gg->get_cog(); @@ -968,11 +986,26 @@ bool AutoBalancer::setAutoBalancerParam(const OpenHRP::AutoBalancerService::Auto for (size_t j = 0; j < 3; j++) default_zmp_offsets_array[i*3+j] = i_param.default_zmp_offsets[i][j]; zmp_interpolator->go(default_zmp_offsets_array, zmp_interpolate_time, true); + graspless_manip_mode = i_param.graspless_manip_mode; + graspless_manip_arm = std::string(i_param.graspless_manip_arm); + for (size_t j = 0; j < 3; j++) + graspless_manip_p_gain[j] = i_param.graspless_manip_p_gain[j]; + for (size_t j = 0; j < 3; j++) + graspless_manip_reference_trans_coords.pos[j] = i_param.graspless_manip_reference_trans_pos[j]; + graspless_manip_reference_trans_coords.rot = (Eigen::Quaternion(i_param.graspless_manip_reference_trans_rot[0], + i_param.graspless_manip_reference_trans_rot[1], + i_param.graspless_manip_reference_trans_rot[2], + i_param.graspless_manip_reference_trans_rot[3]).normalized().toRotationMatrix()); // rtc: (x, y, z, w) but eigen: (w, x, y, z) std::cerr << "[" << m_profile.instance_name << "] setAutoBalancerParam" << std::endl; std::cerr << "[" << m_profile.instance_name << "] move_base_gain = " << move_base_gain << std::endl; std::cerr << "[" << m_profile.instance_name << "] default_zmp_offsets = " << default_zmp_offsets_array[0] << " " << default_zmp_offsets_array[1] << " " << default_zmp_offsets_array[2] << " " << default_zmp_offsets_array[3] << " " << default_zmp_offsets_array[4] << " " << default_zmp_offsets_array[5] << std::endl; + std::cerr << "[" << m_profile.instance_name << "] graspless_manip_mode = " << graspless_manip_mode << std::endl; + std::cerr << "[" << m_profile.instance_name << "] graspless_manip_arm = " << graspless_manip_arm << std::endl; + std::cerr << "[" << m_profile.instance_name << "] graspless_manip_p_gain = " << graspless_manip_p_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << std::endl; + std::cerr << "[" << m_profile.instance_name << "] graspless_manip_reference_trans_pos = " << graspless_manip_reference_trans_coords.pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << std::endl; + std::cerr << "[" << m_profile.instance_name << "] graspless_manip_reference_trans_rot = " << graspless_manip_reference_trans_coords.rot.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", " [", "]")) << std::endl; return true; }; @@ -989,6 +1022,17 @@ bool AutoBalancer::getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalanc case MODE_SYNC_TO_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_ABC; break; default: break; } + i_param.graspless_manip_mode = graspless_manip_mode; + i_param.graspless_manip_arm = graspless_manip_arm.c_str(); + for (size_t j = 0; j < 3; j++) + i_param.graspless_manip_p_gain[j] = graspless_manip_p_gain[j]; + for (size_t j = 0; j < 3; j++) + i_param.graspless_manip_reference_trans_pos[j] = graspless_manip_reference_trans_coords.pos[j]; + Eigen::Quaternion qt(graspless_manip_reference_trans_coords.rot); + i_param.graspless_manip_reference_trans_rot[0] = qt.w(); + i_param.graspless_manip_reference_trans_rot[1] = qt.x(); + i_param.graspless_manip_reference_trans_rot[2] = qt.y(); + i_param.graspless_manip_reference_trans_rot[3] = qt.z(); return true; }; @@ -1076,6 +1120,51 @@ void AutoBalancer::calc_static_balance_point_from_forces(hrp::Vector3& sb_point, sb_point(2) = ref_com_height; }; +#ifndef rad2deg +#define rad2deg(rad) (rad * 180 / M_PI) +#endif +#ifndef deg2rad +#define deg2rad(deg) (deg * M_PI / 180) +#endif + +hrp::Vector3 AutoBalancer::calc_vel_from_hand_error (const coordinates& tmp_fix_coords) +{ + if (graspless_manip_mode) { + hrp::Vector3 dp,dr; + coordinates ref_hand_coords(gg->get_dst_foot_midcoords()), act_hand_coords; + ref_hand_coords.transform(graspless_manip_reference_trans_coords); // desired arm coords + hrp::Vector3 foot_pos(gg->get_dst_foot_midcoords().pos); + if ( graspless_manip_arm == "arms" ) { + // act_hand_coords.pos = (target_coords["rarm"].pos + target_coords["larm"].pos) / 2.0; + // vector3 cur_y(target_coords["larm"].pos - target_coords["rarm"].pos); + // cur_y(2) = 0; + // alias(cur_y) = normalize(cur_y); + // vector3 ref_y(ref_hand_coords.axis(AXIS_Y)); + // ref_y(2) = 0; + // alias(ref_y) = normalize(ref_y); + // dr = 0,0,((vector3(cross(ref_y, cur_y))(2) > 0 ? 1.0 : -1.0) * std::acos(dot(ref_y, cur_y))); // fix for rotation + } else { + ABCIKparam& tmpikp = ikp[graspless_manip_arm]; + act_hand_coords = rats::coordinates(tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos, + tmpikp.target_r0 * tmpikp.localR); + rats::difference_rotation(dr, ref_hand_coords.rot, act_hand_coords.rot); + dr(0) = 0; dr(1) = 0; + } + dp = act_hand_coords.pos - ref_hand_coords.pos + + dr.cross(hrp::Vector3(foot_pos - act_hand_coords.pos)); + dp(2) = 0; + hrp::Matrix33 foot_mt(gg->get_dst_foot_midcoords().rot.transpose()); + //alias(dp) = foot_mt * dp; + hrp::Vector3 dp2 = foot_mt * dp; + //alias(dr) = foot_mt * dr; + return hrp::Vector3(graspless_manip_p_gain[0] * dp2(0)/gg->get_default_step_time(), + graspless_manip_p_gain[1] * dp2(1)/gg->get_default_step_time(), + graspless_manip_p_gain[2] * rad2deg(dr(2))/gg->get_default_step_time()); + } else { + return hrp::Vector3::Zero(); + } +}; + // extern "C" { diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h index 9c15e52949f..f5837188f9c 100644 --- a/rtc/AutoBalancer/AutoBalancer.h +++ b/rtc/AutoBalancer/AutoBalancer.h @@ -187,6 +187,7 @@ class AutoBalancer // static balance point offsetting void static_balance_point_proc_one(hrp::Vector3& tmp_input_sbp, const double ref_com_height); void calc_static_balance_point_from_forces(hrp::Vector3& sb_point, const hrp::Vector3& tmpcog, const double ref_com_height, std::vector& tmp_forces); + hrp::Vector3 calc_vel_from_hand_error (const rats::coordinates& tmp_fix_coords); // for gg typedef boost::shared_ptr ggPtr; @@ -222,6 +223,10 @@ class AutoBalancer unsigned int m_debugLevel; bool is_legged_robot; int loop; + bool graspless_manip_mode; + std::string graspless_manip_arm; + hrp::Vector3 graspless_manip_p_gain; + rats::coordinates graspless_manip_reference_trans_coords; }; From 5d0851a3e1004cc0df388e5ba46142a78019f0fa Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Wed, 18 Feb 2015 01:17:23 +0900 Subject: [PATCH 04/10] Use RPY-fixed linear KF by default --- rtc/KalmanFilter/RPYKalmanFilter.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rtc/KalmanFilter/RPYKalmanFilter.h b/rtc/KalmanFilter/RPYKalmanFilter.h index 6eb22a82696..c8a8aa102a6 100644 --- a/rtc/KalmanFilter/RPYKalmanFilter.h +++ b/rtc/KalmanFilter/RPYKalmanFilter.h @@ -97,7 +97,7 @@ class RPYKalmanFilter { // x[1] = m_rate.data.avx; // rate ( rad/sec, AngularVelocity, gyro, stable/drift ) // use kalman filter with imaginary data hrp::Vector3 gyro2 = gyro; -#if 0 +#if 1 double roll = r_filter.getx()[0]; double pitch = p_filter.getx()[0]; double yaw = y_filter.getx()[0]; From 8b44697b19d47a44d3a750d96845dac0c68fe88b Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Thu, 19 Feb 2015 01:40:16 +0900 Subject: [PATCH 05/10] Reduce ThermoLimiter debug print (once per 0.1[s]) --- rtc/ThermoLimiter/ThermoLimiter.cpp | 3 ++- rtc/ThermoLimiter/ThermoLimiter.h | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/rtc/ThermoLimiter/ThermoLimiter.cpp b/rtc/ThermoLimiter/ThermoLimiter.cpp index e33a5f137d4..b441dfbfbb1 100644 --- a/rtc/ThermoLimiter/ThermoLimiter.cpp +++ b/rtc/ThermoLimiter/ThermoLimiter.cpp @@ -163,6 +163,7 @@ RTC::ReturnCode_t ThermoLimiter::onInitialize() // allocate memory for outPorts m_tauMaxOut.data.length(m_robot->numJoints()); + m_debug_print_freq = static_cast(0.1/m_dt); // once per 0.1 [s] return RTC::RTC_OK; } @@ -335,7 +336,7 @@ double ThermoLimiter::calcEmergencyRatio(RTC::TimedDoubleSeq ¤t, hrp::dvec if (current.data.length() == max.size()) { // estimate same dimension for (int i = 0; i < current.data.length(); i++) { double tmpEmergencyRatio = std::abs(current.data[i] / max[i]); - if (tmpEmergencyRatio > alarmRatio) { + if (tmpEmergencyRatio > alarmRatio && m_loop % m_debug_print_freq == 0) { std::cerr << prefix << "[" << m_robot->joint(i)->name << "]" << " is over " << alarmRatio << " of the limit (" << current.data[i] << "/" << max[i] << ")" << std::endl; } if (maxEmergencyRatio < tmpEmergencyRatio) { diff --git a/rtc/ThermoLimiter/ThermoLimiter.h b/rtc/ThermoLimiter/ThermoLimiter.h index eb1d5f6a66c..333e51d05ec 100644 --- a/rtc/ThermoLimiter/ThermoLimiter.h +++ b/rtc/ThermoLimiter/ThermoLimiter.h @@ -142,7 +142,7 @@ class ThermoLimiter private: double m_dt; long long m_loop; - unsigned int m_debugLevel; + unsigned int m_debugLevel, m_debug_print_freq; double m_alarmRatio; hrp::dvector m_motorTemperatureLimit; hrp::BodyPtr m_robot; From 0929f64eb53d35256557b0c9df647ca7ae8a5a67 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Thu, 19 Feb 2015 01:47:08 +0900 Subject: [PATCH 06/10] Reduce debug print message from SoftErrorLimiter (once per 0.02[s]) --- rtc/SoftErrorLimiter/SoftErrorLimiter.cpp | 39 +++++++++++++++-------- rtc/SoftErrorLimiter/SoftErrorLimiter.h | 2 +- 2 files changed, 26 insertions(+), 15 deletions(-) diff --git a/rtc/SoftErrorLimiter/SoftErrorLimiter.cpp b/rtc/SoftErrorLimiter/SoftErrorLimiter.cpp index 5ddbf93e24e..45bcf783807 100644 --- a/rtc/SoftErrorLimiter/SoftErrorLimiter.cpp +++ b/rtc/SoftErrorLimiter/SoftErrorLimiter.cpp @@ -131,6 +131,9 @@ RTC::ReturnCode_t SoftErrorLimiter::onInitialize() coil::stringTo(dt, prop["dt"].c_str()); soft_limit_error_beep_freq = static_cast(1.0/(4.0*dt)); // soft limit error => 4 times / 1[s] position_limit_error_beep_freq = static_cast(1.0/(2.0*dt)); // position limit error => 2 times / 1[s] + debug_print_freq = static_cast(0.02/dt); // once per 0.02 [s] + /* If you print debug message for all controller loop, please comment in here */ + // debug_print_freq = 1; // load joint limit table hrp::readJointLimitTableFromProperties (joint_limit_tables, m_robot, prop["joint_limit_table"], std::string(m_profile.instance_name)); @@ -225,10 +228,12 @@ RTC::ReturnCode_t SoftErrorLimiter::onExecute(RTC::UniqueId ec_id) double lvlimit = m_robot->joint(i)->lvlimit; double uvlimit = m_robot->joint(i)->uvlimit; if ( servo_state[i] == 1 && ((lvlimit > qvel) || (uvlimit < qvel)) ) { - std::cerr << "velocity limit over " << m_robot->joint(i)->name << "(" << i << "), qvel=" << qvel - << ", lvlimit =" << lvlimit - << ", uvlimit =" << uvlimit - << ", servo_state = " << ( servo_state[i] ? "ON" : "OFF") << std::endl; + if (loop % debug_print_freq == 0) { + std::cerr << "velocity limit over " << m_robot->joint(i)->name << "(" << i << "), qvel=" << qvel + << ", lvlimit =" << lvlimit + << ", uvlimit =" << uvlimit + << ", servo_state = " << ( servo_state[i] ? "ON" : "OFF") << std::endl; + } // fix joint angle if ( lvlimit > qvel ) m_qRef.data[i] = prev_angle[i] + lvlimit * dt; if ( uvlimit < qvel ) m_qRef.data[i] = prev_angle[i] + uvlimit * dt; @@ -263,11 +268,13 @@ RTC::ReturnCode_t SoftErrorLimiter::onExecute(RTC::UniqueId ec_id) } bool servo_limit_state = ((llimit > m_qRef.data[i]) || (ulimit < m_qRef.data[i])); if ( servo_state[i] == 1 && servo_limit_state ) { - std::cerr << "position limit over " << m_robot->joint(i)->name << "(" << i << "), qRef=" << m_qRef.data[i] - << ", llimit =" << llimit - << ", ulimit =" << ulimit - << ", servo_state = " << ( servo_state[i] ? "ON" : "OFF") - << ", prev_angle = " << prev_angle[i] << std::endl; + if (loop % debug_print_freq == 0) { + std::cerr << "position limit over " << m_robot->joint(i)->name << "(" << i << "), qRef=" << m_qRef.data[i] + << ", llimit =" << llimit + << ", ulimit =" << ulimit + << ", servo_state = " << ( servo_state[i] ? "ON" : "OFF") + << ", prev_angle = " << prev_angle[i] << std::endl; + } // fix joint angle if ( llimit > m_qRef.data[i] && prev_angle[i] > m_qRef.data[i] ) // ref < llimit and prev < ref -> OK m_qRef.data[i] = prev_angle[i]; @@ -284,12 +291,16 @@ RTC::ReturnCode_t SoftErrorLimiter::onExecute(RTC::UniqueId ec_id) double limit = m_robot->m_servoErrorLimit[i]; double error = m_qRef.data[i] - m_qCurrent.data[i]; if ( servo_state[i] == 1 && fabs(error) > limit ) { - std::cerr << "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"); + if (loop % debug_print_freq == 0) { + std::cerr << "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"); + } m_qRef.data[i] = m_qCurrent.data[i] + ( error > 0 ? limit : -limit ); - std::cerr << ", q=" << m_qRef.data[i] << std::endl; + if (loop % debug_print_freq == 0) { + std::cerr << ", q=" << m_qRef.data[i] << std::endl; + } m_servoState.data[i][0] |= (0x040 << OpenHRP::RobotHardwareService::SERVO_ALARM_SHIFT); soft_limit_error = true; } diff --git a/rtc/SoftErrorLimiter/SoftErrorLimiter.h b/rtc/SoftErrorLimiter/SoftErrorLimiter.h index 8aa4b0a0c34..f54d9b636e4 100644 --- a/rtc/SoftErrorLimiter/SoftErrorLimiter.h +++ b/rtc/SoftErrorLimiter/SoftErrorLimiter.h @@ -144,7 +144,7 @@ class SoftErrorLimiter boost::shared_ptr m_robot; std::map joint_limit_tables; unsigned int m_debugLevel; - int dummy, position_limit_error_beep_freq, soft_limit_error_beep_freq; + int dummy, position_limit_error_beep_freq, soft_limit_error_beep_freq, debug_print_freq; double dt; }; From 00d111ac931ac73b1933bd569019fc3cc36c71b5 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Sat, 21 Feb 2015 10:25:04 +0900 Subject: [PATCH 07/10] Check leg name alternation --- rtc/AutoBalancer/AutoBalancer.cpp | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp index 0f5d2b8271b..b44aa67c80d 100644 --- a/rtc/AutoBalancer/AutoBalancer.cpp +++ b/rtc/AutoBalancer/AutoBalancer.cpp @@ -877,7 +877,9 @@ bool AutoBalancer::setFootSteps(const OpenHRP::AutoBalancerService::FootstepSequ memcpy(initial_input_coords.pos.data(), fs[0].pos, sizeof(double)*3); initial_input_coords.rot = (Eigen::Quaternion(fs[0].rot[0], fs[0].rot[1], fs[0].rot[2], fs[0].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z) - gg->clear_footstep_node_list(); + std::vector fs_vec; + std::vector leg_name_vec; + std::string prev_leg(std::string(fs[0].leg) == "rleg"?"lleg":"rleg"); for (size_t i = 0; i < fs.length(); i++) { std::string leg(fs[i].leg); if (leg == "rleg" || leg == "lleg") { @@ -886,13 +888,24 @@ bool AutoBalancer::setFootSteps(const OpenHRP::AutoBalancerService::FootstepSequ initial_input_coords.transformation(fstrans, tmpfs); tmpfs = initial_support_coords; tmpfs.transform(fstrans); - gg->append_footstep_node(leg, tmpfs); + if ( prev_leg != leg ) { + leg_name_vec.push_back(leg); + fs_vec.push_back(tmpfs); + } else { + std::cerr << "[" << m_profile.instance_name << "] Invalid footstep (" << leg << "), footsteps should alternate in rleg and lleg." << std::endl; + return false; + } + prev_leg = leg; } else { - std::cerr << "[" << m_profile.instance_name << "] No such target : " << leg << std::endl; + std::cerr << "[" << m_profile.instance_name << "] No such target : " << leg << std::endl; return false; } } std::cerr << "[" << m_profile.instance_name << "] print footsteps " << std::endl; + gg->clear_footstep_node_list(); + for (size_t i = 0; i < fs_vec.size(); i++) { + gg->append_footstep_node(leg_name_vec[i], fs_vec[i]); + } gg->append_finalize_footstep(); gg->print_footstep_list(); startWalking(); From 1e10c5c6bcb88e72c45a5e59b3c7c1e0fb9d3bc9 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 23 Feb 2015 01:11:33 +0900 Subject: [PATCH 08/10] [rtc/CollisionDetector/CMakeLists.txt] add beep.cpp to comp_sources --- rtc/CollisionDetector/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rtc/CollisionDetector/CMakeLists.txt b/rtc/CollisionDetector/CMakeLists.txt index a052185723f..3c5da19e84a 100644 --- a/rtc/CollisionDetector/CMakeLists.txt +++ b/rtc/CollisionDetector/CMakeLists.txt @@ -1,10 +1,10 @@ set(seq_dir ${PROJECT_SOURCE_DIR}/rtc/SequencePlayer) if (USE_HRPSYSUTIL) - set(comp_sources ${seq_dir}/interpolator.cpp CollisionDetector.cpp CollisionDetectorService_impl.cpp GLscene.cpp VclipLinkPair.cpp) + set(comp_sources ${seq_dir}/interpolator.cpp CollisionDetector.cpp CollisionDetectorService_impl.cpp GLscene.cpp VclipLinkPair.cpp ../SoftErrorLimiter/beep.cpp) add_definitions(-DUSE_HRPSYSUTIL) else() # BVutil.cpp can be used without hrpsysUtil dependencies - set(comp_sources ${seq_dir}/interpolator.cpp CollisionDetector.cpp CollisionDetectorService_impl.cpp VclipLinkPair.cpp ../../lib/util/BVutil.cpp) + set(comp_sources ${seq_dir}/interpolator.cpp CollisionDetector.cpp CollisionDetectorService_impl.cpp VclipLinkPair.cpp ../../lib/util/BVutil.cpp ../SoftErrorLimiter/beep.cpp) set(libs hrpModel-3.1 hrpCollision-3.1 hrpsysBaseStub) endif() set(vclip_dir vclip_1.0/) From 540ca575148efe6f22143c2d655aab987bbf1e4b Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 19 Feb 2015 10:32:18 +0900 Subject: [PATCH 09/10] [Hrpsys.h, testKalmanFilterEstimation.cpp] fix for qnx compile, see #470 --- lib/util/Hrpsys.h | 3 +++ rtc/KalmanFilter/testKalmanFilterEstimation.cpp | 1 + 2 files changed, 4 insertions(+) diff --git a/lib/util/Hrpsys.h b/lib/util/Hrpsys.h index e160af6f2ec..a82da8ea622 100644 --- a/lib/util/Hrpsys.h +++ b/lib/util/Hrpsys.h @@ -27,4 +27,7 @@ using std::fclose; using std::fflush; using std::toupper; using std::copysign; +using std::atof; +using std::atan2; +using std::sqrt; #endif diff --git a/rtc/KalmanFilter/testKalmanFilterEstimation.cpp b/rtc/KalmanFilter/testKalmanFilterEstimation.cpp index e8f597530ae..af2d4e7a51c 100644 --- a/rtc/KalmanFilter/testKalmanFilterEstimation.cpp +++ b/rtc/KalmanFilter/testKalmanFilterEstimation.cpp @@ -8,6 +8,7 @@ */ +#include #include #include "RPYKalmanFilter.h" #include "EKFilter.h" From 8c06b58d15fa2368fa11d94432a9f0c63d601d1c Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 23 Feb 2015 09:45:07 +0900 Subject: [PATCH 10/10] [Hrpsys.h] add sin/cos for QNX --- lib/util/Hrpsys.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/lib/util/Hrpsys.h b/lib/util/Hrpsys.h index a82da8ea622..85f6771073f 100644 --- a/lib/util/Hrpsys.h +++ b/lib/util/Hrpsys.h @@ -28,6 +28,8 @@ using std::fflush; using std::toupper; using std::copysign; using std::atof; +using std::sin; +using std::cos; using std::atan2; using std::sqrt; #endif