Merge pull request start-jsk#129 from hyaguchijsk/hrp2w_grasp_and_bas…

…e_control

Hrp2w grasp and base control

hrpsys_ros_bridge_tutorials/euslisp/hrp2w-interface.l

@@ -10,8 +10,52 @@
   (:init (&rest args)
          (send-super* :init :robot hrp2w-robot args)))
 
-(defun hrp2w-init ()
+;; methods for grasp controller
+(defmethod hrp2w-interface
+  (:start-grasp
+   (limb &key (target-error 0.15) (gain-percentage 5) &rest args)
+   (case limb
+     (:larm
+      (send* self :set-servo-gain-percentage
+	     "LARM_JOINT7" gain-percentage args)
+      (send* self :graspcontrollerservice_startgrasp
+	     :name "LHAND"
+	     :target_error target-error args))
+     (:rarm
+      (send* self :set-servo-gain-percentage
+	     "RARM_JOINT7" gain-percentage args)
+      (send* self :graspcontrollerservice_startgrasp
+	     :name "RHAND"
+	     :target_error target-error args))
+     )
+   )
+  (:stop-grasp
+   (limb &rest args)
+   (case limb
+     (:larm
+      (send* self :set-servo-gain-percentage "LARM_JOINT7" 100 args)
+      (send* self :graspcontrollerservice_stopgrasp
+	     :name "LHAND"
+	     args))
+     (:rarm
+      (send* self :set-servo-gain-percentage "RARM_JOINT7" 100 args)
+      (send* self :graspcontrollerservice_stopgrasp
+	     :name "RHAND"
+	     args))
+     )
+   )
+  )
+
+(defun hrp2w-init (&key (sim nil))
   (if (not (boundp '*ri*))
       (setq *ri* (instance hrp2w-interface :init)))
   (if (not (boundp '*hrp2w*))
-      (setq *hrp2w* (instance hrp2w-robot :init))))
+      (setq *hrp2w* (instance hrp2w-robot :init)))
+  (unless sim
+    (if (not (boundp '*vmax*))
+	(progn
+	  (require :vmax "package://vmax_controller/euslisp/vmax-interface.l")
+	  (setq *vmax* (instance vmax-interface :init)))))
+  (setq ((send *hrp2w* :head :neck-p) . max-angle) 44)
+  )
+

hrpsys_ros_bridge_tutorials/euslisp/hrp2w-utils.l

@@ -0,0 +1,39 @@
+;; stop and start grasp
+(defun hrp2w-stop-grasp (limb)
+  (when (boundp '*hrp2w*)
+    (case limb
+	  (:larm
+	   (send *hrp2w* :larm :thumb-r :joint-angle -45.0)
+	   )
+	  (:rarm
+	   (send *hrp2w* :rarm :thumb-r :joint-angle 45.0)
+	   ))
+    (when (boundp '*ri*)
+      (send *ri* :angle-vector (send *hrp2w* :angle-vector) 1000)
+      (send *ri* :wait-interpolation)
+      (send *ri* :stop-grasp limb)
+      )
+    )
+  )
+
+(defun hrp2w-start-grasp (limb &key
+			       (angle 0.0)
+			       (target-error 0.15)
+			       (gain-percentage 5))
+  (when (boundp '*hrp2w*)
+    (case limb
+	  (:larm
+	   (send *hrp2w* :larm :thumb-r :joint-angle angle)
+	   )
+	  (:rarm
+	   (send *hrp2w* :rarm :thumb-r :joint-angle angle)
+	   ))
+    (when (boundp '*ri*)
+      (send *ri* :start-grasp limb
+	    :target-error target-error
+	    :gain-percentage gain-percentage)
+      (send *ri* :angle-vector (send *hrp2w* :angle-vector) 1000)
+      (send *ri* :wait-interpolation)
+      )
+    )
+  )