From a7aa41ba2b864c8f810872b56468d6b913737868 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Sun, 13 Jun 2021 14:34:06 +0900 Subject: [PATCH] [hrpsys_choreonoid_tutorials] add HRP2JSK* --- hrpsys_choreonoid_tutorials/CMakeLists.txt | 29 ++ .../config/BodyRTC_HRP2JSK.RH.conf | 92 ++++ .../config/BodyRTC_HRP2JSKNT.RH.conf | 92 ++++ .../config/BodyRTC_HRP2JSKNTS.RH.conf | 92 ++++ .../config/HRP2JSKCustomizer.yaml | 5 + .../config/HRP2JSKNTCustomizer.yaml | 21 + .../config/HRP2JSKNTSCustomizer.yaml | 21 + .../config/HRP2JSKNTS_RH_LOAD_OBJ.cnoid.in | 413 ++++++++++++++++++ .../config/HRP2JSKNT_RH_LOAD_OBJ.cnoid.in | 372 ++++++++++++++++ .../config/HRP2JSK_RH_LOAD_OBJ.cnoid.in | 368 ++++++++++++++++ .../launch/hrp2jsk_choreonoid.launch | 48 ++ .../launch/hrp2jsknt_choreonoid.launch | 48 ++ .../launch/hrp2jsknts_choreonoid.launch | 61 +++ .../launch/hrp2jsknts_vision_connect.launch | 83 ++++ .../scripts/hrp2jsk_rh_setup.py | 14 + .../scripts/hrp2jsknt_rh_setup.py | 14 + .../scripts/hrp2jsknts_rh_setup.py | 14 + .../hrp2_choreonoid_hrpsys_config.py | 78 ++++ 18 files changed, 1865 insertions(+) create mode 100644 hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSK.RH.conf create mode 100644 hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSKNT.RH.conf create mode 100644 hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSKNTS.RH.conf create mode 100644 hrpsys_choreonoid_tutorials/config/HRP2JSKCustomizer.yaml create mode 100644 hrpsys_choreonoid_tutorials/config/HRP2JSKNTCustomizer.yaml create mode 100644 hrpsys_choreonoid_tutorials/config/HRP2JSKNTSCustomizer.yaml create mode 100644 hrpsys_choreonoid_tutorials/config/HRP2JSKNTS_RH_LOAD_OBJ.cnoid.in create mode 100644 hrpsys_choreonoid_tutorials/config/HRP2JSKNT_RH_LOAD_OBJ.cnoid.in create mode 100644 hrpsys_choreonoid_tutorials/config/HRP2JSK_RH_LOAD_OBJ.cnoid.in create mode 100644 hrpsys_choreonoid_tutorials/launch/hrp2jsk_choreonoid.launch create mode 100644 hrpsys_choreonoid_tutorials/launch/hrp2jsknt_choreonoid.launch create mode 100644 hrpsys_choreonoid_tutorials/launch/hrp2jsknts_choreonoid.launch create mode 100644 hrpsys_choreonoid_tutorials/launch/hrp2jsknts_vision_connect.launch create mode 100755 hrpsys_choreonoid_tutorials/scripts/hrp2jsk_rh_setup.py create mode 100755 hrpsys_choreonoid_tutorials/scripts/hrp2jsknt_rh_setup.py create mode 100755 hrpsys_choreonoid_tutorials/scripts/hrp2jsknts_rh_setup.py create mode 100755 hrpsys_choreonoid_tutorials/src/hrpsys_choreonoid_tutorials/hrp2_choreonoid_hrpsys_config.py diff --git a/hrpsys_choreonoid_tutorials/CMakeLists.txt b/hrpsys_choreonoid_tutorials/CMakeLists.txt index 159ab4ab..9e2b2581 100644 --- a/hrpsys_choreonoid_tutorials/CMakeLists.txt +++ b/hrpsys_choreonoid_tutorials/CMakeLists.txt @@ -27,6 +27,14 @@ else() set (JSK_MODELS_DIR ${jsk_models_SOURCE_PREFIX}) endif() +find_package(hrp2_models QUIET) +if(NOT "${hrp2_models_FOUND}") + string(ASCII 27 Esc) + message(WARNING "${Esc}[1;33mPackage hrp2_models is not found, if you have right to access them please include source code in catkin workspace${Esc}[m") +else() + set (HRP2_MODELS_DIR ${hrp2_models_SOURCE_PREFIX}) +endif() + ################################ ## compile_openhrp_model ## Generate OpenHRP3 .xml and .conf file. @@ -198,6 +206,27 @@ configure_file(${PROJECT_SOURCE_DIR}/config/TABLIS_RH_FLAT.cnoid.in ${PROJECT_SO configure_file(${PROJECT_SOURCE_DIR}/config/TABLIS_BASE_RH_FLAT.cnoid.in ${PROJECT_SOURCE_DIR}/config/TABLIS_BASE_RH_FLAT.cnoid @ONLY) endif() +### +#HRP2JSK conid +### +if (${hrp2_models_FOUND}) +configure_file(${PROJECT_SOURCE_DIR}/config/HRP2JSK_RH_LOAD_OBJ.cnoid.in ${PROJECT_SOURCE_DIR}/config/HRP2JSK_RH_LOAD_OBJ.cnoid @ONLY) +endif() + +### +#HRP2JSKNT conid +### +if (${hrp2_models_FOUND}) +configure_file(${PROJECT_SOURCE_DIR}/config/HRP2JSKNT_RH_LOAD_OBJ.cnoid.in ${PROJECT_SOURCE_DIR}/config/HRP2JSKNT_RH_LOAD_OBJ.cnoid @ONLY) +endif() + +### +#HRP2JSKNTS conid +### +if (${hrp2_models_FOUND}) +configure_file(${PROJECT_SOURCE_DIR}/config/HRP2JSKNTS_RH_LOAD_OBJ.cnoid.in ${PROJECT_SOURCE_DIR}/config/HRP2JSKNTS_RH_LOAD_OBJ.cnoid @ONLY) +endif() + ### # scene.yaml ### diff --git a/hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSK.RH.conf b/hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSK.RH.conf new file mode 100644 index 00000000..38c1677d --- /dev/null +++ b/hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSK.RH.conf @@ -0,0 +1,92 @@ +## +name-server = localhost:15005 +## +## PD Controller +## in: angleRef, angle +## out: torque +## +in-port = tauIn:JOINT_TORQUE +out-port = angleOut:JOINT_VALUE +out-port = qvel:JOINT_VELOCITY +out-port = torque:JOINT_TORQUE +# out-port = ddq:JOINT_ACCELERATION +connection = tauIn:RobotHardware_choreonoid0:torqueOut +connection = angleOut:RobotHardware_choreonoid0:angleIn +connection = qvel:RobotHardware_choreonoid0:qvel_sim +connection = torque:RobotHardware_choreonoid0:torque_sim +### +# debug ## ground truth robot potition +### +out-port = WAIST:WAIST:ABS_TRANSFORM +# out-port = headq:motor_joint:JOINT_VALUE +#### +# sensors +#### +out-port = rfsensor_sim:rfsensor:FORCE_SENSOR +out-port = lfsensor_sim:lfsensor:FORCE_SENSOR +out-port = rhsensor_sim:rhsensor:FORCE_SENSOR +out-port = lhsensor_sim:lhsensor:FORCE_SENSOR +out-port = gsensor_sim:gsensor:ACCELERATION_SENSOR2 +out-port = gyrometer_sim:gyrometer:RATE_GYRO_SENSOR2 +connection = rfsensor_sim:RobotHardware_choreonoid0:rfsensor_sim +connection = lfsensor_sim:RobotHardware_choreonoid0:lfsensor_sim +connection = rhsensor_sim:RobotHardware_choreonoid0:rhsensor_sim +connection = lhsensor_sim:RobotHardware_choreonoid0:lhsensor_sim +connection = gsensor_sim:RobotHardware_choreonoid0:gsensor_sim +connection = gyrometer_sim:RobotHardware_choreonoid0:gyrometer_sim +#### +# vision +#### +# out-port = HEAD_RANGE:HEAD_RANGE:RANGE_SENSOR +# out-port = HEAD_LEFT_DEPTH:HEAD_LEFT_CAMERA:CAMERA_RANGE +# out-port = HEAD_LEFT_CAMERA:HEAD_LEFT_CAMERA:CAMERA_IMAGE +# out-port = HEAD_RIGHT_CAMERA:HEAD_RIGHT_CAMERA:CAMERA_IMAGE +#out-port = CHEST_CAMERA:CHEST_CAMERA:CAMERA_IMAGE +#out-port = LARM_CAMERA:LARM_CAMERA:CAMERA_IMAGE +#out-port = RARM_CAMERA:RARM_CAMERA:CAMERA_IMAGE +#out-port = LARM_CAMERA_N:LARM_CAMERA_N:CAMERA_IMAGE +#out-port = RARM_CAMERA_N:RARM_CAMERA_N:CAMERA_IMAGE + +#### +# constraint +#### +# out-port = F_BODY:WAIST:CONSTRAINT_FORCE +# out-port = F_CHEST_LINK0:CHEST_JOINT0:CONSTRAINT_FORCE +# out-port = F_CHEST_LINK1:CHEST_JOINT1:CONSTRAINT_FORCE +# out-port = F_CHEST_LINK2:CHEST_JOINT2:CONSTRAINT_FORCE +# out-port = F_HEAD_LINK0:HEAD_JOINT0:CONSTRAINT_FORCE +# out-port = F_HEAD_LINK1:HEAD_JOINT1:CONSTRAINT_FORCE +# out-port = F_LARM_LINK0:LARM_JOINT0:CONSTRAINT_FORCE +# out-port = F_LARM_LINK1:LARM_JOINT1:CONSTRAINT_FORCE +# out-port = F_LARM_LINK2:LARM_JOINT2:CONSTRAINT_FORCE +# out-port = F_LARM_LINK3:LARM_JOINT3:CONSTRAINT_FORCE +# out-port = F_LARM_LINK4:LARM_JOINT4:CONSTRAINT_FORCE +# out-port = F_LARM_LINK5:LARM_JOINT5:CONSTRAINT_FORCE +# out-port = F_LARM_LINK6:LARM_JOINT6:CONSTRAINT_FORCE +# out-port = F_LARM_LINK7:LARM_JOINT7:CONSTRAINT_FORCE +# out-port = F_LARM_FINGER0:LARM_F_JOINT0:CONSTRAINT_FORCE +# out-port = F_LARM_FINGER1:LARM_F_JOINT1:CONSTRAINT_FORCE +# out-port = F_RARM_LINK0:RARM_JOINT0:CONSTRAINT_FORCE +# out-port = F_RARM_LINK1:RARM_JOINT1:CONSTRAINT_FORCE +# out-port = F_RARM_LINK2:RARM_JOINT2:CONSTRAINT_FORCE +# out-port = F_RARM_LINK3:RARM_JOINT3:CONSTRAINT_FORCE +# out-port = F_RARM_LINK4:RARM_JOINT4:CONSTRAINT_FORCE +# out-port = F_RARM_LINK5:RARM_JOINT5:CONSTRAINT_FORCE +# out-port = F_RARM_LINK6:RARM_JOINT6:CONSTRAINT_FORCE +# out-port = F_RARM_LINK7:RARM_JOINT7:CONSTRAINT_FORCE +# out-port = F_RARM_FINGER0:RARM_F_JOINT0:CONSTRAINT_FORCE +# out-port = F_RARM_FINGER1:RARM_F_JOINT1:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK0:LLEG_JOINT0:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK1:LLEG_JOINT1:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK2:LLEG_JOINT2:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK3:LLEG_JOINT3:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK4:LLEG_JOINT4:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK5_UPPER:LLEG_JOINT5:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK5_LOWER:LLEG_BUSH_PITCH:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK0:RLEG_JOINT0:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK1:RLEG_JOINT1:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK2:RLEG_JOINT2:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK3:RLEG_JOINT3:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK4:RLEG_JOINT4:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK5_UPPER:RLEG_JOINT5:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK5_LOWER:RLEG_BUSH_PITCH:CONSTRAINT_FORCE diff --git a/hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSKNT.RH.conf b/hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSKNT.RH.conf new file mode 100644 index 00000000..38c1677d --- /dev/null +++ b/hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSKNT.RH.conf @@ -0,0 +1,92 @@ +## +name-server = localhost:15005 +## +## PD Controller +## in: angleRef, angle +## out: torque +## +in-port = tauIn:JOINT_TORQUE +out-port = angleOut:JOINT_VALUE +out-port = qvel:JOINT_VELOCITY +out-port = torque:JOINT_TORQUE +# out-port = ddq:JOINT_ACCELERATION +connection = tauIn:RobotHardware_choreonoid0:torqueOut +connection = angleOut:RobotHardware_choreonoid0:angleIn +connection = qvel:RobotHardware_choreonoid0:qvel_sim +connection = torque:RobotHardware_choreonoid0:torque_sim +### +# debug ## ground truth robot potition +### +out-port = WAIST:WAIST:ABS_TRANSFORM +# out-port = headq:motor_joint:JOINT_VALUE +#### +# sensors +#### +out-port = rfsensor_sim:rfsensor:FORCE_SENSOR +out-port = lfsensor_sim:lfsensor:FORCE_SENSOR +out-port = rhsensor_sim:rhsensor:FORCE_SENSOR +out-port = lhsensor_sim:lhsensor:FORCE_SENSOR +out-port = gsensor_sim:gsensor:ACCELERATION_SENSOR2 +out-port = gyrometer_sim:gyrometer:RATE_GYRO_SENSOR2 +connection = rfsensor_sim:RobotHardware_choreonoid0:rfsensor_sim +connection = lfsensor_sim:RobotHardware_choreonoid0:lfsensor_sim +connection = rhsensor_sim:RobotHardware_choreonoid0:rhsensor_sim +connection = lhsensor_sim:RobotHardware_choreonoid0:lhsensor_sim +connection = gsensor_sim:RobotHardware_choreonoid0:gsensor_sim +connection = gyrometer_sim:RobotHardware_choreonoid0:gyrometer_sim +#### +# vision +#### +# out-port = HEAD_RANGE:HEAD_RANGE:RANGE_SENSOR +# out-port = HEAD_LEFT_DEPTH:HEAD_LEFT_CAMERA:CAMERA_RANGE +# out-port = HEAD_LEFT_CAMERA:HEAD_LEFT_CAMERA:CAMERA_IMAGE +# out-port = HEAD_RIGHT_CAMERA:HEAD_RIGHT_CAMERA:CAMERA_IMAGE +#out-port = CHEST_CAMERA:CHEST_CAMERA:CAMERA_IMAGE +#out-port = LARM_CAMERA:LARM_CAMERA:CAMERA_IMAGE +#out-port = RARM_CAMERA:RARM_CAMERA:CAMERA_IMAGE +#out-port = LARM_CAMERA_N:LARM_CAMERA_N:CAMERA_IMAGE +#out-port = RARM_CAMERA_N:RARM_CAMERA_N:CAMERA_IMAGE + +#### +# constraint +#### +# out-port = F_BODY:WAIST:CONSTRAINT_FORCE +# out-port = F_CHEST_LINK0:CHEST_JOINT0:CONSTRAINT_FORCE +# out-port = F_CHEST_LINK1:CHEST_JOINT1:CONSTRAINT_FORCE +# out-port = F_CHEST_LINK2:CHEST_JOINT2:CONSTRAINT_FORCE +# out-port = F_HEAD_LINK0:HEAD_JOINT0:CONSTRAINT_FORCE +# out-port = F_HEAD_LINK1:HEAD_JOINT1:CONSTRAINT_FORCE +# out-port = F_LARM_LINK0:LARM_JOINT0:CONSTRAINT_FORCE +# out-port = F_LARM_LINK1:LARM_JOINT1:CONSTRAINT_FORCE +# out-port = F_LARM_LINK2:LARM_JOINT2:CONSTRAINT_FORCE +# out-port = F_LARM_LINK3:LARM_JOINT3:CONSTRAINT_FORCE +# out-port = F_LARM_LINK4:LARM_JOINT4:CONSTRAINT_FORCE +# out-port = F_LARM_LINK5:LARM_JOINT5:CONSTRAINT_FORCE +# out-port = F_LARM_LINK6:LARM_JOINT6:CONSTRAINT_FORCE +# out-port = F_LARM_LINK7:LARM_JOINT7:CONSTRAINT_FORCE +# out-port = F_LARM_FINGER0:LARM_F_JOINT0:CONSTRAINT_FORCE +# out-port = F_LARM_FINGER1:LARM_F_JOINT1:CONSTRAINT_FORCE +# out-port = F_RARM_LINK0:RARM_JOINT0:CONSTRAINT_FORCE +# out-port = F_RARM_LINK1:RARM_JOINT1:CONSTRAINT_FORCE +# out-port = F_RARM_LINK2:RARM_JOINT2:CONSTRAINT_FORCE +# out-port = F_RARM_LINK3:RARM_JOINT3:CONSTRAINT_FORCE +# out-port = F_RARM_LINK4:RARM_JOINT4:CONSTRAINT_FORCE +# out-port = F_RARM_LINK5:RARM_JOINT5:CONSTRAINT_FORCE +# out-port = F_RARM_LINK6:RARM_JOINT6:CONSTRAINT_FORCE +# out-port = F_RARM_LINK7:RARM_JOINT7:CONSTRAINT_FORCE +# out-port = F_RARM_FINGER0:RARM_F_JOINT0:CONSTRAINT_FORCE +# out-port = F_RARM_FINGER1:RARM_F_JOINT1:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK0:LLEG_JOINT0:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK1:LLEG_JOINT1:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK2:LLEG_JOINT2:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK3:LLEG_JOINT3:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK4:LLEG_JOINT4:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK5_UPPER:LLEG_JOINT5:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK5_LOWER:LLEG_BUSH_PITCH:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK0:RLEG_JOINT0:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK1:RLEG_JOINT1:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK2:RLEG_JOINT2:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK3:RLEG_JOINT3:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK4:RLEG_JOINT4:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK5_UPPER:RLEG_JOINT5:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK5_LOWER:RLEG_BUSH_PITCH:CONSTRAINT_FORCE diff --git a/hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSKNTS.RH.conf b/hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSKNTS.RH.conf new file mode 100644 index 00000000..2db8001d --- /dev/null +++ b/hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSKNTS.RH.conf @@ -0,0 +1,92 @@ +## +name-server = localhost:15005 +## +## PD Controller +## in: angleRef, angle +## out: torque +## +in-port = tauIn:JOINT_TORQUE +out-port = angleOut:JOINT_VALUE +out-port = qvel:JOINT_VELOCITY +out-port = torque:JOINT_TORQUE +# out-port = ddq:JOINT_ACCELERATION +connection = tauIn:RobotHardware_choreonoid0:torqueOut +connection = angleOut:RobotHardware_choreonoid0:angleIn +connection = qvel:RobotHardware_choreonoid0:qvel_sim +connection = torque:RobotHardware_choreonoid0:torque_sim +### +# debug ## ground truth robot potition +### +out-port = WAIST:WAIST:ABS_TRANSFORM +# out-port = headq:motor_joint:JOINT_VALUE +#### +# sensors +#### +out-port = rfsensor_sim:rfsensor:FORCE_SENSOR +out-port = lfsensor_sim:lfsensor:FORCE_SENSOR +out-port = rhsensor_sim:rhsensor:FORCE_SENSOR +out-port = lhsensor_sim:lhsensor:FORCE_SENSOR +out-port = gsensor_sim:gsensor:ACCELERATION_SENSOR2 +out-port = gyrometer_sim:gyrometer:RATE_GYRO_SENSOR2 +connection = rfsensor_sim:RobotHardware_choreonoid0:rfsensor_sim +connection = lfsensor_sim:RobotHardware_choreonoid0:lfsensor_sim +connection = rhsensor_sim:RobotHardware_choreonoid0:rhsensor_sim +connection = lhsensor_sim:RobotHardware_choreonoid0:lhsensor_sim +connection = gsensor_sim:RobotHardware_choreonoid0:gsensor_sim +connection = gyrometer_sim:RobotHardware_choreonoid0:gyrometer_sim +#### +# vision +#### +#out-port = HEAD_RANGE:HEAD_RANGE:RANGE_SENSOR +out-port = HEAD_DEPTH:CARMINE_DEPTH:CAMERA_RANGE +out-port = HEAD_CAMERA:CARMINE:CAMERA_IMAGE +#out-port = HEAD_RIGHT_CAMERA:HEAD_RIGHT_CAMERA:CAMERA_IMAGE +#out-port = CHEST_CAMERA:CHEST_CAMERA:CAMERA_IMAGE +#out-port = LARM_CAMERA:LARM_CAMERA:CAMERA_IMAGE +#out-port = RARM_CAMERA:RARM_CAMERA:CAMERA_IMAGE +#out-port = LARM_CAMERA_N:LARM_CAMERA_N:CAMERA_IMAGE +#out-port = RARM_CAMERA_N:RARM_CAMERA_N:CAMERA_IMAGE + +#### +# constraint +#### +# out-port = F_BODY:WAIST:CONSTRAINT_FORCE +# out-port = F_CHEST_LINK0:CHEST_JOINT0:CONSTRAINT_FORCE +# out-port = F_CHEST_LINK1:CHEST_JOINT1:CONSTRAINT_FORCE +# out-port = F_CHEST_LINK2:CHEST_JOINT2:CONSTRAINT_FORCE +# out-port = F_HEAD_LINK0:HEAD_JOINT0:CONSTRAINT_FORCE +# out-port = F_HEAD_LINK1:HEAD_JOINT1:CONSTRAINT_FORCE +# out-port = F_LARM_LINK0:LARM_JOINT0:CONSTRAINT_FORCE +# out-port = F_LARM_LINK1:LARM_JOINT1:CONSTRAINT_FORCE +# out-port = F_LARM_LINK2:LARM_JOINT2:CONSTRAINT_FORCE +# out-port = F_LARM_LINK3:LARM_JOINT3:CONSTRAINT_FORCE +# out-port = F_LARM_LINK4:LARM_JOINT4:CONSTRAINT_FORCE +# out-port = F_LARM_LINK5:LARM_JOINT5:CONSTRAINT_FORCE +# out-port = F_LARM_LINK6:LARM_JOINT6:CONSTRAINT_FORCE +# out-port = F_LARM_LINK7:LARM_JOINT7:CONSTRAINT_FORCE +# out-port = F_LARM_FINGER0:LARM_F_JOINT0:CONSTRAINT_FORCE +# out-port = F_LARM_FINGER1:LARM_F_JOINT1:CONSTRAINT_FORCE +# out-port = F_RARM_LINK0:RARM_JOINT0:CONSTRAINT_FORCE +# out-port = F_RARM_LINK1:RARM_JOINT1:CONSTRAINT_FORCE +# out-port = F_RARM_LINK2:RARM_JOINT2:CONSTRAINT_FORCE +# out-port = F_RARM_LINK3:RARM_JOINT3:CONSTRAINT_FORCE +# out-port = F_RARM_LINK4:RARM_JOINT4:CONSTRAINT_FORCE +# out-port = F_RARM_LINK5:RARM_JOINT5:CONSTRAINT_FORCE +# out-port = F_RARM_LINK6:RARM_JOINT6:CONSTRAINT_FORCE +# out-port = F_RARM_LINK7:RARM_JOINT7:CONSTRAINT_FORCE +# out-port = F_RARM_FINGER0:RARM_F_JOINT0:CONSTRAINT_FORCE +# out-port = F_RARM_FINGER1:RARM_F_JOINT1:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK0:LLEG_JOINT0:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK1:LLEG_JOINT1:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK2:LLEG_JOINT2:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK3:LLEG_JOINT3:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK4:LLEG_JOINT4:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK5_UPPER:LLEG_JOINT5:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK5_LOWER:LLEG_BUSH_PITCH:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK0:RLEG_JOINT0:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK1:RLEG_JOINT1:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK2:RLEG_JOINT2:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK3:RLEG_JOINT3:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK4:RLEG_JOINT4:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK5_UPPER:RLEG_JOINT5:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK5_LOWER:RLEG_BUSH_PITCH:CONSTRAINT_FORCE diff --git a/hrpsys_choreonoid_tutorials/config/HRP2JSKCustomizer.yaml b/hrpsys_choreonoid_tutorials/config/HRP2JSKCustomizer.yaml new file mode 100644 index 00000000..f548fd68 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/config/HRP2JSKCustomizer.yaml @@ -0,0 +1,5 @@ +bush: + springT: 3.3e5 + dampingT: 3.3e2 + springR: 1.0e3 + dampingR: 1.0 diff --git a/hrpsys_choreonoid_tutorials/config/HRP2JSKNTCustomizer.yaml b/hrpsys_choreonoid_tutorials/config/HRP2JSKNTCustomizer.yaml new file mode 100644 index 00000000..eab5bf75 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/config/HRP2JSKNTCustomizer.yaml @@ -0,0 +1,21 @@ +bush: + RLEG: + springT: 3.3e5 + dampingT: 3.3e2 + springR: 1.0e3 + dampingR: 1.0 + LLEG: + springT: 3.3e5 + dampingT: 3.3e2 + springR: 1.0e3 + dampingR: 1.0 + RARM: + springT: 1.0e5 + dampingT: 1.0e3 + springR: 3.0e2 + dampingR: 3.0 + LARM: + springT: 1.0e5 + dampingT: 1.0e3 + springR: 3.0e2 + dampingR: 3.0 diff --git a/hrpsys_choreonoid_tutorials/config/HRP2JSKNTSCustomizer.yaml b/hrpsys_choreonoid_tutorials/config/HRP2JSKNTSCustomizer.yaml new file mode 100644 index 00000000..eab5bf75 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/config/HRP2JSKNTSCustomizer.yaml @@ -0,0 +1,21 @@ +bush: + RLEG: + springT: 3.3e5 + dampingT: 3.3e2 + springR: 1.0e3 + dampingR: 1.0 + LLEG: + springT: 3.3e5 + dampingT: 3.3e2 + springR: 1.0e3 + dampingR: 1.0 + RARM: + springT: 1.0e5 + dampingT: 1.0e3 + springR: 3.0e2 + dampingR: 3.0 + LARM: + springT: 1.0e5 + dampingT: 1.0e3 + springR: 3.0e2 + dampingR: 3.0 diff --git a/hrpsys_choreonoid_tutorials/config/HRP2JSKNTS_RH_LOAD_OBJ.cnoid.in b/hrpsys_choreonoid_tutorials/config/HRP2JSKNTS_RH_LOAD_OBJ.cnoid.in new file mode 100644 index 00000000..e85cdf87 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/config/HRP2JSKNTS_RH_LOAD_OBJ.cnoid.in @@ -0,0 +1,413 @@ +items: + id: 0 + name: "Root" + plugin: Base + class: RootItem + children: + - + id: 1 + name: "World" + plugin: Body + class: WorldItem + data: + collisionDetection: false + collisionDetector: AISTCollisionDetector + children: + - + id: 2 + name: "HRP2JSKNTS" + plugin: Body + class: BodyItem + data: + modelFile: "@HRP2_MODELS_DIR@/HRP2JSKNTS_WITH_3HAND_for_OpenHRP3/HRP2JSKNTSmain_bush.wrl" + currentBaseLink: "WAIST" + rootPosition: [ 0.0, 0.0, 0.650 ] + rootAttitude: [ + 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + jointPositions: [ + 0.000366, 0.002938, -0.383414, 0.868598, -0.485184, -0.002938, 0.000000, + 0.000366, 0.002938, -0.383414, 0.868598, -0.485184, -0.002938, 0.000000, + 0.000000, 0.000000, 0.000000, 0.000000, + 0.174533, -0.174533, 0.000000, -0.436332, 0.000000, 0.000000, -0.174533, 0.261799, + 0.174533, 0.174533, 0.000000, -0.436332, 0.000000, 0.000000, -0.174533, -0.261799, + 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, + 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000] + initialRootPosition: [ 0.0, 0.0, 0.650 ] + initialRootAttitude: [ + 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + initialJointPositions: [ + 0.000366, 0.002938, -0.383414, 0.868598, -0.485184, -0.002938, 0.000000, + 0.000366, 0.002938, -0.383414, 0.868598, -0.485184, -0.002938, 0.000000, + 0.000000, 0.000000, 0.000000, 0.000000, + 0.174533, -0.174533, 0.000000, -0.436332, 0.000000, 0.000000, -0.174533, 0.261799, + 0.174533, 0.174533, 0.000000, -0.436332, 0.000000, 0.000000, -0.174533, -0.261799, + 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, + 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000] + zmp: [ 0, 0, 0 ] + collisionDetection: true + selfCollisionDetection: false + isEditable: true + children: + - + id: 3 + name: "BodyRTC" + plugin: OpenRTM + class: BodyRTCItem + data: + isImmediateMode: true + moduleName: "@JVRC_RTC_DIRECTORY@/lib/RobotHardware_choreonoid" + confFileName: "@JVRC_CONF_DIRECTORY@/BodyRTC_HRP2JSKNTS.RH.conf" + configurationMode: Use Configuration File + AutoConnect: false + InstanceName: HRP2JSKNTS(Robot)0 + bodyPeriodicRate: 0.004 + - + id: 4 + name: "add_objects.py" + plugin: Python + class: PythonScriptItem + data: + file: "@JVRC_RTC_DIRECTORY@/launch/add_objects.py" + executionOnLoading: true + backgroundExecution: false + - + id: 5 + name: "AISTSimulator" + plugin: Body + class: AISTSimulatorItem + data: + realtimeSync: true + recording: full + timeRangeMode: TimeBar range + onlyActiveControlPeriod: true + timeLength: 12000 + allLinkPositionOutputMode: false + deviceStateOutput: true + controllerThreads: true + recordCollisionData: false + dynamicsMode: Forward dynamics + integrationMode: Runge Kutta + gravity: [ 0, 0, -9.80665 ] + staticFriction: 1 + slipFriction: 1 + cullingThresh: 0.005 + contactCullingDepth: 0.03 + errorCriterion: 0.001 + maxNumIterations: 1000 + contactCorrectionDepth: 0.0001 + contactCorrectionVelocityRatio: 1 + kinematicWalking: false + 2Dmode: false + children: + - + id: 7 + name: "GLVisionSimulator" + plugin: Body + class: GLVisionSimulatorItem + data: + enabled: true + targetBodies: [ HRP2JSKNTS ] + targetSensors: [ CARMINE ] + maxFrameRate: 1000 + maxLatency: 1 + recordVisionData: false + useThread: true + useThreadsForSensors: true + bestEffort: false + allSceneObjects: false + rangeSensorPrecisionRatio: 2 + depthError: 0 + enableHeadLight: true + enableAdditionalLights: true + - + id: 8 + name: "GLVisionSimulator" + plugin: Body + class: GLVisionSimulatorItem + data: + enabled: true + targetBodies: [ HRP2JSKNTS ] + targetSensors: [ CARMINE_DEPTH ] + maxFrameRate: 1000 + maxLatency: 1 + recordVisionData: false + useThread: true + useThreadsForSensors: true + bestEffort: false + allSceneObjects: false + rangeSensorPrecisionRatio: 2 + depthError: 0 + enableHeadLight: true + enableAdditionalLights: true + - + id: 9 + name: "ros_service_server.py" + plugin: Python + class: PythonScriptItem + data: + file: "@JVRC_RTC_DIRECTORY@/scripts/ros_service_server.py" + executionOnLoading: true + backgroundExecution: false + +views: + - + id: 0 + name: "CameraImage" + plugin: Base + class: ImageView + mounted: true + - + id: 1 + plugin: Base + class: ItemPropertyView + mounted: true + - + id: 2 + plugin: Base + class: ItemTreeView + mounted: true + state: + selected: [ 9 ] + checked: [ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, ] + expanded: [ 1 ] + - + id: 3 + plugin: Base + class: MessageView + mounted: true + - + id: 4 + plugin: Base + class: SceneView + mounted: true + state: + editMode: true + viewpointControlMode: thirdPerson + collisionLines: false + polygonMode: fill + defaultHeadLight: true + defaultHeadLightIntensity: 0.75 + headLightLightingFromBack: false + worldLight: true + worldLightIntensity: 0.5 + worldLightAmbient: 0.3 + additionalLights: true + floorGrid: true + floorGridSpan: 10 + floorGridInterval: 0.5 + xzGridSpan: 10 + xzGridInterval: 0.5 + xzGrid: false + yzGridSpan: 10 + yzGridInterval: 0.5 + texture: true + lineWidth: 1 + pointSize: 1 + normalVisualization: false + normalLength: 0.01 + coordinateAxes: true + showFPS: false + enableNewDisplayListDoubleRendering: false + useBufferForPicking: true + cameras: + - + camera: [ System, Perspective ] + isCurrent: true + fieldOfView: 0.6978 + near: 0.01 + far: 100 + eye: [ 2.7, -2.7, 2 ] + direction: [ -0.7, 0.7, -0.3 ] + up: [ -0.25, 0.25, 1] + - + camera: [ System, Orthographic ] + orthoHeight: 20 + near: 0.01 + far: 100 + backgroundColor: [ 0.100000001, 0.100000001, 0.300000012 ] + gridColor: [ 0.899999976, 0.899999976, 0.899999976, 1 ] + xzgridColor: [ 0.899999976, 0.899999976, 0.899999976, 1 ] + yzgridColor: [ 0.899999976, 0.899999976, 0.899999976, 1 ] + dedicatedItemTreeViewChecks: false + - + id: 5 + name: "Task" + plugin: Base + class: TaskView + state: + layoutMode: horizontal + isAutoMode: false + - + id: 6 + plugin: Body + class: BodyLinkView + mounted: true + state: + showRotationMatrix: false + - + id: 7 + plugin: Body + class: JointSliderView + mounted: true + state: + showAllJoints: true + jointId: false + name: true + numColumns: 1 + spinBox: true + slider: true + labelOnLeft: true + - + id: 8 + plugin: Body + class: LinkSelectionView + mounted: true + state: + listingMode: "Link List" + - + id: 10 + plugin: Python + class: PythonConsoleView + mounted: true +toolbars: + "TimeBar": + minTime: 0 + maxTime: 12000 + frameRate: 1000 + playbackFrameRate: 50 + idleLoopDrivenMode: false + currentTime: 0 + speedScale: 1 + syncToOngoingUpdates: true + autoExpansion: true + "KinematicsBar": + mode: AUTO + enablePositionDragger: true + penetrationBlock: false + collisionLinkHighlight: false + snapDistance: 0.025 + penetrationBlockDepth: 0.0005 + lazyCollisionDetectionMode: true + "LeggedBodyBar": + stanceWidth: 0.15 + "BodyMotionGenerationBar": + autoGenerationForNewBody: true + balancer: false + autoGeneration: false + timeScaleRatio: 1 + preInitialDuration: 1 + postFinalDuration: 1 + onlyTimeBarRange: false + makeNewBodyItem: true + stealthyStepMode: true + stealthyHeightRatioThresh: 2 + flatLiftingHeight: 0.005 + flatLandingHeight: 0.005 + impactReductionHeight: 0.005 + impactReductionTime: 0.04 + autoZmp: true + minZmpTransitionTime: 0.1 + zmpCenteringTimeThresh: 0.03 + zmpTimeMarginBeforeLiftingSpin: 0 + zmpMaxDistanceFromCenter: 0.02 + allLinkPositions: false + lipSyncMix: false + timeToStartBalancer: 0 + balancerIterations: 2 + plainBalancerMode: false + boundaryConditionType: position + boundarySmootherType: quintic + boundarySmootherTime: 0.5 + boundaryCmAdjustment: false + boundaryCmAdjustmentTime: 1 + waistHeightRelaxation: false + gravity: 9.8 + dynamicsTimeRatio: 1 +Body: + "BodyMotionEngine": + updateJointVelocities: false + "EditableSceneBody": + editableSceneBodies: + - + bodyItem: 2 + showCenterOfMass: false + showPpcom: false + showZmp: false + - + bodyItem: 3 + showCenterOfMass: false + showPpcom: false + showZmp: false + - + bodyItem: 4 + showCenterOfMass: false + showPpcom: false + showZmp: false + staticModelEditing: false + "KinematicFaultChecker": + checkJointPositions: true + angleMargin: 0 + translationMargin: 0 + checkJointVelocities: true + velocityLimitRatio: 100 + targetJoints: all + checkSelfCollisions: true + onlyTimeBarRange: false +OpenRTM: + "deleteUnmanagedRTCsOnStartingSimulation": false +viewAreas: + - + type: embedded + tabs: true + contents: + type: splitter + orientation: horizontal + sizes: [ 310, 2195 ] + children: + - + type: splitter + orientation: vertical + sizes: [ 768, 767 ] + children: + - + type: pane + views: [ 2 ] + current: 2 + - + type: pane + views: [ 1, 8 ] + current: 1 + - + type: splitter + orientation: vertical + sizes: [ 1205, 330 ] + children: + - + type: splitter + orientation: horizontal + sizes: [ 449, 1740 ] + children: + - + type: pane + views: [ 6, 7, 0 ] + current: 6 + - + type: pane + views: [ 4 ] + current: 4 + - + type: pane + views: [ 3, 10 ] + current: 10 +layoutOfToolBars: + rows: + - + - { name: "FileBar", x: 0, priority: 0 } + - { name: "ScriptBar", x: 47, priority: 3 } + - { name: "TimeBar", x: 47, priority: 1 } + - { name: "SceneBar", x: 1455, priority: 2 } + - { name: "SimulationBar", x: 1464, priority: 0 } diff --git a/hrpsys_choreonoid_tutorials/config/HRP2JSKNT_RH_LOAD_OBJ.cnoid.in b/hrpsys_choreonoid_tutorials/config/HRP2JSKNT_RH_LOAD_OBJ.cnoid.in new file mode 100644 index 00000000..f63647ce --- /dev/null +++ b/hrpsys_choreonoid_tutorials/config/HRP2JSKNT_RH_LOAD_OBJ.cnoid.in @@ -0,0 +1,372 @@ +items: + id: 0 + name: "Root" + plugin: Base + class: RootItem + children: + - + id: 1 + name: "World" + plugin: Body + class: WorldItem + data: + collisionDetection: false + collisionDetector: AISTCollisionDetector + children: + - + id: 2 + name: "HRP2JSKNT" + plugin: Body + class: BodyItem + data: + modelFile: "@HRP2_MODELS_DIR@/HRP2JSKNT_WITH_3HAND_for_OpenHRP3/HRP2JSKNTmain_bush.wrl" + currentBaseLink: "WAIST" + rootPosition: [ 0.0, 0.0, 0.650 ] + rootAttitude: [ + 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + jointPositions: [ + 0.000366, 0.002938, -0.383414, 0.868598, -0.485184, -0.002938, 0.000000, + 0.000366, 0.002938, -0.383414, 0.868598, -0.485184, -0.002938, 0.000000, + 0.000000, 0.000000, 0.000000, 0.000000, + 0.174533, -0.174533, 0.000000, -0.436332, 0.000000, 0.000000, -0.174533, 0.261799, + 0.174533, 0.174533, 0.000000, -0.436332, 0.000000, 0.000000, -0.174533, -0.261799, + 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, + 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000] + initialRootPosition: [ 0.0, 0.0, 0.650 ] + initialRootAttitude: [ + 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + initialJointPositions: [ + 0.000366, 0.002938, -0.383414, 0.868598, -0.485184, -0.002938, 0.000000, + 0.000366, 0.002938, -0.383414, 0.868598, -0.485184, -0.002938, 0.000000, + 0.000000, 0.000000, 0.000000, 0.000000, + 0.174533, -0.174533, 0.000000, -0.436332, 0.000000, 0.000000, -0.174533, 0.261799, + 0.174533, 0.174533, 0.000000, -0.436332, 0.000000, 0.000000, -0.174533, -0.261799, + 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, + 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000] + zmp: [ 0, 0, 0 ] + collisionDetection: true + selfCollisionDetection: false + isEditable: true + children: + - + id: 3 + name: "BodyRTC" + plugin: OpenRTM + class: BodyRTCItem + data: + isImmediateMode: true + moduleName: "@JVRC_RTC_DIRECTORY@/lib/RobotHardware_choreonoid" + confFileName: "@JVRC_CONF_DIRECTORY@/BodyRTC_HRP2JSKNT.RH.conf" + configurationMode: Use Configuration File + AutoConnect: false + InstanceName: HRP2JSKNT(Robot)0 + bodyPeriodicRate: 0.004 + - + id: 4 + name: "add_objects.py" + plugin: Python + class: PythonScriptItem + data: + file: "@JVRC_RTC_DIRECTORY@/launch/add_objects.py" + executionOnLoading: true + backgroundExecution: false + - + id: 5 + name: "AISTSimulator" + plugin: Body + class: AISTSimulatorItem + data: + realtimeSync: true + recording: full + timeRangeMode: TimeBar range + onlyActiveControlPeriod: true + timeLength: 12000 + allLinkPositionOutputMode: false + deviceStateOutput: true + controllerThreads: true + recordCollisionData: false + dynamicsMode: Forward dynamics + integrationMode: Runge Kutta + gravity: [ 0, 0, -9.80665 ] + staticFriction: 1 + slipFriction: 1 + cullingThresh: 0.005 + contactCullingDepth: 0.03 + errorCriterion: 0.001 + maxNumIterations: 1000 + contactCorrectionDepth: 0.0001 + contactCorrectionVelocityRatio: 1 + kinematicWalking: false + 2Dmode: false + - + id: 9 + name: "ros_service_server.py" + plugin: Python + class: PythonScriptItem + data: + file: "@JVRC_RTC_DIRECTORY@/scripts/ros_service_server.py" + executionOnLoading: true + backgroundExecution: false + +views: + - + id: 0 + name: "CameraImage" + plugin: Base + class: ImageView + mounted: true + - + id: 1 + plugin: Base + class: ItemPropertyView + mounted: true + - + id: 2 + plugin: Base + class: ItemTreeView + mounted: true + state: + selected: [ 9 ] + checked: [ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, ] + expanded: [ 1 ] + - + id: 3 + plugin: Base + class: MessageView + mounted: true + - + id: 4 + plugin: Base + class: SceneView + mounted: true + state: + editMode: true + viewpointControlMode: thirdPerson + collisionLines: false + polygonMode: fill + defaultHeadLight: true + defaultHeadLightIntensity: 0.75 + headLightLightingFromBack: false + worldLight: true + worldLightIntensity: 0.5 + worldLightAmbient: 0.3 + additionalLights: true + floorGrid: true + floorGridSpan: 10 + floorGridInterval: 0.5 + xzGridSpan: 10 + xzGridInterval: 0.5 + xzGrid: false + yzGridSpan: 10 + yzGridInterval: 0.5 + texture: true + lineWidth: 1 + pointSize: 1 + normalVisualization: false + normalLength: 0.01 + coordinateAxes: true + showFPS: false + enableNewDisplayListDoubleRendering: false + useBufferForPicking: true + cameras: + - + camera: [ System, Perspective ] + isCurrent: true + fieldOfView: 0.6978 + near: 0.01 + far: 100 + eye: [ 2.7, -2.7, 2 ] + direction: [ -0.7, 0.7, -0.3 ] + up: [ -0.25, 0.25, 1] + - + camera: [ System, Orthographic ] + orthoHeight: 20 + near: 0.01 + far: 100 + backgroundColor: [ 0.100000001, 0.100000001, 0.300000012 ] + gridColor: [ 0.899999976, 0.899999976, 0.899999976, 1 ] + xzgridColor: [ 0.899999976, 0.899999976, 0.899999976, 1 ] + yzgridColor: [ 0.899999976, 0.899999976, 0.899999976, 1 ] + dedicatedItemTreeViewChecks: false + - + id: 5 + name: "Task" + plugin: Base + class: TaskView + state: + layoutMode: horizontal + isAutoMode: false + - + id: 6 + plugin: Body + class: BodyLinkView + mounted: true + state: + showRotationMatrix: false + - + id: 7 + plugin: Body + class: JointSliderView + mounted: true + state: + showAllJoints: true + jointId: false + name: true + numColumns: 1 + spinBox: true + slider: true + labelOnLeft: true + - + id: 8 + plugin: Body + class: LinkSelectionView + mounted: true + state: + listingMode: "Link List" + - + id: 10 + plugin: Python + class: PythonConsoleView + mounted: true +toolbars: + "TimeBar": + minTime: 0 + maxTime: 12000 + frameRate: 1000 + playbackFrameRate: 50 + idleLoopDrivenMode: false + currentTime: 0 + speedScale: 1 + syncToOngoingUpdates: true + autoExpansion: true + "KinematicsBar": + mode: AUTO + enablePositionDragger: true + penetrationBlock: false + collisionLinkHighlight: false + snapDistance: 0.025 + penetrationBlockDepth: 0.0005 + lazyCollisionDetectionMode: true + "LeggedBodyBar": + stanceWidth: 0.15 + "BodyMotionGenerationBar": + autoGenerationForNewBody: true + balancer: false + autoGeneration: false + timeScaleRatio: 1 + preInitialDuration: 1 + postFinalDuration: 1 + onlyTimeBarRange: false + makeNewBodyItem: true + stealthyStepMode: true + stealthyHeightRatioThresh: 2 + flatLiftingHeight: 0.005 + flatLandingHeight: 0.005 + impactReductionHeight: 0.005 + impactReductionTime: 0.04 + autoZmp: true + minZmpTransitionTime: 0.1 + zmpCenteringTimeThresh: 0.03 + zmpTimeMarginBeforeLiftingSpin: 0 + zmpMaxDistanceFromCenter: 0.02 + allLinkPositions: false + lipSyncMix: false + timeToStartBalancer: 0 + balancerIterations: 2 + plainBalancerMode: false + boundaryConditionType: position + boundarySmootherType: quintic + boundarySmootherTime: 0.5 + boundaryCmAdjustment: false + boundaryCmAdjustmentTime: 1 + waistHeightRelaxation: false + gravity: 9.8 + dynamicsTimeRatio: 1 +Body: + "BodyMotionEngine": + updateJointVelocities: false + "EditableSceneBody": + editableSceneBodies: + - + bodyItem: 2 + showCenterOfMass: false + showPpcom: false + showZmp: false + - + bodyItem: 3 + showCenterOfMass: false + showPpcom: false + showZmp: false + - + bodyItem: 4 + showCenterOfMass: false + showPpcom: false + showZmp: false + staticModelEditing: false + "KinematicFaultChecker": + checkJointPositions: true + angleMargin: 0 + translationMargin: 0 + checkJointVelocities: true + velocityLimitRatio: 100 + targetJoints: all + checkSelfCollisions: true + onlyTimeBarRange: false +OpenRTM: + "deleteUnmanagedRTCsOnStartingSimulation": false +viewAreas: + - + type: embedded + tabs: true + contents: + type: splitter + orientation: horizontal + sizes: [ 310, 2195 ] + children: + - + type: splitter + orientation: vertical + sizes: [ 768, 767 ] + children: + - + type: pane + views: [ 2 ] + current: 2 + - + type: pane + views: [ 1, 8 ] + current: 1 + - + type: splitter + orientation: vertical + sizes: [ 1205, 330 ] + children: + - + type: splitter + orientation: horizontal + sizes: [ 449, 1740 ] + children: + - + type: pane + views: [ 6, 7, 0 ] + current: 6 + - + type: pane + views: [ 4 ] + current: 4 + - + type: pane + views: [ 3, 10 ] + current: 10 +layoutOfToolBars: + rows: + - + - { name: "FileBar", x: 0, priority: 0 } + - { name: "ScriptBar", x: 47, priority: 3 } + - { name: "TimeBar", x: 47, priority: 1 } + - { name: "SceneBar", x: 1455, priority: 2 } + - { name: "SimulationBar", x: 1464, priority: 0 } diff --git a/hrpsys_choreonoid_tutorials/config/HRP2JSK_RH_LOAD_OBJ.cnoid.in b/hrpsys_choreonoid_tutorials/config/HRP2JSK_RH_LOAD_OBJ.cnoid.in new file mode 100644 index 00000000..f934e54f --- /dev/null +++ b/hrpsys_choreonoid_tutorials/config/HRP2JSK_RH_LOAD_OBJ.cnoid.in @@ -0,0 +1,368 @@ +items: + id: 0 + name: "Root" + plugin: Base + class: RootItem + children: + - + id: 1 + name: "World" + plugin: Body + class: WorldItem + data: + collisionDetection: false + collisionDetector: AISTCollisionDetector + children: + - + id: 2 + name: "HRP2JSK" + plugin: Body + class: BodyItem + data: + modelFile: "@HRP2_MODELS_DIR@/HRP2JSK_for_OpenHRP3/HRP2JSKmain_bush.wrl" + currentBaseLink: "WAIST" + rootPosition: [ 0.0, 0.0, 0.650 ] + rootAttitude: [ + 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + jointPositions: [ + -0.000015, 0.003323, -0.468274, 0.872491, -0.404216, -0.003323, + -0.000015, 0.003323, -0.467353, 0.870656, -0.403303, -0.003323, + 0.000000, 0.000000, 0.000000, 0.000000, + 0.174533, -0.174533, 0.000000, -0.436332, 0.000000, 0.000000, -0.174533, 0.261799, + 0.174533, 0.174533, 0.000000, -0.436332, 0.000000, 0.000000, -0.174533, -0.261799] + initialRootPosition: [ 0.0, 0.0, 0.650 ] + initialRootAttitude: [ + 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + initialJointPositions: [ + -0.000015, 0.003323, -0.468274, 0.872491, -0.404216, -0.003323, + -0.000015, 0.003323, -0.467353, 0.870656, -0.403303, -0.003323, + 0.000000, 0.000000, 0.000000, 0.000000, + 0.174533, -0.174533, 0.000000, -0.436332, 0.000000, 0.000000, -0.174533, 0.261799, + 0.174533, 0.174533, 0.000000, -0.436332, 0.000000, 0.000000, -0.174533, -0.261799] + zmp: [ 0, 0, 0 ] + collisionDetection: true + selfCollisionDetection: false + isEditable: true + children: + - + id: 3 + name: "BodyRTC" + plugin: OpenRTM + class: BodyRTCItem + data: + isImmediateMode: true + moduleName: "@JVRC_RTC_DIRECTORY@/lib/RobotHardware_choreonoid" + confFileName: "@JVRC_CONF_DIRECTORY@/BodyRTC_HRP2JSK.RH.conf" + configurationMode: Use Configuration File + AutoConnect: false + InstanceName: HRP2JSK(Robot)0 + bodyPeriodicRate: 0.004 + - + id: 4 + name: "add_objects.py" + plugin: Python + class: PythonScriptItem + data: + file: "@JVRC_RTC_DIRECTORY@/launch/add_objects.py" + executionOnLoading: true + backgroundExecution: false + - + id: 5 + name: "AISTSimulator" + plugin: Body + class: AISTSimulatorItem + data: + realtimeSync: true + recording: full + timeRangeMode: TimeBar range + onlyActiveControlPeriod: true + timeLength: 12000 + allLinkPositionOutputMode: false + deviceStateOutput: true + controllerThreads: true + recordCollisionData: false + dynamicsMode: Forward dynamics + integrationMode: Runge Kutta + gravity: [ 0, 0, -9.80665 ] + staticFriction: 1 + slipFriction: 1 + cullingThresh: 0.005 + contactCullingDepth: 0.03 + errorCriterion: 0.001 + maxNumIterations: 1000 + contactCorrectionDepth: 0.0001 + contactCorrectionVelocityRatio: 1 + kinematicWalking: false + 2Dmode: false + - + id: 9 + name: "ros_service_server.py" + plugin: Python + class: PythonScriptItem + data: + file: "@JVRC_RTC_DIRECTORY@/scripts/ros_service_server.py" + executionOnLoading: true + backgroundExecution: false + +views: + - + id: 0 + name: "CameraImage" + plugin: Base + class: ImageView + mounted: true + - + id: 1 + plugin: Base + class: ItemPropertyView + mounted: true + - + id: 2 + plugin: Base + class: ItemTreeView + mounted: true + state: + selected: [ 9 ] + checked: [ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, ] + expanded: [ 1 ] + - + id: 3 + plugin: Base + class: MessageView + mounted: true + - + id: 4 + plugin: Base + class: SceneView + mounted: true + state: + editMode: true + viewpointControlMode: thirdPerson + collisionLines: false + polygonMode: fill + defaultHeadLight: true + defaultHeadLightIntensity: 0.75 + headLightLightingFromBack: false + worldLight: true + worldLightIntensity: 0.5 + worldLightAmbient: 0.3 + additionalLights: true + floorGrid: true + floorGridSpan: 10 + floorGridInterval: 0.5 + xzGridSpan: 10 + xzGridInterval: 0.5 + xzGrid: false + yzGridSpan: 10 + yzGridInterval: 0.5 + texture: true + lineWidth: 1 + pointSize: 1 + normalVisualization: false + normalLength: 0.01 + coordinateAxes: true + showFPS: false + enableNewDisplayListDoubleRendering: false + useBufferForPicking: true + cameras: + - + camera: [ System, Perspective ] + isCurrent: true + fieldOfView: 0.6978 + near: 0.01 + far: 100 + eye: [ 2.7, -2.7, 2 ] + direction: [ -0.7, 0.7, -0.3 ] + up: [ -0.25, 0.25, 1] + - + camera: [ System, Orthographic ] + orthoHeight: 20 + near: 0.01 + far: 100 + backgroundColor: [ 0.100000001, 0.100000001, 0.300000012 ] + gridColor: [ 0.899999976, 0.899999976, 0.899999976, 1 ] + xzgridColor: [ 0.899999976, 0.899999976, 0.899999976, 1 ] + yzgridColor: [ 0.899999976, 0.899999976, 0.899999976, 1 ] + dedicatedItemTreeViewChecks: false + - + id: 5 + name: "Task" + plugin: Base + class: TaskView + state: + layoutMode: horizontal + isAutoMode: false + - + id: 6 + plugin: Body + class: BodyLinkView + mounted: true + state: + showRotationMatrix: false + - + id: 7 + plugin: Body + class: JointSliderView + mounted: true + state: + showAllJoints: true + jointId: false + name: true + numColumns: 1 + spinBox: true + slider: true + labelOnLeft: true + - + id: 8 + plugin: Body + class: LinkSelectionView + mounted: true + state: + listingMode: "Link List" + - + id: 10 + plugin: Python + class: PythonConsoleView + mounted: true +toolbars: + "TimeBar": + minTime: 0 + maxTime: 12000 + frameRate: 1000 + playbackFrameRate: 50 + idleLoopDrivenMode: false + currentTime: 0 + speedScale: 1 + syncToOngoingUpdates: true + autoExpansion: true + "KinematicsBar": + mode: AUTO + enablePositionDragger: true + penetrationBlock: false + collisionLinkHighlight: false + snapDistance: 0.025 + penetrationBlockDepth: 0.0005 + lazyCollisionDetectionMode: true + "LeggedBodyBar": + stanceWidth: 0.15 + "BodyMotionGenerationBar": + autoGenerationForNewBody: true + balancer: false + autoGeneration: false + timeScaleRatio: 1 + preInitialDuration: 1 + postFinalDuration: 1 + onlyTimeBarRange: false + makeNewBodyItem: true + stealthyStepMode: true + stealthyHeightRatioThresh: 2 + flatLiftingHeight: 0.005 + flatLandingHeight: 0.005 + impactReductionHeight: 0.005 + impactReductionTime: 0.04 + autoZmp: true + minZmpTransitionTime: 0.1 + zmpCenteringTimeThresh: 0.03 + zmpTimeMarginBeforeLiftingSpin: 0 + zmpMaxDistanceFromCenter: 0.02 + allLinkPositions: false + lipSyncMix: false + timeToStartBalancer: 0 + balancerIterations: 2 + plainBalancerMode: false + boundaryConditionType: position + boundarySmootherType: quintic + boundarySmootherTime: 0.5 + boundaryCmAdjustment: false + boundaryCmAdjustmentTime: 1 + waistHeightRelaxation: false + gravity: 9.8 + dynamicsTimeRatio: 1 +Body: + "BodyMotionEngine": + updateJointVelocities: false + "EditableSceneBody": + editableSceneBodies: + - + bodyItem: 2 + showCenterOfMass: false + showPpcom: false + showZmp: false + - + bodyItem: 3 + showCenterOfMass: false + showPpcom: false + showZmp: false + - + bodyItem: 4 + showCenterOfMass: false + showPpcom: false + showZmp: false + staticModelEditing: false + "KinematicFaultChecker": + checkJointPositions: true + angleMargin: 0 + translationMargin: 0 + checkJointVelocities: true + velocityLimitRatio: 100 + targetJoints: all + checkSelfCollisions: true + onlyTimeBarRange: false +OpenRTM: + "deleteUnmanagedRTCsOnStartingSimulation": false +viewAreas: + - + type: embedded + tabs: true + contents: + type: splitter + orientation: horizontal + sizes: [ 310, 2195 ] + children: + - + type: splitter + orientation: vertical + sizes: [ 768, 767 ] + children: + - + type: pane + views: [ 2 ] + current: 2 + - + type: pane + views: [ 1, 8 ] + current: 1 + - + type: splitter + orientation: vertical + sizes: [ 1205, 330 ] + children: + - + type: splitter + orientation: horizontal + sizes: [ 449, 1740 ] + children: + - + type: pane + views: [ 6, 7, 0 ] + current: 6 + - + type: pane + views: [ 4 ] + current: 4 + - + type: pane + views: [ 3, 10 ] + current: 10 +layoutOfToolBars: + rows: + - + - { name: "FileBar", x: 0, priority: 0 } + - { name: "ScriptBar", x: 47, priority: 3 } + - { name: "TimeBar", x: 47, priority: 1 } + - { name: "SceneBar", x: 1455, priority: 2 } + - { name: "SimulationBar", x: 1464, priority: 0 } diff --git a/hrpsys_choreonoid_tutorials/launch/hrp2jsk_choreonoid.launch b/hrpsys_choreonoid_tutorials/launch/hrp2jsk_choreonoid.launch new file mode 100644 index 00000000..b9c50776 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/launch/hrp2jsk_choreonoid.launch @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hrpsys_choreonoid_tutorials/launch/hrp2jsknt_choreonoid.launch b/hrpsys_choreonoid_tutorials/launch/hrp2jsknt_choreonoid.launch new file mode 100644 index 00000000..b72719c9 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/launch/hrp2jsknt_choreonoid.launch @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hrpsys_choreonoid_tutorials/launch/hrp2jsknts_choreonoid.launch b/hrpsys_choreonoid_tutorials/launch/hrp2jsknts_choreonoid.launch new file mode 100644 index 00000000..07f68010 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/launch/hrp2jsknts_choreonoid.launch @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + footcoords: + invert_odom_init: false + + + + + + + + + + + + + diff --git a/hrpsys_choreonoid_tutorials/launch/hrp2jsknts_vision_connect.launch b/hrpsys_choreonoid_tutorials/launch/hrp2jsknts_vision_connect.launch new file mode 100644 index 00000000..b7d55db5 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/launch/hrp2jsknts_vision_connect.launch @@ -0,0 +1,83 @@ + + + + + + + + + + + + + + + + + + + + [500, 0, 319.5, 0, 500, 239.5, 0, 0, 1] + [500, 0, 319.5, 0, 0, 500, 239.5, 0, 0, 0, 1, 0] + + + + + + + + + + + + + + + + + + + + + filter_field_name: z + filter_limit_min: 0.5 # depth camera cannot see too close area. `frontClipDistance` cannot be used for this purpose because camera sees through within `frontClipDistance`. + filter_limit_max: 10.0 + + + + + + + + + + + + + + + + + + + + + + diff --git a/hrpsys_choreonoid_tutorials/scripts/hrp2jsk_rh_setup.py b/hrpsys_choreonoid_tutorials/scripts/hrp2jsk_rh_setup.py new file mode 100755 index 00000000..4219a43c --- /dev/null +++ b/hrpsys_choreonoid_tutorials/scripts/hrp2jsk_rh_setup.py @@ -0,0 +1,14 @@ +#!/usr/bin/env python + +from hrpsys_choreonoid_tutorials.hrp2_choreonoid_hrpsys_config import * + +if __name__ == '__main__': + hcf = JSKHRP2ChoreonoidHrpsysConfigurator("HRP2JSK") + if len(sys.argv) > 2 : + hcf.init(sys.argv[1], sys.argv[2]) + hcf.startABSTIMP() + elif len(sys.argv) > 1 : + hcf.init(sys.argv[1]) + hcf.startABSTIMP() + else : + hcf.init() diff --git a/hrpsys_choreonoid_tutorials/scripts/hrp2jsknt_rh_setup.py b/hrpsys_choreonoid_tutorials/scripts/hrp2jsknt_rh_setup.py new file mode 100755 index 00000000..47613892 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/scripts/hrp2jsknt_rh_setup.py @@ -0,0 +1,14 @@ +#!/usr/bin/env python + +from hrpsys_choreonoid_tutorials.hrp2_choreonoid_hrpsys_config import * + +if __name__ == '__main__': + hcf = JSKHRP2ChoreonoidHrpsysConfigurator("HRP2JSKNT") + if len(sys.argv) > 2 : + hcf.init(sys.argv[1], sys.argv[2]) + hcf.startABSTIMP() + elif len(sys.argv) > 1 : + hcf.init(sys.argv[1]) + hcf.startABSTIMP() + else : + hcf.init() diff --git a/hrpsys_choreonoid_tutorials/scripts/hrp2jsknts_rh_setup.py b/hrpsys_choreonoid_tutorials/scripts/hrp2jsknts_rh_setup.py new file mode 100755 index 00000000..8bca2513 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/scripts/hrp2jsknts_rh_setup.py @@ -0,0 +1,14 @@ +#!/usr/bin/env python + +from hrpsys_choreonoid_tutorials.hrp2_choreonoid_hrpsys_config import * + +if __name__ == '__main__': + hcf = JSKHRP2ChoreonoidHrpsysConfigurator("HRP2JSKNTS") + if len(sys.argv) > 2 : + hcf.init(sys.argv[1], sys.argv[2]) + hcf.startABSTIMP() + elif len(sys.argv) > 1 : + hcf.init(sys.argv[1]) + hcf.startABSTIMP() + else : + hcf.init() diff --git a/hrpsys_choreonoid_tutorials/src/hrpsys_choreonoid_tutorials/hrp2_choreonoid_hrpsys_config.py b/hrpsys_choreonoid_tutorials/src/hrpsys_choreonoid_tutorials/hrp2_choreonoid_hrpsys_config.py new file mode 100755 index 00000000..733cdf04 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/src/hrpsys_choreonoid_tutorials/hrp2_choreonoid_hrpsys_config.py @@ -0,0 +1,78 @@ +#!/usr/bin/env python + +from hrpsys_ros_bridge_tutorials.hrp2_hrpsys_config import * + +class JSKHRP2ChoreonoidHrpsysConfigurator(JSKHRP2HrpsysConfigurator): + def waitForRobotHardware(self, robotname="Robot"): + '''!@brief + Wait for RobotHardware is exists and activated. + + @param robotname str: name of RobotHardware component. + ''' + self.rh = None + timeout_count = 0 + # wait for simulator or RobotHardware setup which sometime takes a long time + while self.rh == None and timeout_count < 10: # <- time out limit + if timeout_count > 0: # do not sleep initial loop + time.sleep(1); + self.rh = rtm.findRTC("RobotHardware_choreonoid0") + print(self.configurator_name + "wait for %s : %s ( timeout %d < 10)" % ( robotname, self.rh, timeout_count)) + if self.rh and self.rh.isActive() == None: # just in case rh is not ready... + self.rh = None + timeout_count += 1 + + if not self.rh: + print(self.configurator_name + "Could not find RobotHardware_choreonoid0") + if self.ms: + print(self.configurator_name + "Candidates are .... " + str([x.name() for x in self.ms.get_components()])) + print(self.configurator_name + "Exitting.... " + robotname) + exit(1) + + print(self.configurator_name + "findComps -> RobotHardware_choreonoid0 : %s isActive? = %s " % (self.rh, self.rh.isActive())) + + # wait for simulator or RobotHardware setup which sometime takes a long time + self.rh_choreonoid = None + while self.rh_choreonoid == None and timeout_count < 10: # <- time out limit + if timeout_count > 0: # do not sleep initial loop + time.sleep(1); + self.rh_choreonoid = rtm.findRTC(robotname) + print(self.configurator_name + "wait for %s : %s ( timeout %d < 10)" % ( robotname, self.rh_choreonoid, timeout_count)) + if self.rh_choreonoid and self.rh_choreonoid.isActive() == None: # just in case rh is not ready... + self.rh_choreonoid = None + timeout_count += 1 + + if not self.rh_choreonoid: + print(self.configurator_name + "Could not find " + robotname) + if self.ms: + print(self.configurator_name + "Candidates are .... " + str([x.name() for x in self.ms.get_components()])) + print(self.configurator_name + "Exitting.... " + robotname) + exit(1) + + print(self.configurator_name + "findComps -> %s : %s isActive? = %s " % (robotname, self.rh_choreonoid, self.rh_choreonoid.isActive())) + + def activateComps(self): + stash_rh = self.rh + self.rh = self.rh_choreonoid + HrpsysConfigurator.activateComps(self) + self.rh = stash_rh + + def startABSTIMP (self): + if self.ROBOT_NAME == "HRP2JSKNT" or self.ROBOT_NAME == "HRP2JSKNTS": + self.el_svc.setServoErrorLimit("RARM_JOINT7", sys.float_info.max) + self.el_svc.setServoErrorLimit("LARM_JOINT7", sys.float_info.max) + self.rh_svc.servo("RARM_JOINT7",OpenHRP.RobotHardwareService.SWITCH_OFF) + self.rh_svc.servo("LARM_JOINT7",OpenHRP.RobotHardwareService.SWITCH_OFF) + self.rh_svc.setServoGainPercentage("RLEG_JOINT6", 30.0) + self.rh_svc.setServoGainPercentage("LLEG_JOINT6", 30.0) + for j in dict(self.Groups)["rhand"] + dict(self.Groups)["lhand"]: + self.el_svc.setServoErrorLimit(j, sys.float_info.max) + self.rh_svc.setServoErrorLimit(j, 0.0) + + self.startAutoBalancer() + self.seq_svc.setJointAngles(self.hrp2ResetPose(), 1.0) + if self.ROBOT_NAME == "HRP2JSKNT" or self.ROBOT_NAME == "HRP2JSKNTS": + self.seq_svc.setJointAnglesOfGroup("rhand", [0, 0, 0, 0, 0, 0], 1.0) + self.seq_svc.setJointAnglesOfGroup("lhand", [0, 0, 0, 0, 0, 0], 1.0) + self.ic_svc.startImpedanceController("larm") + self.ic_svc.startImpedanceController("rarm") + self.startStabilizer()