hrp2_with_3hand

start-jsk · Apr 23, 2021 · 35f5398 · 35f5398
1 parent f0884e3
commit 35f5398
Show file tree

Hide file tree

Showing 5 changed files with 24 additions and 18 deletions.
diff --git 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
@@ -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
@@ -8,12 +8,12 @@
     <arg name="LAUNCH_FOOTCOORDS"    value="false" />
     <arg name="ENVIRONMENT_YAML"     value="$(arg ENVIRONMENT_YAML)" />
     <arg name="CHOREONOID_ROBOT"     value="HRP2JSKNT"/>
-    <arg name="MODEL_FILE"           value="$(find hrp2_models)/HRP2JSKNT_for_OpenHRP3/HRP2JSKNTmain.wrl"/>
-    <arg name="COLLADA_FILE"         value="$(find hrpsys_ros_bridge_tutorials)/models/HRP2JSKNT_WH_SENSORS.urdf"/>
-    <arg name="CONF_FILE"            value="$(find hrpsys_ros_bridge_tutorials)/models/HRP2JSKNT.conf"/>
+    <arg name="MODEL_FILE"           value="$(find hrp2_models)/HRP2JSKNT_WITH_3HAND_for_OpenHRP3/HRP2JSKNTmain.wrl"/>
+    <arg name="COLLADA_FILE"         value="$(find hrpsys_ros_bridge_tutorials)/models/HRP2JSKNT_WITH_3HAND_SENSORS.urdf"/>
+    <arg name="CONF_FILE"            value="$(find hrpsys_ros_bridge_tutorials)/models/HRP2JSKNT_WITH_3HAND.conf"/>
     <arg name="HRPSYS_PY_PKG"        value="hrpsys_choreonoid_tutorials"/>
     <arg name="HRPSYS_PY_NAME"       value="hrp2_rh_setup.py" />
-    <arg name="CONTROLLER_CONFIG"    value="$(find hrpsys_ros_bridge_tutorials)/models/HRP2JSKNT_controller_config.yaml" />
+    <arg name="CONTROLLER_CONFIG"    value="$(find hrpsys_ros_bridge_tutorials)/models/HRP2JSKNT_WITH_3HAND_controller_config.yaml" />
     <arg name="CUSTOMIZER_CONF_FILE" value="$(find hrpsys_choreonoid_tutorials)/config/HRP2JSKNTCustomizer.yaml" />
   </include>
 

diff --git a/hrpsys_choreonoid_tutorials/launch/hrp2jsknts_choreonoid.launch b/hrpsys_choreonoid_tutorials/launch/hrp2jsknts_choreonoid.launch
@@ -9,12 +9,12 @@
     <arg name="LAUNCH_FOOTCOORDS"    default="$(arg LAUNCH_FOOTCOORDS)" />
     <arg name="ENVIRONMENT_YAML"     value="$(arg ENVIRONMENT_YAML)" />
     <arg name="CHOREONOID_ROBOT"     value="HRP2JSKNTS"/>
-    <arg name="MODEL_FILE"           value="$(find hrp2_models)/HRP2JSKNTS_for_OpenHRP3/HRP2JSKNTSmain.wrl"/>
-    <arg name="COLLADA_FILE"         value="$(find hrpsys_ros_bridge_tutorials)/models/HRP2JSKNTS_WH_SENSORS.urdf"/>
-    <arg name="CONF_FILE"            value="$(find hrpsys_ros_bridge_tutorials)/models/HRP2JSKNTS.conf"/>
+    <arg name="MODEL_FILE"           value="$(find hrp2_models)/HRP2JSKNTS_WITH_3HAND_for_OpenHRP3/HRP2JSKNTSmain.wrl"/>
+    <arg name="COLLADA_FILE"         value="$(find hrpsys_ros_bridge_tutorials)/models/HRP2JSKNTS_WITH_3HAND_SENSORS.urdf"/>
+    <arg name="CONF_FILE"            value="$(find hrpsys_ros_bridge_tutorials)/models/HRP2JSKNTS_WITH_3HAND.conf"/>
     <arg name="HRPSYS_PY_PKG"        value="hrpsys_choreonoid_tutorials"/>
     <arg name="HRPSYS_PY_NAME"       value="hrp2_rh_setup.py" />
-    <arg name="CONTROLLER_CONFIG"    value="$(find hrpsys_ros_bridge_tutorials)/models/HRP2JSKNTS_controller_config.yaml" />
+    <arg name="CONTROLLER_CONFIG"    value="$(find hrpsys_ros_bridge_tutorials)/models/HRP2JSKNTS_WITH_3HAND_controller_config.yaml" />
     <arg name="CUSTOMIZER_CONF_FILE" value="$(find hrpsys_choreonoid_tutorials)/config/HRP2JSKNTSCustomizer.yaml" />
     <arg name="USE_SOFTERRORLIMIT" value="true" />
     <arg name="PROJECT_FILE" value="$(arg PROJECT_FILE)" if="$(eval bool(arg('PROJECT_FILE')))" />

diff --git a/hrpsys_choreonoid_tutorials/scripts/hrp2_rh_setup.py b/hrpsys_choreonoid_tutorials/scripts/hrp2_rh_setup.py
@@ -57,16 +57,22 @@ 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)
             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()