[ros_bridge] Enable to use impedance controller through ros bridge with auto-generated idl python

hironx_ros_bridge/.gitignore

@@ -0,0 +1,9 @@
+# For rtmbuild
+msg/OpenHRP_*
+srv/OpenHRP_*
+src_gen
+
+# For others
+conf/*.conf
+conf/*.xml
+*.pyc

hironx_ros_bridge/CMakeLists.txt

@@ -1,15 +1,30 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(hironx_ros_bridge)
 
-find_package(catkin REQUIRED COMPONENTS hrpsys_ros_bridge roslib roslint rostest)
+find_package(catkin REQUIRED COMPONENTS hrpsys_ros_bridge roslib roslint rostest rtmbuild)
 find_package(Boost REQUIRED COMPONENTS system)
 
+# catkin_python_setup() must be before generate_messages() included in rtmbuild_init()
+catkin_python_setup()
+
+# for generating bridge for old impedance controller
+include(${PROJECT_SOURCE_DIR}/cmake/rtmbuild_from_dir.cmake)
+
+# initialize rtmbuild for old impedance controller
+rtmbuild_init_from_dir(idl_315_1_9)
+
+# call catkin_package, after rtmbuild_init, before rtmbuild_gen*
 catkin_package(
     CATKIN_DEPENDS hrpsys_ros_bridge roslib #
     INCLUDE_DIRS include
+    CFG_EXTRAS rtmbuild_from_dir.cmake
 )
 
-catkin_python_setup()
+# generate idl for old impedance controller
+rtmbuild_genidl_from_dir(idl_315_1_9 315_1_9)
+
+# generate bridge for old impedance controller
+rtmbuild_genbridge()
 
 add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/models/kawada-hironx.dae
   COMMAND ${catkin_EXTRAS_DIR}/test/download_checkmd5.py
@@ -72,6 +87,8 @@ install(DIRECTORY conf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} PATTERN "
 install(DIRECTORY models DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} PATTERN ".svn" EXCLUDE)
 install(DIRECTORY resource DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 install(DIRECTORY test DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS PATTERN ".svn" EXCLUDE)
+install(DIRECTORY idl_315_1_9 DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS)
+install(DIRECTORY cmake DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS)
 install(FILES rqt_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 
 install(CODE "

hironx_ros_bridge/idl_315_1_9/AbsoluteForceSensorService.idl

@@ -0,0 +1,31 @@
+/**
+ * @file AbsoluteForceSensorService.idl
+ * @brief Services for the absolute force sensor interface
+ */
+module OpenHRP
+{
+  interface AbsoluteForceSensorService
+  {
+    typedef sequence<double, 3> DblSequence3;
+    struct forcemomentOffsetParam {
+      DblSequence3 force_offset;
+      DblSequence3 moment_offset;
+      DblSequence3 link_offset_centroid;
+      double link_offset_mass;
+    };
+
+    /**
+     * @brief set offset parameters.
+     * @param i_param new offset parameters
+     * @return true if set successfully, false otherwise
+     */
+    boolean setForceMomentOffsetParam(in string name, in forcemomentOffsetParam i_param);
+
+    /**
+     * @brief get offset parameters.
+     * @param name is name of the offset parameter set and i_param output offset parameters
+     * @return true if set successfully, false otherwise
+     */
+    boolean getForceMomentOffsetParam(in string name, out forcemomentOffsetParam i_param);
+  };
+};

hironx_ros_bridge/idl_315_1_9/ImpedanceControllerService.idl

@@ -0,0 +1,66 @@
+/**
+ * @file ImpedanceControllerService.idl
+ * @brief Services for the impedance interface
+ */
+module OpenHRP
+{
+
+  interface ImpedanceControllerService
+  {
+    typedef sequence<double, 3> DblSequence3;
+
+    struct impedanceParam {
+      string name;
+      string base_name;
+      string target_name;
+      double M_p;
+      double D_p;
+      double K_p;
+      double M_r;
+      double D_r;
+      double K_r;
+      DblSequence3 ref_force;
+      DblSequence3 force_gain;
+      DblSequence3 ref_moment;
+      DblSequence3 moment_gain;
+      double sr_gain;
+      double avoid_gain;
+      double reference_gain;
+      double manipulability_limit;
+    };
+
+    /**
+     * @brief set impedance parameters.
+     * @param i_param new impedance parameters
+     * @return true if set successfully, false otherwise
+     */
+    boolean setImpedanceControllerParam(in impedanceParam i_param);
+
+    /**
+     * @brief get impedance parameters.
+     * @param name is name of the impedance parameter set and i_param output impedance parameters
+     * @return true if set successfully, false otherwise
+     */
+    boolean getImpedanceControllerParam(in string name, out impedanceParam i_param);
+
+    /**
+     * @brief remove impedance parameters.
+     * @param name name of the impedance parameter set
+     * @return true if set successfully, false otherwise
+     */
+    boolean deleteImpedanceController(in string name);
+
+    /**
+     * @brief wait to finish deleting the impedance param object.
+     * @param name name of the impedance parameter set
+     */
+    void waitDeletingImpedanceController(in string name);
+
+    /**
+     * @brief remove impedance parameters and wait to finish deleting the impedance param object.
+     * @param name name of the impedance parameter set
+     * @return true if set successfully, false otherwise
+     */
+    boolean deleteImpedanceControllerAndWait(in string name);
+  };
+};

hironx_ros_bridge/launch/hironx_ros_bridge.launch

@@ -34,12 +34,20 @@
     <!-- In hrpsys_ros_bridge.launch, Collision check might be turned on -->
     <arg name="USE_COLLISIONCHECK" value="$(arg USE_COLLISIONCHECK)" />
     <arg name="USE_ROBOTHARDWARE" value="$(arg USE_ROBOTHARDWARE)" />
-    <arg name="USE_IMPEDANCECONTROLLER" value="$(arg USE_IMPEDANCECONTROLLER)" />
+    <arg name="USE_IMPEDANCECONTROLLER" value="false" />
     <arg name="USE_SOFTERRORLIMIT" value="false" />
     <arg name="USE_SERVOCONTROLLER" value="$(arg USE_SERVOCONTROLLER)" />
 
     <arg name="corbaport" default="$(arg corbaport)" />
   </include>
+
+  <!-- Launch ros bridge for old impedance controller in HiroNX -->
+  <include file="$(find hironx_ros_bridge)/launch/impedance_controller.launch" if="$(arg USE_IMPEDANCECONTROLLER)">
+    <arg name="corbaport" default="$(arg corbaport)" />
+    <arg name="nameserver" value="$(arg nameserver)" />
+    <arg name="SIMULATOR_NAME" value="$(arg SIMULATOR_NAME)" />
+  </include>
+
 <!--
   <node name="hironx_rviz" pkg="rviz" type="rviz" respawn="true"
   args="-sync -d $(find hrpsys_ros_bridge_tutorials)/launch/hironx.vcg"

hironx_ros_bridge/launch/hironx_ros_bridge_real.launch

@@ -7,7 +7,7 @@
   <env name="RTCTREE_NAMESERVERS" value="$(arg nameserver):$(arg corbaport)" />
   <arg name="USE_ROBOTHARDWARE" default="true" />
   <arg name="USE_SERVOCONTROLLER" default="true" />
-  <arg name="USE_IMPEDANCECONTROLLER" default="false"/>
+  <arg name="USE_IMPEDANCECONTROLLER" default="true"/>
   <arg name="USE_COLLISIONCHECK" default="false"/>
   <arg name="USE_HAND_JOINT_STATE_PUBLISHER" default="true"/>
 

hironx_ros_bridge/launch/impedance_controller.launch

@@ -0,0 +1,23 @@
+<!-- Launch ros bridge for old impedance controller in HiroNX -->
+<launch>
+  <arg name="nameserver" default="localhost" />
+  <arg name="corbaport" default="15005" />
+  <arg name="SIMULATOR_NAME" default="RobotHardware0"/>
+  <env name="SIMULATOR_NAME" value="$(arg SIMULATOR_NAME)" />
+  <env name="RTCTREE_NAMESERVERS" value="$(arg nameserver):$(arg corbaport)" />
+
+  <arg name="omniorb_args" default="-ORBInitRef NameService=corbaloc:iiop:$(arg nameserver):$(arg corbaport)/NameService" />
+  <arg name="openrtm_args" default='-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"' />
+
+  <!-- Launch ImpedanceController and AbsoluteForceSensor -->
+  <node pkg="hironx_ros_bridge" name="ImpedanceControllerServiceROSBridge" type="ImpedanceControllerServiceROSBridgeComp"
+        output="screen" args ="$(arg openrtm_args)" />
+  <node pkg="hironx_ros_bridge" name="AbsoluteForceSensorServiceROSBridge" type="AbsoluteForceSensorServiceROSBridgeComp"
+        output="screen" args="$(arg openrtm_args)" />
+  <rtconnect from="ImpedanceControllerServiceROSBridge.rtc:ImpedanceControllerService" to="ic.rtc:ImpedanceControllerService"  subscription_type="new"/>
+  <rtactivate component="ImpedanceControllerServiceROSBridge.rtc" />
+  <rtconnect from="AbsoluteForceSensorServiceROSBridge.rtc:AbsoluteForceSensorService" to="rmfo.rtc:AbsoluteForceSensorService"  subscription_type="new"/>
+  <rtactivate component="AbsoluteForceSensorServiceROSBridge.rtc" />
+  <node name="rtmlaunch_impedance_controller" pkg="openrtm_tools" type="rtmlaunch.py"
+        args="$(find hironx_ros_bridge)/launch/impedance_controller.launch" />
+</launch>

hironx_ros_bridge/package.xml

@@ -29,6 +29,7 @@
   <build_depend>rosbash</build_depend>
   <build_depend>rosbuild</build_depend>
   <build_depend>roslang</build_depend>
+  <build_depend>rtmbuild</build_depend>
   <build_depend>unzip</build_depend>
   <build_depend>gnuplot</build_depend>