[hrpsys_choreonoid][hrpsys_choreonoid_tutorials] add HRP3HAND for HRP2JSKNT*

Author: Naoki-Hiraoka

hrpsys_choreonoid/iob/RobotHardware_choreonoid.cpp

@@ -28,7 +28,7 @@ static const char* robothardware_choreonoid_spec[] =
   };
 // </rtc-template>
 
-extern RobotHardware_choreonoid *self_ptr;
+extern RTC::DataFlowComponentBase *self_ptr;
 extern void iob_update();
 extern void iob_finish();
 extern void iob_set_torque_limit(std::vector<double> &vec);

hrpsys_choreonoid/iob/iob.cpp

@@ -3,7 +3,6 @@
 #include <cstdlib>
 #include <cstring>
 #include <vector>
-#include "iob.h"
 
 #include <stdio.h>
 
@@ -16,8 +15,14 @@
 #include <rtm/idl/BasicDataTypeSkel.h>
 #include <hrpModel/ModelLoaderUtil.h>
 #include <hrpModel/Sensor.h>
+#include <rtm/idl/BasicDataType.hh>
+#include <rtm/idl/ExtendedDataTypes.hh>
+
+#if defined IOB_NAMESPACE
+namespace IOB_NAMESPACE {
+#endif
 
-#include "RobotHardware_choreonoid.h"
+#include "iob.h"
 
 static std::vector<double> command;
 static std::vector<double> act_angle;
@@ -39,8 +44,6 @@ static timespec g_ts;
 static long g_period_ns=5000000;
 static std::vector<bool> isPosTq;
 
-Time iob_time;
-
 #define FORCE_AVERAGE 8
 #define TORQUE_AVERAGE 2
 
@@ -57,7 +60,9 @@ static std::vector<std::vector<double> > torque_queue;
 
 using namespace RTC;
 
-RobotHardware_choreonoid *self_ptr;
+Time iob_time;
+
+RTC::DataFlowComponentBase *self_ptr;
 
 //* *//
 static TimedDoubleSeq m_angleIn;
@@ -100,6 +105,7 @@ static std::vector<double> Pgain_orig, Dgain_orig;
 static std::vector<double> tlimit;
 static size_t dof, loop;
 static unsigned int m_debugLevel;
+static bool initial_p=true;
 
 static int iob_step;
 static int iob_nstep;
@@ -189,6 +195,23 @@ int set_number_of_joints(int num)
     for (int j = 0; j < TORQUE_AVERAGE; j++) {
       torque_queue[j].resize(num);
     }
+
+    dof = num;
+    m_torqueOut.data.length(dof);
+    m_angleIn.data.length(dof);
+    qold.resize(dof);
+    qold_ref.resize(dof);
+    tqold.resize(dof);
+    tqold_ref.resize(dof);
+    Pgain.resize(dof);
+    Dgain.resize(dof);
+    tqPgain.resize(dof);
+    tqDgain.resize(dof);
+    initial_Pgain.resize(dof);
+    initial_Dgain.resize(dof);
+    initial_tqPgain.resize(dof);
+    initial_tqDgain.resize(dof);
+
     return TRUE;
 }
 
@@ -667,6 +690,8 @@ int open_iob(void)
       }
     }
 
+    readGainFile();
+
     std::cerr << "choreonoid IOB is opened" << std::endl;
     return TRUE;
 }
