From d8dc78bbb1a9bf166d84fb1b99a0f9a8fbcdd77e Mon Sep 17 00:00:00 2001
From: Yuki Asano <asano@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 2 Oct 2017 15:43:45 +0900
Subject: [PATCH] add khr model

---
 hrpsys_ros_bridge_tutorials/CMakeLists.txt    |  13 ++
 .../models/KHR.PDgain.dat                     | 212 ++++++++++++++++++
 hrpsys_ros_bridge_tutorials/models/khr.yaml   |  96 ++++++++
 3 files changed, 321 insertions(+)
 create mode 100644 hrpsys_ros_bridge_tutorials/models/KHR.PDgain.dat
 create mode 100644 hrpsys_ros_bridge_tutorials/models/khr.yaml

diff --git a/hrpsys_ros_bridge_tutorials/CMakeLists.txt b/hrpsys_ros_bridge_tutorials/CMakeLists.txt
index 24890c17..27ed30b7 100644
--- a/hrpsys_ros_bridge_tutorials/CMakeLists.txt
+++ b/hrpsys_ros_bridge_tutorials/CMakeLists.txt
@@ -277,6 +277,18 @@ if(EXISTS ${hrp2_models_MODEL_DIR}/HRP3HAND_R/HRP3HAND_Rmain.wrl)
     HRP3HAND_R)
 endif()
 
+# KHR
+compile_openhrp_model_for_closed_robots(KHR KHR KHR
+  --conf-dt-option "0.002"
+  --simulation-timestep-option "0.002"
+  --conf-file-option "abc_leg_offset: 0,0.09,0"
+  --conf-file-option "abc_stride_parameter: 0.15,0.05,10"
+  --conf-file-option "end_effectors: lleg,LLEG_LINK4,WAIST,0.0,0.0,0.0,0.0,0.0,0.0,0.0, rleg,RLEG_LINK4,WAIST,0.0,0.0,0.0,0.0,0.0,0.0,0.0"
+  --conf-file-option "collision_pair: HEAD_LINK0:LARM_LINK2"
+  --conf-file-option "pdgains_sim_file_name: ${PROJECT_SOURCE_DIR}/models/KHR.PDgain.dat"
+  --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
+)
+
 # URATALEG
 compile_rbrain_model_for_closed_robots(URATALEG URATALEG URATALEG
   --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
@@ -433,6 +445,7 @@ generate_default_launch_eusinterface_files_for_jsk_closed_openhrp_robots(HRP2JSK
 generate_default_launch_eusinterface_files_for_jsk_closed_openhrp_robots(HRP2W_for_OpenHRP3 HRP2W "--use-robot-hrpsys-config")
 generate_default_launch_eusinterface_files_for_jsk_closed_openhrp_robots(HRP2G_for_OpenHRP3 HRP2G "--use-robot-hrpsys-config")
 generate_default_launch_eusinterface_files_for_jsk_closed_openhrp_robots(HRP4R HRP4R "--use-unstable-hrpsys-config")
+generate_default_launch_eusinterface_files_for_jsk_closed_openhrp_robots(KHR KHR "--use-robot-hrpsys-config")
 generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(STARO STARO "--use-unstable-hrpsys-config")
 generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(JAXON JAXON "--use-robot-hrpsys-config")
 if("$ENV{TRAVIS_JOB_ID}" STREQUAL "")
diff --git a/hrpsys_ros_bridge_tutorials/models/KHR.PDgain.dat b/hrpsys_ros_bridge_tutorials/models/KHR.PDgain.dat
new file mode 100644
index 00000000..424f762f
--- /dev/null
+++ b/hrpsys_ros_bridge_tutorials/models/KHR.PDgain.dat
@@ -0,0 +1,212 @@
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
+12000 700
diff --git a/hrpsys_ros_bridge_tutorials/models/khr.yaml b/hrpsys_ros_bridge_tutorials/models/khr.yaml
new file mode 100644
index 00000000..12e8fc65
--- /dev/null
+++ b/hrpsys_ros_bridge_tutorials/models/khr.yaml
@@ -0,0 +1,96 @@
+##
+## - collada_joint_name : euslisp_joint_name (start with :)
+##
+
+rleg:
+  - RLEG_JOINT0 : rleg-crotch-r
+  - RLEG_JOINT1 : rleg-crotch-p
+  - RLEG_JOINT2 : rleg-knee-p
+  - RLEG_JOINT3 : rleg-ankle-p
+  - RLEG_JOINT4 : rleg-ankle-r
+lleg:
+  - LLEG_JOINT0 : lleg-crotch-r
+  - LLEG_JOINT1 : lleg-crotch-p
+  - LLEG_JOINT2 : lleg-knee-p
+  - LLEG_JOINT3 : lleg-ankle-p
+  - LLEG_JOINT4 : lleg-ankle-r
+torso:
+  - CHEST_JOINT0 : torso-chest-y
+head:
+  - HEAD_JOINT0 : head-neck-r
+rarm:
+  - RARM_JOINT0 : rarm-shoulder-p
+  - RARM_JOINT1 : rarm-shoulder-r
+  - RARM_JOINT2 : rarm-elbow-p
+larm:
+  - LARM_JOINT0 : larm-shoulder-p
+  - LARM_JOINT1 : larm-shoulder-r
+  - LARM_JOINT2 : larm-elbow-p
+
+##
+## end-coords
+##
+larm-end-coords:
+  #translate : [0, 0, -0.12]
+  #rotate    : [0, 1,  0, 0]
+  parent    : LARM_LINK2
+rarm-end-coords:
+  parent    : RARM_LINK2
+lleg-end-coords:
+  parent    : LLEG_LINK4
+rleg-end-coords:
+  parent    : RLEG_LINK4
+head-end-coords:
+  parent    : HEAD_LINK0
+
+#
+# reset-pose
+#
+angle-vector:
+# old reset-pose
+#  reset-pose : [0.0, -20.0, 0.0, 47.0, -27.0, 0.0, # rleg
+#                30.0, 0.0, 0.0, -60.0,  9.0, -6.5,  36.5, # rarm
+#                0.0, -20.0, 0.0, 47.0, -27.0, 0.0, # lleg
+#                30.0, 0.0, 0.0, -60.0, -9.0, -6.5, -36.5, # larm
+#                0.0, 0.0, 0.0] # torso
+#   openhrp reset pose from the initial line of OpenHRP-3.1/sample/controller/SampleController/etc/Sample.pos
+    reset-pose : [0, -25, 50, -25, 0, #rleg  
+                  0, -25, 50, -25, 0, #lleg
+                  0, 0,          #torso, head
+                  0, 0, 0,       #rarm
+                  0, 0, 0        #larm
+                  ]
+    reset-manip-pose : [0, -15, 30, -15, 0, 
+                        0, -15, 30, -15, 0,   
+                        0, 0,              
+                        0, 0, 0,
+                        0, 0, 0
+                        ]
+
+# 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