[hrpsys_ros_bridge_tutorials][HRP2JSKNT*] hrpsysやROSではHRP3HandをHRP2全身の関節に含めるようにする

hrpsys_ros_bridge_tutorials/euslisp/hrp2-common-interface.l

@@ -4,53 +4,66 @@
   )
 
 (defmethod hrp2-common-interface
-  (:define-all-ROSBridge-srv-methods
-   (&key (debug-view nil) (ros-pkg-name "hrpsys_ros_bridge"))
-   (send-super :define-all-ROSBridge-srv-methods)
-   (if (ros::resolve-ros-path "package://jsk_hrp2_ros_bridge")
-       (send-super :define-all-ROSBridge-srv-methods :ros-pkg-name "jsk_hrp2_ros_bridge"))
-   )
-  (:hand-controlmode-vector
-   (cmode)
-   (if (find-method self :hrp3handcontrollerservice_setcontrolmodevector)
-       (send self :hrp3handcontrollerservice_setcontrolmodevector :i_cmode cmode)
-     (warn ";; :hrp3handcontrollerservice_setcontrolmodevector is not implemented!~%"))
-   )
   (:hand-angle-vector
    (av &optional (tm 1000))
-   (if (find-method self :hrp3handcontrollerservice_setjointangles)
-       (send self :hrp3handcontrollerservice_setjointangles :jvs av :tm (/ tm 1000.0))
-     (warn ";; :hrp3handcontrollerservice_setjointangles is not implemented!~%"))
-   )
-  ;;(:hand-wait-interpolation-raw
+   (let ((av-all (instantiate float-vector (length (send robot :angle-vector)))))
+     (dotimes (i 6)
+       (setelt av-all (position (elt (send robot :limb :rhand :joint-list) i) (send robot :joint-list)) (elt av i))
+       (setelt av-all (position (elt (send robot :limb :lhand :joint-list) i) (send robot :joint-list)) (elt av (+ i 6))))
+     (send self :angle-vector av-all tm :rhand-controller)
+     (send self :angle-vector av-all tm :lhand-controller)
+     av))
   (:hand-wait-interpolation
    ()
-   (if (find-method self :hrp3handcontrollerservice_waitinterpolation)
-       (send self :hrp3handcontrollerservice_waitinterpolation)
-     (warn ";; :hrp3handcontrollerservice_waitinterpolation is not implemented!~%"))
+   (list (send self :wait-interpolation :rhand-controller)
+         (send self :wait-interpolation :lhand-controller))
    )
   (:hand-servo-on
    ()
-   (if (find-method self :hrp3handcontrollerservice_handservoon)
-       (send self :hrp3handcontrollerservice_handservoon)
-     (warn ";; :hrp3handcontrollerservice_handservoon is not implemented!~%"))
+   (warn ";; :hrp3handcontrollerservice_handservoon is not implemented!~%")
    )
   (:hand-servo-off
    ()
-   (if (find-method self :hrp3handcontrollerservice_handservooff)
-       (send self :hrp3handcontrollerservice_handservooff)
-     (warn ";; :hrp3handcontrollerservice_handservooff is not implemented!~%"))
+   (warn ";; :hrp3handcontrollerservice_handservooff is not implemented!~%")
    )
   (:hand-joint-calib
    ()
-   (if (find-method self :hrp3handcontrollerservice_handjointcalib)
-       (send self :hrp3handcontrollerservice_handjointcalib)
-     (warn ";; :hrp3handcontrollerservice_handjointcalib is not implemented!~%"))
+   (warn ";; :hrp3handcontrollerservice_handjointcalib is not implemented!~%")
    )
   (:hand-state
    ()
-   (if (find-method self :hrp3handcontrollerservice_getRobotState)
-       (send self :hrp3handcontrollerservice_getRobotState)
-     (warn ";; :hrp3handcontrollerservice_getRobotState is not implemented!~%"))
+   (send self :state)
+   (let ((reference-all (send self :reference-vector))
+         (reference (instantiate float-vector 12)))
+     (dotimes (i 6)
+       (setelt reference i (elt reference-all (position (elt (send robot :limb :rhand :joint-list) i) (send robot :joint-list))))
+       (setelt reference (+ i 6) (elt reference-all (position (elt (send robot :limb :lhand :joint-list) i) (send robot :joint-list)))))
+     (instance hrp3hand-state :init
+               :reference reference
+               :potentio (send robot :hand-angle-vector)
+               )
+     )
    )
   )
+
+(defclass hrp3hand-state
+  :super propertied-object
+  :slots (_reference _potentio)
+  )
+
+(defmethod hrp3hand-state
+  (:init
+   (&key
+    ((:reference __reference) (make-array 0 :initial-element 0.0 :element-type :float))
+    ((:potentio __potentio) (make-array 0 :initial-element 0.0 :element-type :float))
+    )
+   (setq _reference __reference)
+   (setq _potentio __potentio)
+   self)
+  (:reference
+   (&optional __reference)
+   (if __reference (setq _reference __reference)) _reference)
+  (:potentio
+   (&optional __potentio)
+   (if __potentio (setq _potentio __potentio)) _potentio)
+  )

hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-interface.l

@@ -3,8 +3,6 @@
 (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")
-(if (ros::resolve-ros-path "package://jsk_hrp2_ros_bridge")
-    (ros::load-ros-manifest "jsk_hrp2_ros_bridge"))
 
 (defclass hrp2jsknt-interface
   :super hrp2-common-interface
@@ -14,7 +12,7 @@
     (prog1
         (send-super* :init :robot hrp2jsknt-robot args)
       ;; add controller
-      (dolist (limb '(:rarm :larm :rleg :lleg :head :torso))
+      (dolist (limb '(:rarm :larm :rleg :lleg :head :torso :rhand :lhand))
         (send self :def-limb-controller-method limb)
         (send self :add-controller (read-from-string (format nil "~A-controller" limb)) :joint-enable-check t :create-actions t))))
   )

hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-utils.l

@@ -1,8 +1,6 @@
-(load "package://hrpsys_ros_bridge_tutorials/models/hrp2jsknt.l")
+(require :hrp2jsknt "package://hrpsys_ros_bridge_tutorials/models/hrp2jsknt.l")
 (load "package://hrpsys_ros_bridge_tutorials/euslisp/hrp3hand-utils.l")
 
-(unless (assoc :init-org (send hrp2jsknt-robot :methods))
-  (rplaca (assoc :init (send hrp2jsknt-robot :methods)) :init-org))
 (eval
  `(defmethod hrp2jsknt-robot
     ,@(get-hrp2-with-hand-class-methods)))

hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-interface.l

@@ -3,8 +3,6 @@
 (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")
-(if (ros::resolve-ros-path "package://jsk_hrp2_ros_bridge")
-    (ros::load-ros-manifest "jsk_hrp2_ros_bridge"))
 
 (defclass hrp2jsknts-interface
   :super hrp2-common-interface
@@ -14,7 +12,7 @@
     (prog1
         (send-super* :init :robot hrp2jsknts-robot args)
       ;; add controller
-      (dolist (limb '(:rarm :larm :rleg :lleg :head :torso))
+      (dolist (limb '(:rarm :larm :rleg :lleg :head :torso :rhand :lhand))
         (send self :def-limb-controller-method limb)
         (send self :add-controller (read-from-string (format nil "~A-controller" limb)) :joint-enable-check t :create-actions t))))
   )

hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-utils.l

@@ -1,8 +1,6 @@
-(load "package://hrpsys_ros_bridge_tutorials/models/hrp2jsknts.l")
+(require :hrp2jsknts "package://hrpsys_ros_bridge_tutorials/models/hrp2jsknts.l")
 (load "package://hrpsys_ros_bridge_tutorials/euslisp/hrp3hand-utils.l")
 
-(unless (assoc :init-org (send hrp2jsknts-robot :methods))
-  (rplaca (assoc :init (send hrp2jsknts-robot :methods)) :init-org))
 (eval
  `(defmethod hrp2jsknts-robot
     ,@(get-hrp2-with-hand-class-methods)))

hrpsys_ros_bridge_tutorials/euslisp/hrp3hand-utils.l

@@ -1,120 +1,50 @@
-(load "package://hrpsys_ros_bridge_tutorials/models/hrp3hand_l.l")
-(load "package://hrpsys_ros_bridge_tutorials/models/hrp3hand_r.l")
-
-(defun get-hrp3hand-class-methods ()
-  '(
-  ;; poses
-  (:open-pose ()
-   (send self :angle-vector (float-vector  0 60 -10 30  0  0)))
-  (:preclose-pose ()
-   (send self :angle-vector (float-vector  40  60 -10  35  10 90)))
-  (:close-pose ()
-   (send self :angle-vector (float-vector  65   0  30  50  90 90)))
-  (:reset-pose ()
-   (send self :angle-vector (float-vector  0 0 0 0 0 0)))
-  ;;
-  (:standard-pose () ;; PoS
-   (send self :angle-vector (float-vector  20 90 0 10 -20 -20)))
-  (:hook-pose () ;; PoH
-   (send self :angle-vector (float-vector  90 90 0 10 -20 -20)))
-  (:index-pose () ;; PoI
-   (send self :angle-vector (float-vector  60 90 0 70 -20 -20)))
-  (:extension-pose () ;; PoE
-   (send self :angle-vector (float-vector  90 30 0 10 -20 -20)))
-  (:distal-pose () ;; PoD
-   (send self :angle-vector (float-vector  50 60 -20 10 20 40)))
-  (:hook-pose2 ()
-    (send self :angle-vector (float-vector  90 70 0 10 -20 -40)))
-  (:distal-pose2 ()
-    (send self :angle-vector (float-vector  90 90 -20 10 20 60)))
-  (:grasp-pose ()
-    (send self :angle-vector (float-vector 77.9709 -11.4732 8.28742 -16.3569 106.185 86.0974)))
-  (:index-avoid-extention-pose2
-   ()
-   (send self :angle-vector (float-vector 90.0 -30.0 -10.0 10.0 -40.0 -40.0)))
-  ;;
-  ;; def index avoid methods
-  ;;  index avoid methods -> does not use index finger to avoid overload ;; index finger maximum torque is very low.
-  ;;  User can customize index finger joint angles by using index-angle-vector argument
-  ;;  Currently we prepare index avoid method for :hook-pose, :reset-pose, :hook-pose2, :distal-pose2
-  (:def-index-avoid-pose-methods
-   ()
-   ;;
-   ;;
-   (dolist (pose (list :hook-pose :reset-pose :hook-pose2 :distal-pose2 :grasp-pose))
-     (eval
-      `(defmethod ,(send (class self) :name)
-         (,(read-from-string (format nil ":index-avoid-~A" (string-downcase pose)))
-          (&optional (index-angle-vector (float-vector -10 -10 -40)))
-          (send self ,pose)
-          (send self :f1-1r :joint-angle (elt index-angle-vector 0))
-          (send self :f1-1p :joint-angle (elt index-angle-vector 1))
-          (send self :f1-2r :joint-angle (elt index-angle-vector 2))
-          (send self :angle-vector)
-          ))))
-   )
-  ;;
-  (:f1-1r (&rest args) (forward-message-to (send self (read-from-string (format nil "~A-f1-1r" (find-if #'(lambda (x) (send self x)) '(:rarm :larm))))) args))
-  (:f1-1p (&rest args) (forward-message-to (send self (read-from-string (format nil "~A-f1-1p" (find-if #'(lambda (x) (send self x)) '(:rarm :larm))))) args))
-  (:f1-2r (&rest args) (forward-message-to (send self (read-from-string (format nil "~A-f1-2r" (find-if #'(lambda (x) (send self x)) '(:rarm :larm))))) args))
-  (:f2-2r (&rest args) (forward-message-to (send self (read-from-string (format nil "~A-f2-2r" (find-if #'(lambda (x) (send self x)) '(:rarm :larm))))) args))
-  (:t-1y (&rest args) (forward-message-to (send self (read-from-string (format nil "~A-t-1y" (find-if #'(lambda (x) (send self x)) '(:rarm :larm))))) args))
-  (:t-1p (&rest args) (forward-message-to (send self (read-from-string (format nil "~A-t-1p" (find-if #'(lambda (x) (send self x)) '(:rarm :larm))))) args))
-  )
-  )
-
 (defun get-hrp2-with-hand-class-methods ()
  '(
-  (:init
-   (&rest args)
-   (prog1
-       (send* self :init-org args)
-     (send self :put :lhand-model (instance hrp3hand_l-robot :init))
-     (send self :put :rhand-model (instance hrp3hand_r-robot :init))
-     (dolist (h (list (send self :get :lhand-model) (send self :get :rhand-model)))
-       (send h :def-index-avoid-pose-methods))
-     (mapcar #'(lambda (l lm)
-                 (send (send self :get lm) :newcoords
-                       (send (send self l :end-coords :parent) :copy-worldcoords))
-                 (send (send self l :end-coords :parent) :assoc (send self :get lm)))
-             '(:larm :rarm) '(:lhand-model :rhand-model))
-     (setq bodies (append bodies
-                          (send (send self :get :lhand-model) :bodies)
-                          (send (send self :get :rhand-model) :bodies)))
-     ))
   (:hand (arm &rest args)
-   (let (hr ret)
-     (case arm
-       (:larm
-        (setq hr (send self :get :lhand-model))
-        (setq ret (forward-message-to hr args)))
-       (:rarm
-        (setq hr (send self :get :rhand-model))
-        (setq ret (forward-message-to hr args)))
-       ((:both :arms)
-        (setq hr (send self :get :lhand-model))
-        (push (forward-message-to hr args) ret)
-        (setq hr (send self :get :rhand-model))
-        (push (forward-message-to hr args) ret))
-       )
-     ret))
+   (case arm
+     (:rarm (send* self :limb :rhand args))
+     (:larm (send* self :limb :lhand args))
+     ((:both :arms) (list (send* self :limb :rhand args) (send* self :limb :lhand args)))))
+  (:limb
+   (limb &optional method &rest args)
+   (if (member limb '(:rhand :lhand))
+       (let ((av (case method
+                   (:open-pose (float-vector  0 60 -10 30  0  0))
+                   (:preclose-pose (float-vector  40  60 -10  35  10 90))
+                   (:close-pose (float-vector  65   0  30  50  90 90))
+                   (:reset-pose (float-vector  0 0 0 0 0 0))
+                   ;;
+                   (:standard-pose (float-vector  20 90 0 10 -20 -20))
+                   (:hook-pose (float-vector  90 90 0 10 -20 -20))
+                   (:index-pose (float-vector  60 90 0 70 -20 -20))
+                   (:extension-pose (float-vector  90 30 0 10 -20 -20))
+                   (:distal-pose (float-vector  50 60 -20 10 20 40))
+                   (:hook-pose2 (float-vector  90 70 0 10 -20 -40))
+                   (:distal-pose2 (float-vector  90 90 -20 10 20 60))
+                   (:grasp-pose (float-vector 77.9709 -11.4732 8.28742 -16.3569 106.185 86.0974))
+                   (:index-avoid-extention-pose2 (float-vector 90.0 -30.0 -10.0 10.0 -40.0 -40.0))
+                   (:index-avoid-hook-pose (float-vector  90 90 -10 -10 -40 -20))
+                   (:index-avoid-reset-pose (float-vector  0 0 -10 -10 -40 0))
+                   (:index-avoid-hook-pose2 (float-vector  90 70 -10 -10 -40 -40))
+                   (:index-avoid-distal-pose (float-vector  50 60 -10 -10 -40 40))
+                   (:index-avoid-grasp-pose (float-vector 77.9709 -11.4732 -10 -10 -40 86.0974)))))
+         (if av
+             (send-super :limb limb :angle-vector av)
+             (send-super* :limb limb method args)))
+       (send-super* :limb limb method args)))
   (:hand-angle-vector
    (&optional (av))
    (when av
-     (send self :hand :rarm :angle-vector (subseq av 0 6))
-     (send self :hand :larm :angle-vector (subseq av 6))
+     (send self :limb :rhand :angle-vector (subseq av 0 6))
+     (send self :limb :lhand :angle-vector (subseq av 6))
      )
    (concatenate float-vector
-                (send self :hand :rarm :angle-vector)
-                (send self :hand :larm :angle-vector))
+                (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))
    )
-  )
  )
-
-(eval
- `(defmethod hrp3hand_l-robot
-    ,@(get-hrp3hand-class-methods)))
-
-(eval
- `(defmethod hrp3hand_r-robot
-    ,@(get-hrp3hand-class-methods)))

hrpsys_ros_bridge_tutorials/models/HRP2G.PDgain.dat

@@ -0,0 +1,23 @@
+2.5 0.4 25
+1.2 0.4 12
+
+4.0 0.4 40
+4.0 0.4 40
+
+1.5 0.4 15
+1.5 0.4 15
+1 0.4 10
+2 0.4 20
+2 0.4 20
+1 0.4 10
+1 0.4 10
+1 0.4 10
+
+1.5 0.4 15
+1.5 0.4 15
+1 0.4 10
+2 0.4 20
+2 0.4 20
+1 0.4 10
+1 0.4 10
+1 0.4 10

hrpsys_ros_bridge_tutorials/models/HRP2G.PDgains_sim.dat

@@ -0,0 +1,23 @@
+3200 20
+10900 42
+
+330 2.5
+330 2.5
+
+3600 6.7
+3700 6.7
+430 10
+4350 5
+700 5
+460 10
+1870 10
+700 5
+
+3600 6.7
+3700 6.7
+430 10
+4350 5
+700 5
+460 10
+1870 10
+700 5

hrpsys_ros_bridge_tutorials/models/HRP2G_withoutGrippers.PDgain.dat

@@ -0,0 +1,23 @@
+2.5 0.4 25
+1.2 0.4 12
+
+4.0 0.4 40
+4.0 0.4 40
+
+1.5 0.4 15
+1.5 0.4 15
+1 0.4 10
+2 0.4 20
+2 0.4 20
+1 0.4 10
+1 0.4 10
+0 0 0
+
+1.5 0.4 15
+1.5 0.4 15
+1 0.4 10
+2 0.4 20
+2 0.4 20
+1 0.4 10
+1 0.4 10
+0 0 0