@@ -678,10 +703,18 @@ void iob_update(void)
 {
     if(ip_angleIn->isNew()) {
       ip_angleIn->read();
-      if (dof == 0) {
-        dof = m_angleIn.data.length();
-        m_torqueOut.data.length(dof);
-        readGainFile();
+      if (initial_p){
+        if (dof != m_angleIn.data.length()) {
+          dof = m_angleIn.data.length();
+          m_torqueOut.data.length(dof);
+          readGainFile();
+        }
+        // initialize angleRef, old_ref and old with angle
+        for(int i = 0; i < dof; ++i) {
+          command[i] = qold_ref[i] = qold[i] = m_angleIn.data[i];
+          com_torque[i] = tqold_ref[i] = tqold[i] = 0/*act_torque[i]*/;
+        }
+        initial_p = false;
       }
       for(int i = 0; i < m_angleIn.data.length() && i < dof; i++) {
         act_angle[i] = m_angleIn.data[i];
@@ -917,11 +950,6 @@ static void readGainFile()
     } else {
       std::cerr << "[iob] Gain file [" << gain_fname << "] not opened" << std::endl;
     }
-    // initialize angleRef, old_ref and old with angle
-    for(int i = 0; i < dof; ++i) {
-      command[i] = qold_ref[i] = qold[i] = m_angleIn.data[i];
-      com_torque[i] = tqold_ref[i] = tqold[i] = 0/*act_torque[i]*/;
-    }
 }
 
 int close_iob(void)
@@ -1290,4 +1318,6 @@ int read_digital_output(char *doutput)
     return FALSE;
 }
 
-
+#if defined IOB_NAMESPACE
+}
+#endif

hrpsys_choreonoid_tutorials/CMakeLists.txt

@@ -35,6 +35,14 @@ else()
   set (HRP2_MODELS_DIR ${hrp2_models_SOURCE_PREFIX})
 endif()
 
+find_package(jsk_hrp2_ros_bridge QUIET)
+if(NOT "${jsk_hrp2_ros_bridge_FOUND}")
+  string(ASCII 27 Esc)
+  message(WARNING "${Esc}[1;33mPackage jsk_hrp2_ros_bridge is not found, if you have right to access them please include source code in catkin workspace${Esc}[m")
+else()
+  set (JSK_HRP2_ROS_BRIDGE_DIR ${jsk_hrp2_ros_bridge_SOURCE_PREFIX})
+endif()
+
 ################################
 ## compile_openhrp_model
 ##  Generate OpenHRP3 .xml and .conf file.

hrpsys_choreonoid_tutorials/config/BodyRTC_HRP3HAND.RH.conf

@@ -0,0 +1,16 @@
+##
+name-server = localhost:15005
+##
+## PD Controller
+##     in: angleRef, angle
+##     out: torque
+##
+in-port = tauIn:R_THUMBCM_Y,R_THUMBCM_P,R_INDEXMP_R,R_INDEXMP_P,R_INDEXPIP_R,R_MIDDLEPIP_R,L_THUMBCM_Y,L_THUMBCM_P,L_INDEXMP_R,L_INDEXMP_P,L_INDEXPIP_R,L_MIDDLEPIP_R:JOINT_TORQUE
+out-port = angleOut:R_THUMBCM_Y,R_THUMBCM_P,R_INDEXMP_R,R_INDEXMP_P,R_INDEXPIP_R,R_MIDDLEPIP_R,L_THUMBCM_Y,L_THUMBCM_P,L_INDEXMP_R,L_INDEXMP_P,L_INDEXPIP_R,L_MIDDLEPIP_R:JOINT_VALUE
+out-port = qvel:R_THUMBCM_Y,R_THUMBCM_P,R_INDEXMP_R,R_INDEXMP_P,R_INDEXPIP_R,R_MIDDLEPIP_R,L_THUMBCM_Y,L_THUMBCM_P,L_INDEXMP_R,L_INDEXMP_P,L_INDEXPIP_R,L_MIDDLEPIP_R:JOINT_VELOCITY
+out-port = torque:R_THUMBCM_Y,R_THUMBCM_P,R_INDEXMP_R,R_INDEXMP_P,R_INDEXPIP_R,R_MIDDLEPIP_R,L_THUMBCM_Y,L_THUMBCM_P,L_INDEXMP_R,L_INDEXMP_P,L_INDEXPIP_R,L_MIDDLEPIP_R:JOINT_TORQUE
+# out-port = ddq:JOINT_ACCELERATION
+connection = tauIn:HRP3HandController_choreonoid0:torqueOut
+connection = angleOut:HRP3HandController_choreonoid0:angleIn
+connection = qvel:HRP3HandController_choreonoid0:qvel_sim
+connection = torque:HRP3HandController_choreonoid0:torque_sim

