WholeBodyMasterSlaveとHapticController

idl/CMakeLists.txt

@@ -29,6 +29,8 @@ set(idl_files
   ReferenceForceUpdaterService.idl
   AccelerationFilterService.idl
   ObjectContactTurnaroundDetectorService.idl
+  WholeBodyMasterSlaveService.idl
+  HapticControllerService.idl
   PCDLoaderService.idl
   )
 

idl/HapticControllerService.idl

@@ -0,0 +1,53 @@
+/**
+ * @file HapticControllerService.idl
+ * @brief Services for the HapticController interface
+ */
+//#include "OpenHRPCommon.idl"
+module OpenHRP
+{
+
+  interface HapticControllerService
+  {
+    typedef sequence<double, 2> DblSequence2;
+    typedef sequence<double, 3> DblSequence3;
+    typedef sequence<double, 4> DblSequence4;
+    typedef sequence<double, 6> DblSequence6;
+    typedef sequence<sequence<double, 6> , 4> DblSeqSeq6;
+    typedef double DblArray2[2];
+    typedef double DblArray3[3];
+    typedef double DblArray4[4];
+    typedef sequence<string> StrSequence;
+
+    struct HapticControllerParam
+    {
+        double dqAct_filter_cutoff_hz;
+        double ee_vel_filter_cutoff_hz;
+        double ex_gravity_compensation_ratio_lower;
+        double ex_gravity_compensation_ratio_upper;
+        double baselink_height_from_floor;
+        double foot_min_distance;
+        double force_feedback_ratio;
+        double gravity_compensation_ratio;
+        double q_friction_coeff;
+        double q_ref_max_torque_ratio;
+        double torque_feedback_ratio;
+        double wrench_hpf_cutoff_hz;
+        double wrench_lpf_cutoff_hz;
+        double wrench_hpf_gain;
+        double wrench_lpf_gain;
+        DblSequence2 ee_pos_rot_friction_coeff;
+        DblSequence2 floor_pd_gain;
+        DblSequence2 foot_horizontal_pd_gain;
+        DblSequence2 force_feedback_limit_ft;
+        DblSequence2 q_ref_pd_gain;
+        DblSeqSeq6   ex_ee_ref_wrench;
+    };
+    boolean startHapticController();
+    boolean stopHapticController();
+    boolean pauseHapticController();
+    boolean resumeHapticController();
+    void resetOdom();
+    void setParams(in HapticControllerParam i_param);
+    void getParams(out HapticControllerParam i_param);
+  };
+};

idl/WholeBodyMasterSlaveService.idl

@@ -0,0 +1,53 @@
+/**
+ * @file WholeBodyMasterSlaveService.idl
+ * @brief Services for the WholeBodyMasterSlave interface
+ */
+//#include "OpenHRPCommon.idl"
+module OpenHRP
+{
+
+  interface WholeBodyMasterSlaveService
+  {
+    typedef sequence<double, 2> DblSequence2;
+    typedef sequence<double, 3> DblSequence3;
+    typedef sequence<double, 4> DblSequence4;
+    typedef double DblArray2[2];
+    typedef double DblArray3[3];
+    typedef double DblArray4[4];
+    typedef sequence<string> StrSequence;
+
+    struct WholeBodyMasterSlaveParam
+    {
+        boolean auto_com_mode;
+        boolean auto_floor_h_mode;
+        boolean auto_foot_landing_by_act_cp;
+        boolean auto_foot_landing_by_act_zmp;
+        double additional_double_support_time;
+        double auto_com_foot_move_detect_height;
+        double auto_floor_h_detect_fz;
+        double auto_floor_h_reset_fz;
+        double base_to_hand_min_distance;
+        double capture_point_extend_ratio;
+        double com_filter_cutoff_hz;
+        double foot_collision_avoidance_distance;
+        double foot_landing_vel;
+        double force_double_support_com_h;
+        double human_to_robot_ratio;
+        double max_double_support_width;
+        double upper_body_rmc_ratio;
+        double single_foot_zmp_safety_distance;
+        double swing_foot_height_offset;
+        DblSequence3 com_offset;
+        DblSequence4 actual_foot_vert_fbio;
+        DblSequence4 safety_foot_vert_fbio;
+        StrSequence use_joints;
+        StrSequence use_targets;
+    };
+    boolean startWholeBodyMasterSlave();
+    boolean stopWholeBodyMasterSlave();
+    boolean pauseWholeBodyMasterSlave();
+    boolean resumeWholeBodyMasterSlave();
+    void setParams(in WholeBodyMasterSlaveParam i_param);
+    void getParams(out WholeBodyMasterSlaveParam i_param);
+  };
+};

python/hrpsys_config.py

@@ -267,6 +267,16 @@ class HrpsysConfigurator(object):
     octd = None
     octd_svc = None
     octd_version = None
+
+    # WholeBodyMasterSLave
+    wbms = None
+    wbms_svc = None
+    wbms_version = None
+
+    # HapticController
+    hc = None
+    hc_svc = None
+    hc_version = None
 
     # rtm manager
     ms = None
@@ -379,14 +389,39 @@ def connectComps(self):
             connectPorts(self.seq.port(sen + "Ref"),
                          self.sh.port(sen + "In"))
 
+        # connection for wbms
+        if self.wbms:
+            connectPorts(self.sh.port("qOut"), self.wbms.port("qRef"))
+            connectPorts(self.rh.port("q"), self.wbms.port("qAct"))
+            connectPorts(self.sh.port("zmpOut"), self.wbms.port("zmpIn"))
+            connectPorts(self.sh.port("basePosOut"), self.wbms.port("basePosIn"))
+            connectPorts(self.sh.port("baseRpyOut"), self.wbms.port("baseRpyIn"))
+            connectPorts(self.sh.port("optionalDataOut"), self.wbms.port("optionalData"))
+            connectPorts(self.rmfo.port("off_lfsensor"), self.wbms.port("local_lleg_wrench"))
+            connectPorts(self.rmfo.port("off_rfsensor"), self.wbms.port("local_rleg_wrench"))
+            connectPorts(self.rmfo.port("off_lhsensor"), self.wbms.port("local_larm_wrench"))
+            connectPorts(self.rmfo.port("off_rhsensor"), self.wbms.port("local_rarm_wrench"))
+
         # connection for st
         if rtm.findPort(self.rh.ref, "lfsensor") and rtm.findPort(
                                      self.rh.ref, "rfsensor") and self.st:
             connectPorts(self.kf.port("rpy"), self.st.port("rpy"))
-            connectPorts(self.sh.port("zmpOut"), self.abc.port("zmpIn"))
-            connectPorts(self.sh.port("basePosOut"), self.abc.port("basePosIn"))
-            connectPorts(self.sh.port("baseRpyOut"), self.abc.port("baseRpyIn"))
-            connectPorts(self.sh.port("optionalDataOut"), self.abc.port("optionalData"))
+            if self.wbms:
+#                connectPorts(self.wbms.port("q"), self.ic.port("qRef")) // really needed ?
+                connectPorts(self.st.port("actCapturePoint"),   self.wbms.port("actCapturePoint"))
+                connectPorts(self.st.port("zmp"),               self.wbms.port("zmp"))
+                connectPorts(self.wbms.port("zmpOut"),          self.abc.port("zmpIn"))
+                connectPorts(self.wbms.port("basePosOut"),      self.abc.port("basePosIn"))
+                connectPorts(self.wbms.port("baseRpyOut"),      self.abc.port("baseRpyIn"))
+                connectPorts(self.wbms.port("optionalDataOut"), self.abc.port("optionalData"))
+                connectPorts(self.wbms.port("AutoBalancerService"), self.abc.port("AutoBalancerService"))
+                connectPorts(self.wbms.port("StabilizerService"),   self.st.port("StabilizerService"))
+            else:
+                connectPorts(self.sh.port("zmpOut"),            self.abc.port("zmpIn"))
+                connectPorts(self.sh.port("basePosOut"),        self.abc.port("basePosIn"))
+                connectPorts(self.sh.port("baseRpyOut"),        self.abc.port("baseRpyIn"))
+                connectPorts(self.sh.port("optionalDataOut"),   self.abc.port("optionalData"))
+
             connectPorts(self.abc.port("zmpOut"), self.st.port("zmpRef"))
             connectPorts(self.abc.port("baseRpyOut"), self.st.port("baseRpyIn"))
             connectPorts(self.abc.port("basePosOut"), self.st.port("basePosIn"))
@@ -397,6 +432,7 @@ def connectComps(self):
             connectPorts(self.seq.port("qRef"), self.st.port("qRefSeq"))
             connectPorts(self.abc.port("walkingStates"), self.st.port("walkingStates"))
             connectPorts(self.abc.port("sbpCogOffset"), self.st.port("sbpCogOffset"))
+
             connectPorts(self.abc.port("toeheelRatio"), self.st.port("toeheelRatio"))
             if self.es:
                 connectPorts(self.st.port("emergencySignal"), self.es.port("emergencySignal"))
@@ -409,6 +445,15 @@ def connectComps(self):
                 connectPorts(self.rfu.port("refFootOriginExtMomentIsHoldValue"), self.abc.port("refFootOriginExtMomentIsHoldValue"))
             if self.octd:
                 connectPorts(self.abc.port("contactStates"), self.octd.port("contactStates"))
