diff --git a/hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSKNT.RH.conf b/hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSKNT.RH.conf
index 9f5f0003..38c1677d 100644
--- a/hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSKNT.RH.conf
+++ b/hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSKNT.RH.conf
@@ -5,10 +5,10 @@ name-server = localhost:15005
## in: angleRef, angle
## out: torque
##
-in-port = tauIn:RLEG_JOINT0,RLEG_JOINT1,RLEG_JOINT2,RLEG_JOINT3,RLEG_JOINT4,RLEG_JOINT5,RLEG_JOINT6,LLEG_JOINT0,LLEG_JOINT1,LLEG_JOINT2,LLEG_JOINT3,LLEG_JOINT4,LLEG_JOINT5,LLEG_JOINT6,CHEST_JOINT0,CHEST_JOINT1,HEAD_JOINT0,HEAD_JOINT1,RARM_JOINT0,RARM_JOINT1,RARM_JOINT2,RARM_JOINT3,RARM_JOINT4,RARM_JOINT5,RARM_JOINT6,RARM_JOINT7,LARM_JOINT0,LARM_JOINT1,LARM_JOINT2,LARM_JOINT3,LARM_JOINT4,LARM_JOINT5,LARM_JOINT6,LARM_JOINT7:JOINT_TORQUE
-out-port = angleOut:RLEG_JOINT0,RLEG_JOINT1,RLEG_JOINT2,RLEG_JOINT3,RLEG_JOINT4,RLEG_JOINT5,RLEG_JOINT6,LLEG_JOINT0,LLEG_JOINT1,LLEG_JOINT2,LLEG_JOINT3,LLEG_JOINT4,LLEG_JOINT5,LLEG_JOINT6,CHEST_JOINT0,CHEST_JOINT1,HEAD_JOINT0,HEAD_JOINT1,RARM_JOINT0,RARM_JOINT1,RARM_JOINT2,RARM_JOINT3,RARM_JOINT4,RARM_JOINT5,RARM_JOINT6,RARM_JOINT7,LARM_JOINT0,LARM_JOINT1,LARM_JOINT2,LARM_JOINT3,LARM_JOINT4,LARM_JOINT5,LARM_JOINT6,LARM_JOINT7:JOINT_VALUE
-out-port = qvel:RLEG_JOINT0,RLEG_JOINT1,RLEG_JOINT2,RLEG_JOINT3,RLEG_JOINT4,RLEG_JOINT5,RLEG_JOINT6,LLEG_JOINT0,LLEG_JOINT1,LLEG_JOINT2,LLEG_JOINT3,LLEG_JOINT4,LLEG_JOINT5,LLEG_JOINT6,CHEST_JOINT0,CHEST_JOINT1,HEAD_JOINT0,HEAD_JOINT1,RARM_JOINT0,RARM_JOINT1,RARM_JOINT2,RARM_JOINT3,RARM_JOINT4,RARM_JOINT5,RARM_JOINT6,RARM_JOINT7,LARM_JOINT0,LARM_JOINT1,LARM_JOINT2,LARM_JOINT3,LARM_JOINT4,LARM_JOINT5,LARM_JOINT6,LARM_JOINT7:JOINT_VELOCITY
-out-port = torque:RLEG_JOINT0,RLEG_JOINT1,RLEG_JOINT2,RLEG_JOINT3,RLEG_JOINT4,RLEG_JOINT5,RLEG_JOINT6,LLEG_JOINT0,LLEG_JOINT1,LLEG_JOINT2,LLEG_JOINT3,LLEG_JOINT4,LLEG_JOINT5,LLEG_JOINT6,CHEST_JOINT0,CHEST_JOINT1,HEAD_JOINT0,HEAD_JOINT1,RARM_JOINT0,RARM_JOINT1,RARM_JOINT2,RARM_JOINT3,RARM_JOINT4,RARM_JOINT5,RARM_JOINT6,RARM_JOINT7,LARM_JOINT0,LARM_JOINT1,LARM_JOINT2,LARM_JOINT3,LARM_JOINT4,LARM_JOINT5,LARM_JOINT6,LARM_JOINT7:JOINT_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
diff --git a/hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSKNTS.RH.conf b/hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSKNTS.RH.conf
index 58eb9708..2db8001d 100644
--- a/hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSKNTS.RH.conf
+++ b/hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSKNTS.RH.conf
@@ -5,10 +5,10 @@ name-server = localhost:15005
## in: angleRef, angle
## out: torque
##
-in-port = tauIn:RLEG_JOINT0,RLEG_JOINT1,RLEG_JOINT2,RLEG_JOINT3,RLEG_JOINT4,RLEG_JOINT5,RLEG_JOINT6,LLEG_JOINT0,LLEG_JOINT1,LLEG_JOINT2,LLEG_JOINT3,LLEG_JOINT4,LLEG_JOINT5,LLEG_JOINT6,CHEST_JOINT0,CHEST_JOINT1,HEAD_JOINT0,HEAD_JOINT1,RARM_JOINT0,RARM_JOINT1,RARM_JOINT2,RARM_JOINT3,RARM_JOINT4,RARM_JOINT5,RARM_JOINT6,RARM_JOINT7,LARM_JOINT0,LARM_JOINT1,LARM_JOINT2,LARM_JOINT3,LARM_JOINT4,LARM_JOINT5,LARM_JOINT6,LARM_JOINT7:JOINT_TORQUE
-out-port = angleOut:RLEG_JOINT0,RLEG_JOINT1,RLEG_JOINT2,RLEG_JOINT3,RLEG_JOINT4,RLEG_JOINT5,RLEG_JOINT6,LLEG_JOINT0,LLEG_JOINT1,LLEG_JOINT2,LLEG_JOINT3,LLEG_JOINT4,LLEG_JOINT5,LLEG_JOINT6,CHEST_JOINT0,CHEST_JOINT1,HEAD_JOINT0,HEAD_JOINT1,RARM_JOINT0,RARM_JOINT1,RARM_JOINT2,RARM_JOINT3,RARM_JOINT4,RARM_JOINT5,RARM_JOINT6,RARM_JOINT7,LARM_JOINT0,LARM_JOINT1,LARM_JOINT2,LARM_JOINT3,LARM_JOINT4,LARM_JOINT5,LARM_JOINT6,LARM_JOINT7:JOINT_VALUE
-out-port = qvel:RLEG_JOINT0,RLEG_JOINT1,RLEG_JOINT2,RLEG_JOINT3,RLEG_JOINT4,RLEG_JOINT5,RLEG_JOINT6,LLEG_JOINT0,LLEG_JOINT1,LLEG_JOINT2,LLEG_JOINT3,LLEG_JOINT4,LLEG_JOINT5,LLEG_JOINT6,CHEST_JOINT0,CHEST_JOINT1,HEAD_JOINT0,HEAD_JOINT1,RARM_JOINT0,RARM_JOINT1,RARM_JOINT2,RARM_JOINT3,RARM_JOINT4,RARM_JOINT5,RARM_JOINT6,RARM_JOINT7,LARM_JOINT0,LARM_JOINT1,LARM_JOINT2,LARM_JOINT3,LARM_JOINT4,LARM_JOINT5,LARM_JOINT6,LARM_JOINT7:JOINT_VELOCITY
-out-port = torque:RLEG_JOINT0,RLEG_JOINT1,RLEG_JOINT2,RLEG_JOINT3,RLEG_JOINT4,RLEG_JOINT5,RLEG_JOINT6,LLEG_JOINT0,LLEG_JOINT1,LLEG_JOINT2,LLEG_JOINT3,LLEG_JOINT4,LLEG_JOINT5,LLEG_JOINT6,CHEST_JOINT0,CHEST_JOINT1,HEAD_JOINT0,HEAD_JOINT1,RARM_JOINT0,RARM_JOINT1,RARM_JOINT2,RARM_JOINT3,RARM_JOINT4,RARM_JOINT5,RARM_JOINT6,RARM_JOINT7,LARM_JOINT0,LARM_JOINT1,LARM_JOINT2,LARM_JOINT3,LARM_JOINT4,LARM_JOINT5,LARM_JOINT6,LARM_JOINT7:JOINT_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
diff --git a/hrpsys_choreonoid_tutorials/launch/hrp2jsknt_choreonoid.launch b/hrpsys_choreonoid_tutorials/launch/hrp2jsknt_choreonoid.launch
index 7146be1d..21dfebc8 100644
--- a/hrpsys_choreonoid_tutorials/launch/hrp2jsknt_choreonoid.launch
+++ b/hrpsys_choreonoid_tutorials/launch/hrp2jsknt_choreonoid.launch
@@ -8,12 +8,12 @@
-
-
-
+
+
+
-
+
diff --git a/hrpsys_choreonoid_tutorials/launch/hrp2jsknts_choreonoid.launch b/hrpsys_choreonoid_tutorials/launch/hrp2jsknts_choreonoid.launch
index c22f18fb..aff404f3 100644
--- a/hrpsys_choreonoid_tutorials/launch/hrp2jsknts_choreonoid.launch
+++ b/hrpsys_choreonoid_tutorials/launch/hrp2jsknts_choreonoid.launch
@@ -9,12 +9,12 @@
-
-
-
+
+
+
-
+
diff --git a/hrpsys_choreonoid_tutorials/scripts/hrp2_rh_setup.py b/hrpsys_choreonoid_tutorials/scripts/hrp2_rh_setup.py
index 339afec7..d7b56acb 100755
--- a/hrpsys_choreonoid_tutorials/scripts/hrp2_rh_setup.py
+++ b/hrpsys_choreonoid_tutorials/scripts/hrp2_rh_setup.py
@@ -57,7 +57,6 @@ def activateComps(self):
self.rh = stash_rh
def startABSTIMP (self):
- ### not used on hrpsys
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)
@@ -65,8 +64,15 @@ def startABSTIMP (self):
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()