hrpsys_choreonoid_tutorials/config/HRP2JSKNTS_LOAD_OBJ.cnoid.in

@@ -65,6 +65,19 @@ items:
                 AutoConnect: false
                 InstanceName: HRP2JSKNTS(Robot)0
                 bodyPeriodicRate: 0.004
+            -
+              id: 6
+              name: "BodyRTC_hand"
+              plugin: OpenRTM
+              class: BodyRTCItem
+              data:
+                isImmediateMode: true
+                moduleName: "@JSK_HRP2_ROS_BRIDGE_DIR@/lib/choreonoid/HRP3HandController_choreonoid"
+                confFileName: "@JVRC_CONF_DIRECTORY@/BodyRTC_HRP3HAND.RH.conf"
+                configurationMode: Use Configuration File
+                AutoConnect: false
+                InstanceName: HRP3HAND(Robot)0
+                bodyPeriodicRate: 0.004
         -
           id: 4
           name: "add_objects.py"

hrpsys_choreonoid_tutorials/config/HRP2JSKNT_LOAD_OBJ.cnoid.in

@@ -65,6 +65,19 @@ items:
                 AutoConnect: false
                 InstanceName: HRP2JSKNT(Robot)0
                 bodyPeriodicRate: 0.004
+            -
+              id: 6
+              name: "BodyRTC_hand"
+              plugin: OpenRTM
+              class: BodyRTCItem
+              data:
+                isImmediateMode: true
+                moduleName: "@JSK_HRP2_ROS_BRIDGE_DIR@/lib/choreonoid/HRP3HandController_choreonoid"
+                confFileName: "@JVRC_CONF_DIRECTORY@/BodyRTC_HRP3HAND.RH.conf"
+                configurationMode: Use Configuration File
+                AutoConnect: false
+                InstanceName: HRP3HAND(Robot)0
+                bodyPeriodicRate: 0.004
         -
           id: 4
           name: "add_objects.py"

hrpsys_choreonoid_tutorials/launch/hrp2jsknt_choreonoid.launch

@@ -15,6 +15,22 @@
     <arg name="HRPSYS_PY_NAME"       value="hrp2_rh_setup.py" />
     <arg name="CONTROLLER_CONFIG"    value="$(find hrpsys_ros_bridge_tutorials)/models/HRP2JSKNT_controller_config.yaml" />
     <arg name="CUSTOMIZER_CONF_FILE" value="$(find hrpsys_choreonoid_tutorials)/config/HRP2JSKNTCustomizer.yaml" />
+    <arg name="hrpsys_opt_rtc_config_args" value='-o "example.HRP3HandController_choreonoid.config_file:$(find hrpsys_ros_bridge_tutorials)/models/HRP2JSKNT.conf" -o "example.HRP3HandController_choreonoid.pdgains_sim_file_name:$(find jsk_hrp2_ros_bridge)/rtc/HRP3HandController/HRP3HAND.PDgains.dat"'/>
   </include>
 
+  <!-- HRP3HANDControllerServiceROSBridge -->
+  <env name="LANG" value="C" />
+  <env name="ORBgiopMaxMsgSize" value="2147483648" />
+  <arg name="nameserver" default="localhost" />
+  <arg name="corbaport" default="15005" />
+  <arg name="managerport" default="2810" />
+  <env name="RTCTREE_NAMESERVERS" value="$(arg nameserver):$(arg corbaport)" />
+  <env name="RTC_CONNECTION_CHECK_ONCE" value="true" />
+
+  <node pkg="jsk_hrp2_ros_bridge" name="HRP3HandControllerServiceROSBridge" type="HRP3HandControllerServiceROSBridgeComp"
+        output="screen" args='-o "corba.master_manager:$(arg nameserver):$(arg managerport)" -o "corba.nameservers:$(arg nameserver):$(arg corbaport)" -o "naming.formats:%n.rtc" -o "exec_cxt.periodic.type:PeriodicExecutionContext" -o "exec_cxt.periodic.rate:2000" -o "logger.file_name:/tmp/rtc%p.log"' />
+  <rtconnect from="HRP3HandControllerServiceROSBridge.rtc:HRP3HandControllerService" to="HRP3HandController_choreonoid0.rtc:HRP3HandControllerService" subscription_type="new"/>
+  <rtactivate component="HRP3HandControllerServiceROSBridge.rtc" />
+  <node name="rtmlaunch_hrp2jsknt_choreonoid" pkg="openrtm_tools" type="rtmlaunch.py" args="$(find hrpsys_choreonoid_tutorials)/launch/hrp2jsknt_choreonoid.launch" output="screen"/>
+
 </launch>