+
+        # connection for hc
+        if self.hc:
+            connectPorts(self.sh.port("qOut"),   self.hc.port("qRef"))
+            connectPorts(self.rh.port("q"),   self.hc.port("qAct"))
+            connectPorts(self.rh.port("dq"),  self.hc.port("dqAct"))
+            connectPorts(self.hc.port("tau"), self.rh.port("tauRef"))
+#            connectPorts(self.hc.port("q"), self.rh.port("??????"))
+#            connectPorts(self.hc.port("teleopOdom"), self.rh.port("??????"))
 
         # ref force moment connection
         for sen in self.getForceSensorNames():
@@ -502,7 +547,6 @@ def connectComps(self):
             if self.rmfo:
                 for sen in filter(lambda x: x.type == "Force", self.sensors):
                     connectPorts(self.rmfo.port("off_" + sen.name), self.octd.port(sen.name))
-
         # connection for gc
         if self.gc:
             connectPorts(self.rh.port("q"), self.gc.port("qCurrent"))  # other connections
@@ -749,6 +793,8 @@ def getRTCListUnstable(self):
             ['octd', "ObjectContactTurnaroundDetector"],
             ['es', "EmergencyStopper"],
             ['rfu', "ReferenceForceUpdater"],
+            ['wbms', "WholeBodyMasterSlave"],
+            ['hc', "HapticController"],
             ['ic', "ImpedanceController"],
             ['abc', "AutoBalancer"],
             ['st', "Stabilizer"],
@@ -767,8 +813,8 @@ def getJointAngleControllerList(self):
         '''!@brief
         Get list of controller list that need to control joint angles
         '''
-        controller_list = [self.es, self.ic, self.gc, self.abc, self.st, self.co,
-                           self.tc, self.hes, self.el]
+#        controller_list = [self.es, self.ic, self.gc, self.abc, self.st, self.co, self.tc, self.hes, self.el]
+        controller_list = [self.es, self.wbms, self.ic, self.gc, self.abc, self.st, self.co, self.tc, self.hes, self.el]
         return filter(lambda c: c != None, controller_list)  # only return existing controllers
 
     def getRTCInstanceList(self, verbose=True):
@@ -882,6 +928,36 @@ def setupLogger(self, maxLength=4000):
             self.connectLoggerPort(self.sh, 'zmpOut')
         if self.ic != None:
             self.connectLoggerPort(self.ic, 'q')
+        if self.hc != None:
+            self.connectLoggerPort(self.hc, 'tau')
+            self.connectLoggerPort(self.hc, 'debugData')
+            self.connectLoggerPort(self.hc, 'master_rleg_pose')
+            self.connectLoggerPort(self.hc, 'master_lleg_pose')
+            self.connectLoggerPort(self.hc, 'master_rarm_pose')
+            self.connectLoggerPort(self.hc, 'master_larm_pose')
+            self.connectLoggerPort(self.hc, 'master_com_pose')
+            self.connectLoggerPort(self.hc, 'master_head_pose')
+            self.connectLoggerPort(self.hc, 'master_rleg_wrench_dbg')
+            self.connectLoggerPort(self.hc, 'master_lleg_wrench_dbg')
+            self.connectLoggerPort(self.hc, 'master_rarm_wrench_dbg')
+            self.connectLoggerPort(self.hc, 'master_larm_wrench_dbg')
+
+        if self.wbms != None:
+            self.connectLoggerPort(self.wbms, 'q')
+            self.connectLoggerPort(self.wbms, 'basePosOut')
+            self.connectLoggerPort(self.wbms, 'baseRpyOut')
+            self.connectLoggerPort(self.wbms, 'zmpOut')
+            self.connectLoggerPort(self.wbms, 'baseTformOut')
+            self.connectLoggerPort(self.wbms, 'slave_rleg_wrench')
+            self.connectLoggerPort(self.wbms, 'slave_lleg_wrench')
+            self.connectLoggerPort(self.wbms, 'slave_rarm_wrench')
+            self.connectLoggerPort(self.wbms, 'slave_larm_wrench')
+            self.connectLoggerPort(self.wbms, 'slave_rleg_pose')
+            self.connectLoggerPort(self.wbms, 'slave_lleg_pose')
+            self.connectLoggerPort(self.wbms, 'slave_rarm_pose')
+            self.connectLoggerPort(self.wbms, 'slave_larm_pose')
+            self.connectLoggerPort(self.wbms, 'slave_com_pose')
+            self.connectLoggerPort(self.wbms, 'slave_head_pose')
         if self.abc != None:
             self.connectLoggerPort(self.abc, 'zmpOut')
             self.connectLoggerPort(self.abc, 'baseTformOut')