[hrpsys_ros_bridge_tutorials] overwrite HRP2JSKNT* by HRP2JSKNT*_WITH_3HAND

Author: Naoki-Hiraoka

hrpsys_ros_bridge_tutorials/CMakeLists.txt

@@ -1,5 +1,5 @@
 (load "package://hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l")
-(require :hrp2jsknt "package://hrpsys_ros_bridge_tutorials/models/hrp2jsknt_with_3hand.l")
+(require :hrp2jsknt "package://hrpsys_ros_bridge_tutorials/models/hrp2jsknt.l")
 (when (probe-file (ros::resolve-ros-path "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-utils.l"))
   (require :hrp2jsknt-utils "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-utils.l"))
 (load "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2-common-interface.l")

hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-interface.l

@@ -1,4 +1,4 @@
-(require :hrp2jsknt "package://hrpsys_ros_bridge_tutorials/models/hrp2jsknt_with_3hand.l")
+(require :hrp2jsknt "package://hrpsys_ros_bridge_tutorials/models/hrp2jsknt.l")
 (load "package://hrpsys_ros_bridge_tutorials/euslisp/hrp3hand-utils.l")
 
 (eval
@@ -11,7 +11,7 @@
    (prog1
        (send-super* :init-ending args)
      (send self :set-additional-end-coords)
-     ;;(send self :define-min-max-table)
+     (send self :define-min-max-table)
      (when (< (norm (send (send (send self :worldcoords) :transformation
                                 (send (car (send self :links)) :worldcoords)) :pos))
               0.1)

hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-utils.l

@@ -1,5 +1,5 @@
 (load "package://hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l")
-(require :hrp2jsknts "package://hrpsys_ros_bridge_tutorials/models/hrp2jsknts_with_3hand.l")
+(require :hrp2jsknts "package://hrpsys_ros_bridge_tutorials/models/hrp2jsknts.l")
 (when (probe-file (ros::resolve-ros-path "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-utils.l"))
   (require :hrp2jsknts-utils "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-utils.l"))
 (load "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2-common-interface.l")

hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-interface.l

@@ -1,4 +1,4 @@
-(load "package://hrpsys_ros_bridge_tutorials/models/hrp2jsknts_with_3hand.l")
+(require :hrp2jsknts "package://hrpsys_ros_bridge_tutorials/models/hrp2jsknts.l")
 (load "package://hrpsys_ros_bridge_tutorials/euslisp/hrp3hand-utils.l")
 
 (eval
@@ -10,7 +10,7 @@
    (&rest args)
    (prog1
        (send-super* :init-ending args)
-     ;;(send self :define-min-max-table)
+     (send self :define-min-max-table)
      (when (< (norm (send (send (send self :worldcoords) :transformation
                                 (send (car (send self :links)) :worldcoords)) :pos))
               0.1)

hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-utils.l

@@ -42,5 +42,9 @@
                 (send self :limb :rhand :angle-vector)
                 (send self :limb :lhand :angle-vector))
    )
-  )
+   (:collision-check-pairs
+    (&key ((:links ls) (set-difference (cons (car links) (all-child-links (car links)))
+                                       (append (send self :limb :rhand :links) (send self :limb :lhand :links)))))
+    (send-super :collision-check-pairs :links ls))
+   )
  )

hrpsys_ros_bridge_tutorials/euslisp/hrp3hand-utils.l

@@ -31,4 +31,16 @@
 1400 100
 460 100
 1870 100
-0 0
+0 0
+50 10
+50 10
+50 10
+50 10
+50 10
+50 10
+50 10
+50 10
+50 10
+50 10
+50 10
+50 10

hrpsys_ros_bridge_tutorials/models/HRP2JSKNT.PDgain.dat

@@ -1,7 +1,5 @@
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="HRP2JSKNT" >
-  <xacro:include filename="HRP2JSKNT_body.urdf" />
-  <xacro:include filename="HRP3HAND_L.urdf" />
-  <xacro:include filename="HRP3HAND_R.urdf" />
+  <xacro:include filename="HRP2JSKNT.urdf" />
   <xacro:include filename="$(find multisense_description)/urdf/multisenseSL.urdf" />
   <joint name="hrp2_to_multisense" type="fixed">
     <parent link="HEAD_LINK1" />

hrpsys_ros_bridge_tutorials/models/HRP2JSKNT.urdf.xacro

@@ -31,4 +31,16 @@
 1400 100
 460 100
 1870 100
-0 0
+0 0
+50 10
+50 10
+50 10
+50 10
+50 10
+50 10
+50 10
+50 10
+50 10
+50 10
+50 10
+50 10

hrpsys_ros_bridge_tutorials/models/HRP2JSKNTS.PDgain.dat

@@ -1,6 +1,7 @@
 ##
 ## - collada_joint_name : euslisp_joint_name (start with :)
 ##
+limbs: [rleg, lleg, torso, head, rarm, larm, rhand, lhand]
 
 rleg:
   - RLEG_JOINT0  : rleg-crotch-y
@@ -19,8 +20,8 @@ lleg:
   - LLEG_JOINT5  : lleg-ankle-r
   - LLEG_JOINT6  : lleg-toe-p
 torso:
-  - CHEST_JOINT0 : torso-waist-y
-  - CHEST_JOINT1 : torso-waist-p
+  - CHEST_JOINT0 : torso-waist-p
+  - CHEST_JOINT1 : torso-waist-y
 head:
   - HEAD_JOINT0  : head-neck-y
   - HEAD_JOINT1  : head-neck-p
@@ -42,6 +43,20 @@ larm:
   - LARM_JOINT5  : larm-wrist-r
   - LARM_JOINT6  : larm-wrist-p
   - LARM_JOINT7  : larm-thumb-r
+rhand:
+  - R_THUMBCM_Y  : rarm-t-1y
+  - R_THUMBCM_P  : rarm-t-1p
+  - R_INDEXMP_R  : rarm-f1-1r
+  - R_INDEXMP_P  : rarm-f1-1p
+  - R_INDEXPIP_R  : rarm-f1-2r
+  - R_MIDDLEPIP_R : rarm-f2-2r
+lhand:
+  - L_THUMBCM_Y  : larm-t-1y
+  - L_THUMBCM_P  : larm-t-1p
+  - L_INDEXMP_R  : larm-f1-1r
+  - L_INDEXMP_P  : larm-f1-1p
+  - L_INDEXPIP_R  : larm-f1-2r
+  - L_MIDDLEPIP_R : larm-f2-2r
 
 ##
 ## end-coords
@@ -140,13 +155,17 @@ angle-vector:
                 0.0, 0.0,
                 0.0, 0.0,
                 10.0, -10.0, 0.0, -25.0, 0.0, 0.0, -10.0,  15.0,
-                10.0,  10.0, 0.0, -25.0, 0.0, 0.0, -10.0, -15.0]
+                10.0,  10.0, 0.0, -25.0, 0.0, 0.0, -10.0, -15.0,
+                0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+                0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
   reset-manip-pose : [0.0,   0.0, -26.0,   50.0, -24.0,  0.0, 0.0,
                       0.0,   0.0, -26.0,   50.0, -24.0,  0.0, 0.0,
                       0.0,   0.0,
                       0.0,  40.0,
                      50.0, -30.0, -10.0, -120.0, -25.0, -5.0, -20.0,  60.0,
-                     50.0,  30.0,  10.0, -120.0,  25.0,  5.0, -20.0, -60.0]
+                     50.0,  30.0,  10.0, -120.0,  25.0,  5.0, -20.0, -60.0,
+                     0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+                     0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
 # for gazebo simulation
 replace_xmls:
   - match_rule: