Merge remote-tracking branch 'origin/master' into fix-torque-controll…

…er-pass-qref-mode

Conflicts:
	rtc/TorqueController/TorqueController.cpp

.travis.sh

@@ -37,7 +37,8 @@ travis_time_start mongo_hack
 
 # MongoDB hack
 dpkg -s mongodb || echo "ok"; export HAVE_MONGO_DB=$?
-if [ $HAVE_MONGO_DB == 0 ]; then sudo apt-get remove -qq -y mongodb mongodb-10gen || echo "ok"; fi
+# Remove configuration file for mongodb using --purge option (reported in https://github.com/fkanehiro/hrpsys-base/pull/900#issuecomment-162392884)
+if [ $HAVE_MONGO_DB == 0 ]; then sudo apt-get remove --purge -qq -y mongodb mongodb-10gen || echo "ok"; fi
 if [ $HAVE_MONGO_DB == 0 ]; then sudo apt-get install -qq -y mongodb-clients mongodb-server -o Dpkg::Options::="--force-confdef" || echo "ok"; fi # default actions
 
 travis_time_end
@@ -362,6 +363,12 @@ case $TEST_PACKAGE in
  if [ -e /opt/ros/hydro/lib/python2.7/dist-packages/hironx_ros_bridge/hironx_client.py ]; then
  sudo wget https://raw.githubusercontent.com/k-okada/rtmros_hironx/stop_unfinished_battle/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py -O /opt/ros/hydro/lib/python2.7/dist-packages/hironx_ros_bridge/hironx_client.py
  fi
+ #https://github.com/start-jsk/rtmros_common/commit/51ec26b899f09304705fe0528a068e57b061b9b7
+ #https://github.com/start-jsk/rtmros_common/pull/880
+ #https://github.com/start-jsk/rtmros_common/pull/879
+ if [ -e /opt/ros/hydro/share/hrpsys_ros_bridge/test/test-samplerobot.test ]; then
+ sudo wget https://raw.githubusercontent.com/start-jsk/rtmros_common/1.3.1/hrpsys_ros_bridge/test/test-samplerobot.test -O /opt/ros/hydro/share/hrpsys_ros_bridge/test/test-samplerobot.test
+ fi
  travis_time_end
 
  # Check make test
@@ -379,7 +386,13 @@ case $TEST_PACKAGE in
  rostest $test_file && travis_time_end || export TMP_EXIT_STATUS=$?
  if [ "$TMP_EXIT_STATUS" != 0 ]; then
  export EXIT_STATUS=$TMP_EXIT_STATUS
+ # Print results of rostest-*.xml files
  find ~/.ros/test_results -type f -iname "*`basename $test_file .test`.xml" -print -exec echo "=== {} ===" \; -exec cat {} \;
+ # Print results of each rosunit-*.xml file
+ # Get rosunit*.xml file path from rostest-*.xml file by usig awk and cut.
+ # Files are assumed to include "xxx results are in [/home/xxx/rosunit-yy.xml]"
+ rosunit_xml_result_files=$(find ~/.ros/test_results -type f -iname "*`basename $test_file .test`.xml" -print -exec echo "=== {} ===" \; -exec cat {} \; | grep "results are in" | awk -F'results are in ' '{print $2}' | cut -d\[ -f2 | cut -d\] -f1)
+ if [ "${rosunit_xml_result_files}" != "" ]; then cat ${rosunit_xml_result_files}; fi
  travis_time_end 31
  fi
  done

.travis.yml

@@ -26,6 +26,7 @@ env:
  # Exec USE_SRC_OPENHRP3=true tests in faster orders to make debug of these tests easy.
  - TEST_TYPE=work_with_downstream TEST_PACKAGE=hironx-ros-bridge USE_SRC_OPENHRP3=true
  - TEST_TYPE=work_with_315_1_10 TEST_PACKAGE=hironx-ros-bridge USE_SRC_OPENHRP3=true
+ - TEST_TYPE=work_with_downstream TEST_PACKAGE=hrpsys-base USE_SRC_OPENHRP3=true
  # USE_SRC_OPENHRP3=false tests
  - TEST_TYPE=work_with_downstream TEST_PACKAGE=hrpsys-base
  - TEST_TYPE=work_with_315_1_10 TEST_PACKAGE=hrpsys-base

