[hrpsys_ros_bridge_tutorials] Add kxr model

hrpsys_ros_bridge_tutorials/CMakeLists.txt

@@ -14,6 +14,11 @@ if(NOT "${jsk_models_FOUND}")
   string(ASCII 27 Esc)
   message(WARNING "${Esc}[1;33mPackage jsk_models is not found, if you have right to access them please include source code in catkin workspace${Esc}[m")
 endif()
+find_package(kondo_models QUIET)
+if(NOT "${kondo_models_FOUND}")
+  string(ASCII 27 Esc)
+  message(WARNING "${Esc}[1;33mPackage kondo_models is not found, if you have right to access them please include source code in catkin workspace${Esc}[m")
+endif()
 
 include(FindPkgConfig)
 pkg_check_modules(openhrp3 REQUIRED openhrp3.1)
@@ -96,6 +101,12 @@ macro(compile_rbrain_model_for_closed_robots _OpenHRP2_robot_vrml_name _OpenHRP2
     ${_OpenHRP2_robot_name}
     ${ARGN})
 endmacro()
+macro(compile_kondo_model_for_closed_robots _OpenHRP2_robot_vrml_name _OpenHRP2_robot_dir _OpenHRP2_robot_name)
+  compile_model_for_closed_robots(
+    ${kondo_models_MODEL_DIR}/${_OpenHRP2_robot_dir}/${_OpenHRP2_robot_vrml_name}main.wrl
+    ${_OpenHRP2_robot_name}
+    ${ARGN})
+endmacro()
 macro(gen_minmax_table_for_closed_robots _OpenHRP2_robot_vrml_name _OpenHRP2_robot_dir _OpenHRP2_robot_name)
   if (EXISTS ${hrp2_models_MODEL_DIR}/${_OpenHRP2_robot_dir}/${_OpenHRP2_robot_vrml_name}main.wrl OR
       EXISTS ${jsk_models_MODEL_DIR}/${_OpenHRP2_robot_dir}/${_OpenHRP2_robot_vrml_name}main.wrl)
@@ -430,6 +441,22 @@ if(EXISTS ${PROJECT_SOURCE_DIR}/models/TESTMDOFARM.wrl)
     TESTMDOFARM)
 endif()
 
+# Kondo Kagaku KXR series
+# KXRL2W2L6A4H2
+compile_kondo_model_for_closed_robots(KXRL2W2L6A4H2 KXRL2W2L6A4H2 KXRL2W2L6A4H2
+  --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
+  ## do not use CHIDORI.conf for real robot
+  --conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/KXRL2W2L6A4H2.PDgains.sav"
+  --conf-file-option "pdgains_sim_file_name: ${PROJECT_SOURCE_DIR}/models/KXRL2W2L6A4H2.PDgains_sim.dat"
+  --conf-file-option "abc_leg_offset: 0.0, 0.1, 0.0"
+  --conf-file-option "end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,0.0,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,0.0,0.0,0.0,0.0,0.0,rarm,RARM_JOINT3,WAIST,0.0,0.0,0.0,0.0,0.0,0.0,0.0, larm,LARM_JOINT3,WAIST,0.0,0.0,0.0,0.0,0.0,0.0,0.0,"
+  --conf-dt-option "0.01"
+  --simulation-timestep-option "0.01"
+  --conf-file-option "collision_loop: 20"
+  --conf-file-option "collision_pair: WAIST:LLEG_JOINT2 WAIST:RLEG_JOINT2 LLEG_JOINT2:LLEG_JOINT5 LLEG_JOINT2:RLEG_JOINT2 LLEG_JOINT2:RLEG_JOINT3 LLEG_JOINT2:RLEG_JOINT5 LLEG_JOINT3:RLEG_JOINT2 LLEG_JOINT3:RLEG_JOINT3 LLEG_JOINT3:RLEG_JOINT5 LLEG_JOINT5:RLEG_JOINT2 LLEG_JOINT5:RLEG_JOINT3 LLEG_JOINT5:RLEG_JOINT5 RLEG_JOINT2:RLEG_JOINT5"
+  --conf-file-option "# SequencePlayer optional data (contactStates x 2 + controlSwingTime x 2 (2 is rfsensor, lfsensor)"
+  --conf-file-option "seq_optional_data_dim: 4"
+  )
 
 ################################
 ## Generate default simulation launch files and euslisp interface files