hrpsys_choreonoid_tutorials/launch/hrp2jsknts_choreonoid.launch

@@ -15,6 +15,22 @@
     <arg name="HRPSYS_PY_NAME"       value="hrp2_rh_setup.py" />
     <arg name="CONTROLLER_CONFIG"    value="$(find hrpsys_ros_bridge_tutorials)/models/HRP2JSKNTS_controller_config.yaml" />
     <arg name="CUSTOMIZER_CONF_FILE" value="$(find hrpsys_choreonoid_tutorials)/config/HRP2JSKNTSCustomizer.yaml" />
+    <arg name="hrpsys_opt_rtc_config_args" value='-o "example.HRP3HandController_choreonoid.config_file:$(find hrpsys_ros_bridge_tutorials)/models/HRP2JSKNT.conf" -o "example.HRP3HandController_choreonoid.pdgains_sim_file_name:$(find jsk_hrp2_ros_bridge)/rtc/HRP3HandController/HRP3HAND.PDgains.dat"'/>
   </include>
 
+  <!-- HRP3HANDControllerServiceROSBridge -->
+  <env name="LANG" value="C" />
+  <env name="ORBgiopMaxMsgSize" value="2147483648" />
+  <arg name="nameserver" default="localhost" />
+  <arg name="corbaport" default="15005" />
+  <arg name="managerport" default="2810" />
+  <env name="RTCTREE_NAMESERVERS" value="$(arg nameserver):$(arg corbaport)" />
+  <env name="RTC_CONNECTION_CHECK_ONCE" value="true" />
+
+  <node pkg="jsk_hrp2_ros_bridge" name="HRP3HandControllerServiceROSBridge" type="HRP3HandControllerServiceROSBridgeComp"
+        output="screen" args='-o "corba.master_manager:$(arg nameserver):$(arg managerport)" -o "corba.nameservers:$(arg nameserver):$(arg corbaport)" -o "naming.formats:%n.rtc" -o "exec_cxt.periodic.type:PeriodicExecutionContext" -o "exec_cxt.periodic.rate:2000" -o "logger.file_name:/tmp/rtc%p.log"' />
+  <rtconnect from="HRP3HandControllerServiceROSBridge.rtc:HRP3HandControllerService" to="HRP3HandController_choreonoid0.rtc:HRP3HandControllerService" subscription_type="new"/>
+  <rtactivate component="HRP3HandControllerServiceROSBridge.rtc" />
+  <node name="rtmlaunch_hrp2jsknts_choreonoid" pkg="openrtm_tools" type="rtmlaunch.py" args="$(find hrpsys_choreonoid_tutorials)/launch/hrp2jsknts_choreonoid.launch" output="screen"/>
+
 </launch>

hrpsys_choreonoid_tutorials/launch/robot_choreonoid.launch

@@ -7,6 +7,7 @@
   <arg name="USE_VISION" default="true" />
   <arg name="GUI" default="true" />
   <arg name="hrpsys_precreate_rtc" default=""/>
+  <arg name="hrpsys_opt_rtc_config_args" default=""/>
   <arg name="LAUNCH_FOOTCOORDS" default="true" />
 
   <arg name="CHOREONOID_ROBOT" default="JAXON_RED"/>
@@ -49,6 +50,7 @@
     <arg name="REALTIME" value="$(arg REALTIME)" />
     <arg name="GUI" value="$(arg GUI)" />
     <arg name="hrpsys_precreate_rtc" value="$(arg hrpsys_precreate_rtc)" />
