add nozawa's patches

idl/AutoBalancerService.idl

@@ -127,6 +127,11 @@ module OpenHRP
       sequence<DblSequence3, 2> 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;
     };
 
     /**

lib/util/Hrpsys.h

@@ -27,4 +27,9 @@ using std::fclose;
 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

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

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<size_t>(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<size_t>(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();
@@ -859,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<double>(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<coordinates> fs_vec;
+    std::vector<std::string> 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") {
@@ -868,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();
@@ -968,11 +999,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<double>(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 +1035,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<double> 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 +1133,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"
 {

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<hrp::Vector3>& tmp_forces);
+  hrp::Vector3 calc_vel_from_hand_error (const rats::coordinates& tmp_fix_coords);
 
   // for gg
   typedef boost::shared_ptr<rats::gait_generator> 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;
 };
 
 

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/)

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<int>(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<tp.posture.size(); i++) tp.posture[i] = m_q.data[i];

rtc/CollisionDetector/CollisionDetector.h

@@ -192,6 +192,7 @@ class CollisionDetector
   int default_recover_time;
   unsigned int m_debugLevel;
   bool m_enable;
+  int collision_beep_freq, collision_beep_count;
   OpenHRP::CollisionDetectorService::CollisionState m_state;
 };
 

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];

rtc/KalmanFilter/testKalmanFilterEstimation.cpp

@@ -8,6 +8,7 @@
  */
 
 
+#include <stdio.h>
 #include <fstream>
 #include "RPYKalmanFilter.h"
 #include "EKFilter.h"