Override nextage-robot

Author: Michi-Tsubaki

nextage_tutorials/euslisp/nextage-utils.l

@@ -1,18 +1,52 @@
-(defun nextage-collision-check ()
-  (let ((ret (send *nextage* :self-collision-check)))
-    (when ret
-      (setq ret (remove-if #'(lambda (l)
-                               (or (and (eq (car l) (send *nextage* :link "CHEST_JOINT0_Link"))
-                                        (eq (cdr l) (send *nextage* :link "HEAD_JOINT1_Link")))
-                                   (and (eq (cdr l) (send *nextage* :link "CHEST_JOINT0_Link"))
-                                        (eq (car l) (send *nextage* :link "HEAD_JOINT1_Link")))
-                                   (and (eq (car l) (send *nextage* :link "LARM_JOINT4_Link"))
-                                        (eq (cdr l) (send *nextage* :link "LARM_JOINT5_Link")))
-                                   (and (eq (cdr l) (send *nextage* :link "LARM_JOINT4_Link"))
-                                        (eq (car l) (send *nextage* :link "LARM_JOINT5_Link")))
-                                   (and (eq (car l) (send *nextage* :link "RARM_JOINT4_Link"))
-                                        (eq (cdr l) (send *nextage* :link "RARM_JOINT5_Link")))
-                                   (and (eq (cdr l) (send *nextage* :link "RARM_JOINT4_Link"))
-                                        (eq (car l) (send *nextage* :link "RARM_JOINT5_Link")))))
-                           ret)))
-    ret))
+(require "package://nextage_tutorials/nextage.l")
+(require "package://nextage_tutorials/euslisp/nextage-interface.l")
+
+(defmethod NextageOpen-robot
+  (:inverse-kinematics
+   (target-coords &rest args &key (avoid-collision-distance 5) &allow-other-keys)
+   (let ((r) (original-av (send self :angle-vector)))
+     (setq r (send-super* :inverse-kinematics target-coords :avoid-collision-distance avoid-collision-distance :warnp nil :dump-command nil args))
+     (unless r
+       (format *error-output* "; failed for normal ik, try to start from ik-friendly position~%")
+       (let* ((move-joints (send-all (cadr (memq :link-list args)) :joint))
+              (av-after-first-ik (send self :angle-vector)))
+         (send self :angle-vector original-av)
+         (send self :reset-pose)
+         (send self :head :angle-vector #f(0 60))
+         (send self :larm :move-end-pos #f(0 0 50))
+         (send self :rarm :move-end-pos #f(0 0 50))
+         (mapcar #'(lambda (j a)
+                     (if (not (memq j move-joints))
+                         (send j :joint-angle a)))
+                 (send self :joint-list) original-av)
+         (setq r (send-super* :inverse-kinematics target-coords :avoid-collision-distance avoid-collision-distance :warnp nil :dump-command nil args))
+         (unless r
+           (if (and (memq :revert-if-fail args) (null (cadr (memq :revert-if-fail args))))
+               (send self :angle-vector original-av)
+             (send self :angle-vector av-after-first-ik)))))
+     r))
+  (:l/r-reverse
+    (av)
+    (let ((av-tmp (copy-list av)))
+      (setf (elt av-tmp 0) (- (elt av-tmp 0)))
+      (setf (elt av-tmp 3) (- (elt av-tmp 3)))
+      (setf (elt av-tmp 5) (- (elt av-tmp 5)))
+      av-tmp))
+  (:self-collision-check (&rest args)
+    (let ((ret (send-super* :self-collision-check args)))
+      (when ret
+        (setq ret (remove-if #'(lambda (l)
+                                 (or (and (eq (car l) (send self :link "CHEST_JOINT0_Link"))
+                                          (eq (cdr l) (send self :link "HEAD_JOINT1_Link")))
+                                     (and (eq (cdr l) (send self :link "CHEST_JOINT0_Link"))
+                                          (eq (car l) (send self :link "HEAD_JOINT1_Link")))
+                                     (and (eq (car l) (send self :link "LARM_JOINT4_Link"))
+                                          (eq (cdr l) (send self :link "LARM_JOINT5_Link")))
+                                     (and (eq (cdr l) (send self :link "LARM_JOINT4_Link"))
+                                          (eq (car l) (send self :link "LARM_JOINT5_Link")))
+                                     (and (eq (car l) (send self :link "RARM_JOINT4_Link"))
+                                          (eq (cdr l) (send self :link "RARM_JOINT5_Link")))
+                                     (and (eq (cdr l) (send self :link "RARM_JOINT4_Link"))
+                                          (eq (car l) (send self :link "RARM_JOINT5_Link")))))
+                             ret)))
+      ret)))