diff --git a/hrpsys_ros_bridge_tutorials/CMakeLists.txt b/hrpsys_ros_bridge_tutorials/CMakeLists.txt
index 09a63050..b593f8bd 100644
--- a/hrpsys_ros_bridge_tutorials/CMakeLists.txt
+++ b/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
diff --git a/hrpsys_ros_bridge_tutorials/models/kxrl2w2l6a4h2.yaml b/hrpsys_ros_bridge_tutorials/models/kxrl2w2l6a4h2.yaml
new file mode 100644
index 00000000..a465b2ec
--- /dev/null
+++ b/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: '\n 1000000.0\n 100.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n '
+ - match_rule:
+ tag: gazebo
+ attribute_name: reference
+ attribute_value: RLEG-FOOT_S
+ replaced_xml: '\n 1000000.0\n 100.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n '
+ - 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
diff --git a/hrpsys_ros_bridge_tutorials/package.xml b/hrpsys_ros_bridge_tutorials/package.xml
index ecea66cc..40999040 100644
--- a/hrpsys_ros_bridge_tutorials/package.xml
+++ b/hrpsys_ros_bridge_tutorials/package.xml
@@ -53,5 +53,7 @@
+
+
diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/kxrl2w2l6a4h2_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/kxrl2w2l6a4h2_hrpsys_config.py
new file mode 100755
index 00000000..eb71ac0b
--- /dev/null
+++ b/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()
diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py
index 91465817..7a1883d1 100755
--- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py
+++ b/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":