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()