@@ -450,6 +477,14 @@ macro (generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots R
     generate_default_launch_eusinterface_files("$(find jsk_models)/${ROBOT_DIR}/${ROBOT_NAME}main.wrl" hrpsys_ros_bridge_tutorials ${ROBOT_NAME} ${_arg_list})
   endif()
 endmacro ()
+macro (generate_default_launch_eusinterface_files_for_closed_kondo_robots ROBOT_DIR ROBOT_NAME)
+  set(_arg_list ${ARGV})
+  # remove arguments of this macro
+  list(REMOVE_AT _arg_list 0 1)
+  if(EXISTS ${kondo_models_MODEL_DIR}/${ROBOT_DIR}/${ROBOT_NAME}main.wrl)
+    generate_default_launch_eusinterface_files("$(find kondo_models)/${ROBOT_DIR}/${ROBOT_NAME}main.wrl" hrpsys_ros_bridge_tutorials ${ROBOT_NAME} ${_arg_list})
+  endif()
+endmacro ()
 
 generate_default_launch_eusinterface_files_for_jsk_closed_openhrp_robots(HRP2JSK_for_OpenHRP3 HRP2JSK "--use-robot-hrpsys-config")
 generate_default_launch_eusinterface_files_for_jsk_closed_openhrp_robots(HRP2JSKNT_for_OpenHRP3 HRP2JSKNT "--use-robot-hrpsys-config")
@@ -467,6 +502,8 @@ generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(URATALEG
 generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(YSTLEG YSTLEG "--use-robot-hrpsys-config")
 generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(CHIDORI CHIDORI "--use-robot-hrpsys-config")
 generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(TQLEG0 TQLEG0 "--use-robot-hrpsys-config")
+generate_default_launch_eusinterface_files_for_closed_kondo_robots(KXRL2W2L6A4H2 KXRL2W2L6A4H2 "--use-robot-hrpsys-config")
+
 generate_default_launch_eusinterface_files(
   "$(find hrpsys_ros_bridge_tutorials)/models/TESTMDOFARM.wrl"
   hrpsys_ros_bridge_tutorials

hrpsys_ros_bridge_tutorials/models/kxrl2w2l6a4h2.yaml

@@ -0,0 +1,101 @@
+##
+## - collada_joint_name : euslisp_joint_name (start with :)
+##
+
+rleg:
+  - RLEG_JOINT0 : rleg-crotch-y
+  - RLEG_JOINT1 : rleg-crotch-r
+  - RLEG_JOINTW : rleg-crotch-w
+  - RLEG_JOINT2 : rleg-crotch-p
+  - RLEG_JOINT3 : rleg-knee-p
+  - RLEG_JOINT4 : rleg-ankle-p
+  - RLEG_JOINT5 : rleg-ankle-r
+lleg:
+  - LLEG_JOINT0 : lleg-crotch-y
+  - LLEG_JOINT1 : lleg-crotch-r
+  - LLEG_JOINTW : lleg-crotch-w
+  - LLEG_JOINT2 : lleg-crotch-p
+  - LLEG_JOINT3 : lleg-knee-p
+  - LLEG_JOINT4 : lleg-ankle-p
+  - LLEG_JOINT5 : lleg-ankle-r
+head:
+  - HEAD_JOINT0 : head-neck-y
+  - HEAD_JOINT1 : head-neck-p
+rarm:
+  - RARM_JOINT0 : rarm-shoulder-p
+  - RARM_JOINT1 : rarm-shoulder-r
+  - RARM_JOINT2 : rarm-shoulder-y
+  - RARM_JOINT3 : rarm-elbow-p
+  - RARM_JOINT4 : rarm-gripper-r
+larm:
+  - LARM_JOINT0 : larm-shoulder-p
+  - LARM_JOINT1 : larm-shoulder-r
+  - LARM_JOINT2 : larm-shoulder-y
+  - LARM_JOINT3 : larm-elbow-p
+  - LARM_JOINT4 : larm-gripper-r
+
+##
+## end-coords
+##
+larm-end-coords:
+  parent    : LARM_LINK3
+rarm-end-coords:
+  parent    : RARM_LINK3
+lleg-end-coords:
+  parent    : LLEG_LINK5
+rleg-end-coords:
+  parent    : RLEG_LINK5
+head-end-coords:
+  parent    : HEAD_LINK1
+
+#
+# reset-pose
+#
+angle-vector:
+    reset-pose : [0, 0, 0, 25, 50, 25, 0,  #rleg  , wheel
+                  0, 0, 0, 25, 50, 25, 0,  #lleg  , wheel
+                  0, 0,         #head
+                  0, 0, 0, 0, 0,      #rarm
+                  0, 0, 0, 0, 0       #larm
+                  ]
+    reset-manip-pose : [0, 0, 0, 15, 30, 15, 0,
+                        0, 0, 0, 15, 30, 15, 0,
+                        0, 0,              
+                        0, 0, 0, 0, 0,
+                        0, 0, 0, 0, 0
+                        ]
+
+sensors:
+### sensor position should be confirmed
+  - {sensor_name: 'rfsensor', sensor_type: 'base_force6d', parent_link: 'RLEG_LINK5', translate: '0 0 0', rotate: '0 1 0 180'}
+  - {sensor_name: 'lfsensor', sensor_type: 'base_force6d', parent_link: 'LLEG_LINK5', translate: '0 0 0', rotate: '0 1 0 180'}
+  - {sensor_name: 'rhsensor', sensor_type: 'base_force6d', parent_link: 'RARM_LINK3', translate: '0 0 0', rotate: '0 1 0 180'}
+  - {sensor_name: 'lhsensor', sensor_type: 'base_force6d', parent_link: 'LARM_LINK3', translate: '0 0 0', rotate: '0 1 0 180'}
+
+# for gazebo simulation
+replace_xmls:
+  - match_rule:
+      tag: gazebo
+      attribute_name: reference
+      attribute_value: LLEG-FOOT_S
+      replaced_xml: '<gazebo reference="LLEG-FOOT_S">\n    <kp>1000000.0</kp>\n    <kd>100.0</kd>\n    <mu1>1.5</mu1>\n    <mu2>1.5</mu2>\n    <fdir1>1 0 0</fdir1>\n    <maxVel>10.0</maxVel>\n    <minDepth>0.00</minDepth>\n  </gazebo>'
+  - match_rule:
+      tag: gazebo
+      attribute_name: reference
+      attribute_value: RLEG-FOOT_S
+      replaced_xml: '<gazebo reference="RLEG-FOOT_S">\n    <kp>1000000.0</kp>\n    <kd>100.0</kd>\n    <mu1>1.5</mu1>\n    <mu2>1.5</mu2>\n    <fdir1>1 0 0</fdir1>\n    <maxVel>10.0</maxVel>\n    <minDepth>0.00</minDepth>\n  </gazebo>'
+  - match_rule:
+      tag: limit
+      attribute_name: effort
+      attribute_value: 100
+    replaced_attribute_value: 200
+  - match_rule:
+      tag: limit
+      attribute_name: velocity
+      attribute_value: 0.5
+    replaced_attribute_value: 6.0
+  - match_rule:
+      tag: joint
+      attribute_name: type
+      attribute_value: continuous
+    replaced_attribute_value: revolute

hrpsys_ros_bridge_tutorials/package.xml

@@ -53,5 +53,7 @@
     <pr2eus robot-name="urataleg" interface-file="${prefix}/euslisp/urataleg-interface.l" />
     <pr2eus robot-name="chidori" interface-file="${prefix}/euslisp/chidori-interface.l" />
     <pr2eus robot-name="ystleg" interface-file="${prefix}/euslisp/ystleg-interface.l" />
+    <pr2eus robot-name="kxrl2w2l6a4h2" interface-file="${prefix}/euslisp/kxrl2w2l6a4h2-interface.l" />
+
   </export>
 </package>

hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/kxrl2w2l6a4h2_hrpsys_config.py

@@ -0,0 +1,12 @@
+#!/usr/bin/env python
+
+from urata_hrpsys_config import *
+
+if __name__ == '__main__':
+    hcf = URATAHrpsysConfigurator("KXRL2W2L6A4H2")
+    if len(sys.argv) > 2 :
+        hcf.init(sys.argv[1], sys.argv[2])
+    elif len(sys.argv) > 1 :
+        hcf.init(sys.argv[1], sys.argv[2])
+    else :
+        hcf.init()

hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py

@@ -49,6 +49,13 @@ def defJointGroups (self):
             head_group = ['head', ['HEAD_JOINT0', 'HEAD_JOINT1']]
             torso_group = ['torso', ['CHEST_JOINT0', 'CHEST_JOINT1', 'CHEST_JOINT2']]
             self.Groups = [rarm_group, larm_group, rleg_group, lleg_group, head_group, torso_group]
+        elif self.ROBOT_NAME == "KXRL2W2L6A4H2":
+            rarm_group = ['rarm', ['RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4']]
+            larm_group = ['larm', ['LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4']]
+            rleg_group = ['rleg', ['RLEG_JOINT0', 'RLEG_JOINT1', 'RLEG_JOINT2', 'RLEG_JOINT3', 'RLEG_JOINT4', 'RLEG_JOINT5']]
+            lleg_group = ['lleg', ['LLEG_JOINT0', 'LLEG_JOINT1', 'LLEG_JOINT2', 'LLEG_JOINT3', 'LLEG_JOINT4', 'LLEG_JOINT5']]
+            head_group = ['head', ['HEAD_JOINT0', 'HEAD_JOINT1']]
+            self.Groups = [rarm_group, larm_group, rleg_group, lleg_group, head_group]
         else:
             rarm_group = ['rarm', ['RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5', 'RARM_JOINT6', 'RARM_JOINT7']]
             larm_group = ['larm', ['LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'LARM_JOINT6', 'LARM_JOINT7']]
@@ -75,6 +82,8 @@ def setStAbcParameters (self):
             self.setStAbcParametersCHIDORI()
         elif self.ROBOT_NAME == "TQLEG0":
             self.setStAbcParametersTQLEG0()
+        elif self.ROBOT_NAME == "KXRL2W2L6A4H2":
+            self.setStAbcParametersKXRL2W2L6A4H2()
 
     def setStAbcParametersSTARO (self):
         # abc setting
@@ -616,6 +625,39 @@ def setStAbcParametersTQLEG0 (self):
         # remove soft-error-limit for torque controlled robot ( ONLY FOR THIS ROBOT !!! )
         self.el_svc.setServoErrorLimit("ALL", 100000)
 
+    def setStAbcParametersKXRL2W2L6A4H2 (self):
+        abcp=self.abc_svc.getAutoBalancerParam()[1]
+        abcp.default_zmp_offsets=[[0.01, -0.005, 0.0], [0.01, 0.005, 0.0], [0, 0, 0], [0, 0, 0]];
+#        abcp.move_base_gain=0.8
+        self.abc_svc.setAutoBalancerParam(abcp)
+        # ST parameters
+        stp=self.st_svc.getParameter()
+        stp.st_algorithm=OpenHRP.StabilizerService.EEFMQP
+        #   eefm st params
+        stp.eefm_leg_inside_margin=71.12*1e-3
+        stp.eefm_leg_outside_margin=71.12*1e-3
+        stp.eefm_leg_front_margin=182.0*1e-3
+        stp.eefm_leg_rear_margin=72.0*1e-3
+        stp.eefm_k1=[-1.39899,-1.39899]
+        stp.eefm_k2=[-0.386111,-0.386111]
+        stp.eefm_k3=[-0.175068,-0.175068]
+        stp.eefm_rot_damping_gain=[[20*1.6*10, 20*1.6*10, 1e5]]*4 # Stiff parameter for simulation
+        stp.eefm_pos_damping_gain=[[3500*50, 3500*50, 3500*1.0*5]]*4 # Stiff parameter for simulation
+        #   tpcc st params
+        stp.k_tpcc_p=[0.2, 0.2]
+        stp.k_tpcc_x=[4.0, 4.0]
+        stp.k_brot_p=[0.0, 0.0]
+        stp.k_brot_tc=[0.1, 0.1]
+        self.st_svc.setParameter(stp)
+        gg=self.abc_svc.getGaitGeneratorParam()[1]
+        gg.default_step_time=5
+        gg.default_step_height=0.015
+        gg.leg_default_translate_pos[0][1] = -0.027
+        gg.leg_default_translate_pos[1][1] = 0.027
+        gg.stride_parameter[0] = 0.025
+        gg.stride_parameter[1] = 0.025
+        gg.default_orbit_type = OpenHRP.AutoBalancerService.CYCLOIDDELAY
+        self.abc_svc.setGaitGeneratorParam(gg)
 
     def jaxonResetPose (self):
         ## Different from (send *robot* :reset-pose)
@@ -680,6 +722,9 @@ def tqleg0ResetPose(self):
     def tqleg0InitPose (self):
         return [0]*len(self.tqleg0ResetPose())
 
+    def kxrl2w2l6a4h2ResetPose(self):
+        return [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.45, 0.45, 0.9, 0.9, 0.45, 0.45, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+
     # (mapcar #'deg2rad (concatenate cons (send *robot* :reset-landing-pose)))
     def jaxonResetLandingPose (self):
         return [0.004318,0.005074,-0.134838,1.18092,-0.803855,-0.001463,0.004313,0.005079,-0.133569,1.18206,-0.806262,-0.001469,0.003782,-0.034907,0.004684,0.0,0.0,0.0,0.698132,-0.349066,-0.087266,-1.39626,0.0,0.0,-0.349066,0.0,0.698132,0.349066,0.087266,-1.39626,0.0,0.0,-0.349066]
@@ -704,6 +749,8 @@ def setResetPose(self):
             self.seq_svc.setJointAngles(self.chidoriResetPose(), 5.0)
         elif self.ROBOT_NAME == "TQLEG0":
             self.seq_svc.setJointAngles(self.tqleg0ResetPose(), 5.0)
+        elif self.ROBOT_NAME == "KXRL2W2L6A4H2":
+            self.seq_svc.setJointAngles(self.kxrl2w2l6a4h2ResetPose(), 5.0)
 
     def setResetManipPose(self):
         if self.ROBOT_NAME == "STARO":