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":