+    <arg name="hrpsys_opt_rtc_config_args" value="$(arg hrpsys_opt_rtc_config_args)"/>
     <arg name="CONNECT_CONSTRAINT_FORCE_LOGGER_PORTS" default="$(arg CONNECT_CONSTRAINT_FORCE_LOGGER_PORTS)"/>
   </include>
   <!-- ros_bridge -->

hrpsys_choreonoid_tutorials/scripts/hrp2_rh_setup.py

@@ -3,6 +3,19 @@
 from hrpsys_ros_bridge_tutorials.hrp2_hrpsys_config import *
 
 class JSKHRP2ChoreonoidHrpsysConfigurator(JSKHRP2HrpsysConfigurator):
+    def getRTCList (self):
+        rtclist = JSKHRP2HrpsysConfigurator.getRTCList(self)
+        if self.ROBOT_NAME.find("HRP2JSKNT") != -1:
+            if ['hc', "HRP3HandController"] in rtclist:
+                rtclist.remove(['hc', "HRP3HandController"]) #HRP3HandController is created by choreonoid like RobotHardware
+        return rtclist
+
+    def getRTCInstanceList(self, verbose=True):
+        ret = JSKHRP2HrpsysConfigurator.getRTCInstanceList(self, verbose)
+        if self.ROBOT_NAME.find("HRP2JSKNT") != -1:
+            ret.append(self.hc)
+        return ret
+
     def waitForRobotHardware(self, robotname="Robot"):
         '''!@brief
         Wait for RobotHardware is exists and activated.
@@ -50,6 +63,45 @@ def waitForRobotHardware(self, robotname="Robot"):
 
         print(self.configurator_name + "findComps -> %s : %s isActive? = %s " % (robotname, self.rh_choreonoid,  self.rh_choreonoid.isActive()))
 
+    def waitForHRP3HandController(self, robotname="Robot"):
+        '''!@brief
+        Wait for HRP3HandController is exists and activated.
+
+        @param robotname str: name of RobotHardware component.
+        '''
+        self.hc = None
+        timeout_count = 0
+        # wait for simulator or HRP3HANDController setup which sometime takes a long time
+        while self.hc == None and timeout_count < 10:  # <- time out limit
+            if timeout_count > 0: # do not sleep initial loop
+                time.sleep(1);
+            self.hc = rtm.findRTC("HRP3HandController_choreonoid0")
+            print(self.configurator_name + "wait for %s : %s ( timeout %d < 10)" % ( "HRP3Hand", self.hc, timeout_count))
+            if self.hc and self.hc.isActive() == None:  # just in case hc is not ready...
+                self.hc = None
+            timeout_count += 1
+
+        if not self.hc:
+            print(self.configurator_name + "Could not find HRP3HandController_choreonoid0")
+            if self.ms:
+                print(self.configurator_name + "Candidates are .... " + str([x.name()  for x in self.ms.get_components()]))
+            print(self.configurator_name + "Exitting.... " + robotname)
+            exit(1)
+
+        print(self.configurator_name + "findComps -> HRP3HandController_choreonoid0 : %s isActive? = %s " % (self.hc,  self.hc.isActive()))
+
+    def waitForRTCManagerAndRobotHardware(self, robotname="Robot", managerhost=nshost):
+        '''!@brief
+        Wait for both RTC Manager (waitForRTCManager()) and RobotHardware (waitForRobotHardware()) and HRP3HandController (waitForHRP3HandController)
+        @param managerhost str: name of host computer that manager is running
+        @param robotname str: name of RobotHardware component.
+        '''
+        self.waitForRTCManager(managerhost)
+        self.waitForRobotHardware(robotname)
+        if self.ROBOT_NAME.find("HRP2JSKNT") != -1:
+            self.waitForHRP3HandController(robotname)
+        self.checkSimulationMode()
+
     def activateComps(self):
         stash_rh = self.rh
         self.rh = self.rh_choreonoid