idl/AutoBalancerService.idl

@@ -211,6 +211,8 @@ module OpenHRP
  sequence<DblSequence3> leg_default_translate_pos;
  /// Number of optional finalize footsteps in goPos
  long optional_go_pos_finalize_footstep_num;
+ /// Offset for overwritable footstep index. Offset from current footstep index. Used in emergency_stop and velocity_mode.
+ long overwritable_footstep_index_offset;
  };
 
  /**

package.xml

@@ -1,6 +1,6 @@
 <package>
  <name>hrpsys</name>
- <version>315.7.0</version>
+ <version>315.8.0</version>
  <description>
  <p>An <a href = "http:https://www.openrtm.org/pub/OpenRTM-aist/">OpenRTM-aist</a>-based robot controller. This package is the most tailored for humanoid (dual-arm and/or biped) robots for historical reason.</p>
  <p>hrpsys package does not only wraps but build and installs the code from its mainstream repository (<a href = "https://github.com/fkanehiro/hrpsys-base/">github.com/fkanehiro/hrpsys-base</a>).</p>

rtc/AutoBalancer/AutoBalancer.cpp

@@ -133,7 +133,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
  if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), 
  CosNaming::NamingContext::_duplicate(naming.getRootContext())
  )){
- std::cerr << m_profile.instance_name << " failed to load model[" << prop["model"] << "]" << std::endl;
+ std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
  return RTC::RTC_ERROR;
  }
 
@@ -181,7 +181,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
  coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str());
  }
  tp.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
- tp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), m_dt, false));
+ tp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), m_dt, false, std::string(m_profile.instance_name)));
  // Fix for toe joint
  // Toe joint is defined as end-link joint in the case that end-effector link != force-sensor link
  // Without toe joints, "end-effector link == force-sensor link" is assumed.
@@ -838,7 +838,7 @@ void AutoBalancer::getTargetParameters()
  }
  //
  {
- if ( gg_is_walking && gg->get_lcg_count() == static_cast<size_t>(gg->get_default_step_time()/(2*m_dt))-1) {
+ if ( gg_is_walking && gg->get_lcg_count() == gg->get_overwrite_check_timing()+2 ) {
  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 {
@@ -894,11 +894,9 @@ void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matri
  // get current foot mid pos + rot
  std::vector<coordinates> foot_coords;
  for (size_t i = 0; i < leg_names.size(); i++) {
- if (leg_names[i].find("leg") != std::string::npos) {
- ABCIKparam& tmpikp = ikp[leg_names[i]];
- foot_coords.push_back(coordinates((tmpikp.target_link->p + tmpikp.target_link->R * tmpikp.localPos),
- (tmpikp.target_link->R * tmpikp.localR)));
- }
+ ABCIKparam& tmpikp = ikp[leg_names[i]];
+ foot_coords.push_back(coordinates((tmpikp.target_link->p + tmpikp.target_link->R * tmpikp.localPos),
+ (tmpikp.target_link->R * tmpikp.localR)));
  }
  coordinates current_foot_mid_coords;
  multi_mid_coords(current_foot_mid_coords, foot_coords);
@@ -1099,9 +1097,7 @@ void AutoBalancer::stopWalking ()
 {
  std::vector<coordinates> tmp_end_coords_list;
  for (std::vector<string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) {
- if ((*it).find("leg") != std::string::npos) {
- tmp_end_coords_list.push_back(ikp[*it].target_end_coords);
- }
+ tmp_end_coords_list.push_back(ikp[*it].target_end_coords);
  }
  multi_mid_coords(fix_leg_coords, tmp_end_coords_list);
  fixLegToCoords(fix_leg_coords.pos, fix_leg_coords.rot);
@@ -1144,18 +1140,26 @@ void AutoBalancer::waitABCTransition()
 }
 bool AutoBalancer::goPos(const double& x, const double& y, const double& th)
 {
- if ( !gg_is_walking && !is_stop_mode) {
+ // if ( !gg_is_walking && !is_stop_mode) {
+ if ( !is_stop_mode) {
  gg->set_all_limbs(leg_names);
  coordinates start_ref_coords;
  mid_coords(start_ref_coords, 0.5, ikp["rleg"].target_end_coords, ikp["lleg"].target_end_coords);
- gg->go_pos_param_2_footstep_nodes_list(x, y, th,
- (y > 0 ? boost::assign::list_of(ikp["rleg"].target_end_coords) : boost::assign::list_of(ikp["lleg"].target_end_coords)),
- start_ref_coords,
- (y > 0 ? boost::assign::list_of(RLEG) : boost::assign::list_of(LLEG)));
- startWalking();
- return true;
+ bool ret = gg->go_pos_param_2_footstep_nodes_list(x, y, th,
+ (y > 0 ? boost::assign::list_of(ikp["rleg"].target_end_coords) : boost::assign::list_of(ikp["lleg"].target_end_coords)), // Dummy if gg_is_walking
+ start_ref_coords, // Dummy if gg_is_walking
+ (y > 0 ? boost::assign::list_of(RLEG) : boost::assign::list_of(LLEG)), // Dummy if gg_is_walking
+ (!gg_is_walking)); // If gg_is_walking, initialize. Otherwise, not initialize and overwrite footsteps.
+
+ if ( !gg_is_walking ) { // Initializing
+ startWalking();
+ }
+ if (!ret) {
+ std::cerr << "[" << m_profile.instance_name << "] Cannot goPos because of invalid timing." << std::endl;
+ }
+ return ret;
  } else {
- std::cerr << "[" << m_profile.instance_name << "] Cannot goPos while walking." << std::endl;
+ std::cerr << "[" << m_profile.instance_name << "] Cannot goPos while stopping mode." << std::endl;
  return false;
  }
 }
@@ -1386,6 +1390,7 @@ bool AutoBalancer::setGaitGeneratorParam(const OpenHRP::AutoBalancerService::Gai
  gg->set_use_toe_heel_transition(i_param.use_toe_heel_transition);
  gg->set_zmp_weight_map(boost::assign::map_list_of<leg_type, double>(RLEG, i_param.zmp_weight_map[0])(LLEG, i_param.zmp_weight_map[1])(RARM, i_param.zmp_weight_map[2])(LARM, i_param.zmp_weight_map[3]));
  gg->set_optional_go_pos_finalize_footstep_num(i_param.optional_go_pos_finalize_footstep_num);
+ gg->set_overwritable_footstep_index_offset(i_param.overwritable_footstep_index_offset);
 
  // print
  gg->print_param(std::string(m_profile.instance_name));
@@ -1454,6 +1459,7 @@ bool AutoBalancer::getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGener
  i_param.zmp_weight_map[2] = tmp_zmp_weight_map[RARM];
  i_param.zmp_weight_map[3] = tmp_zmp_weight_map[LARM];
  i_param.optional_go_pos_finalize_footstep_num = gg->get_optional_go_pos_finalize_footstep_num();
+ i_param.overwritable_footstep_index_offset = gg->get_overwritable_footstep_index_offset();
  return true;
 };
 
@@ -1794,14 +1800,16 @@ hrp::Vector3 AutoBalancer::calc_vel_from_hand_error (const coordinates& tmp_fix_
  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
+ hrp::Vector3 rarm_pos = ikp["rarm"].target_p0 + ikp["rarm"].target_r0 * ikp["rarm"].localPos;
+ hrp::Vector3 larm_pos = ikp["larm"].target_p0 + ikp["larm"].target_r0 * ikp["larm"].localPos;
+ act_hand_coords.pos = (rarm_pos+larm_pos)/2.0;
+ hrp::Vector3 act_y = larm_pos-rarm_pos;
+ act_y(2) = 0;
+ act_y.normalize();
+ hrp::Vector3 ref_y(ref_hand_coords.rot * hrp::Vector3::UnitY());
+ ref_y(2) = 0;
+ ref_y.normalize();
+ dr = hrp::Vector3(0,0,(hrp::Vector3(ref_y.cross(act_y))(2) > 0 ? 1.0 : -1.0) * std::acos(ref_y.dot(act_y))); // fix for rotation
  } else {
  ABCIKparam& tmpikp = ikp[graspless_manip_arm];
  act_hand_coords = rats::coordinates(tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos,