Add open/close-holder

Author: Michi-Tsubaki

nextage_tutorials/euslisp/jsk-nextage-init.l

@@ -1,8 +1,6 @@
 #!/usr/bin/env roseus
 
 (require "package://nextage_tutorials/euslisp/nextage-utils.l")
-(require "package://dynamixel_detachable_hand/euslisp/lhand-interface.l")
-(require "package://dynamixel_detachable_hand/euslisp/rhand-interface.l")
 (nextage-init)
 (lhand-init)
 (rhand-init)
@@ -15,6 +13,6 @@
 (send *robot* :rarm :move-end-pos #f(0 0 100))
 (send *ri* :angle-vector (send *nextage* :angle-vector) 2000)
 (send *ri* :wait-interpolation)
-(send *rhand* :close-holder)
-(send *lhand* :open-forceps)
+(send *ri* :close-holder :rarm)
+(send *ri* :open-forceps :larm)
 (send *irtviewer* :draw-objects)

nextage_tutorials/euslisp/nextage-interface.l

@@ -77,7 +77,20 @@
       (:larm (send* (send self :lhand-init) :close-forceps args))
       (:rarm (send* (send self :rhand-init) :close-forceps args))
       (:arms (send* (send self :lhand-init) :close-forceps args)
-             (send* (send self :rhand-init) :close-forceps args)))))
+             (send* (send self :rhand-init) :close-forceps args))))
+  (:open-holder (&optional (arm :arms) &rest args)
+    (case arm
+      (:larm (send* (send self :lhand-init) :open-holder args))
+      (:rarm (send* (send self :rhand-init) :open-holder args))
+      (:arms (send* (send self :lhand-init) :open-holder args)
+             (send* (send self :rhand-init) :open-holder args))))
+  (:close-holder (&optional (arm :arms) &rest args)
+    (case arm
+      (:larm (send* (send self :lhand-init) :close-holder args))
+      (:rarm (send* (send self :rhand-init) :close-holder args))
+      (:arms (send* (send self :lhand-init) :close-holder args)
+             (send* (send self :rhand-init) :close-holder args))))
+  )
 
 (defun nextage-init ()
   (if (not (boundp '*ri*))