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