dump ik log both sucess/fail

irteus/irtmodel.l

@@ -1732,7 +1732,7 @@
                           (t (make-list (length move-target) :initial-element (deg2rad 1)))))
                   (union-link-list)
 		  (centroid-thre 1.0) (target-centroid-pos) (centroid-offset-func)
-		  (dump-command "/tmp")
+		  (dump-command t)
 		  &allow-other-keys)
    ;; target-coords, move-target, rotation-axis, translation-axis, link-list
    ;; -> both list and atom OK.
@@ -1747,14 +1747,35 @@
 	  ;;
           (success t) 
 	  (old-analysis-level (send-all union-link-list :analysis-level))
-          ik-args)       
+          command-directory command-filename command-id ik-args)       
      (send-all union-link-list :analysis-level :coords)
      ;; argument check
      (when (or (null link-list) (null move-target))
        (warn ";; ERROR: :link-list or :move-target required~%")
        (return-from :inverse-kinematics t))
      (if (and (null translation-axis) (null rotation-axis))
          (return-from :inverse-kinematics t))
+     ;; setup fname for log
+     (when dump-command
+       (setq command-directory
+	     (format nil "/tmp/irtmodel-ik-~A" (unix::getpid))
+	     command-id
+	     (let ((lt (unix::localtime))) (substitute #\0 #\  (format nil "~04d-~02d-~02d-~02d-~02d-~02d" (+ 1900 (elt lt 5)) (elt lt 4) (elt lt 3) (elt lt 2) (elt lt 1) (elt lt 0))))
+	     command-filename
+	     (format nil "~A-~A" (send (class self) :name) command-id))
+       (unix::mkdir command-directory)
+       (with-open-file
+	(f (format nil "~A/~A.l" command-directory command-filename) :direction :output)
+	(format f ";; ik ~A log at ~A on ~A~%;;~%" (if success "success" "fail") (string-trim '(10) (unix:asctime (unix:localtime))) lisp::lisp-implementation-version)
+	(format f ";; link-list ~A~%" link-list)
+	(format f ";; move-target ~A~%" move-target)
+	(format f ";; rotatoin-axis ~A, translation-axis ~A~%" rotation-axis translation-axis)
+	(format f ";; thre ~A, rthre ~A, stop ~A~%" thre rthre stop)
+	(if (atom target-coords)
+	    (dump-structure f `(setq c0 ,target-coords))
+	  (dump-structure f `(setq c0 ',(mapcar #'(lambda (x) x) target-coords))))
+	(dump-structure f `(setq av0 ,(send self :angle-vector)))
+	))
      ;; atom -> list
      (when debug-view
        (if (atom debug-view) (setq debug-view (list debug-view)))
@@ -1829,6 +1850,9 @@
 		   centroid-thre target-centroid-pos centroid-offset-func)))
         ;; update difference
      (mapcar #'(lambda (l a) (send l :analysis-level a)) union-link-list old-analysis-level)
+     ;; rename log file
+     (when dump-command
+       (unix::rename (format nil "~A/~A.l" command-directory command-filename ) (format nil "~A/~A-~A.l" command-directory command-filename (if (or success (not revert-if-fail)) "success" "failure"))))
      ;; check solved or not
      (if (or success (not revert-if-fail))
          (send self :angle-vector)
@@ -1846,7 +1870,7 @@
 	   (warn ";;  angles : ~a~%" av0)
 	   (warn ";;    args : ~a~%" (append (list target-coords) args))
 	   (when dump-command
-	     (let (i (print-args (copy-list args)) command-init command-setup command-args command-id)
+	     (let (i (print-args (copy-list args)) command-init command-setup command-args)
 	       (dotimes (j (count :link-list print-args))
 		 (if (setq i (position :link-list print-args :count (1+ j)))
 		     (cond ((atom (car (elt print-args (+ i 1))))
@@ -1870,15 +1894,13 @@
 						  (elt print-args (+ i 1)))))))))
 	       (setq command-init `(instance ,(send (class self) :name) :init)
 		     command-setup `(progn  (send r :newcoords (make-coords :4x4 ,(send self :4x4))) (mapc #'(lambda (j a) (send* j :joint-angle a ,joint-args)) (list . ,(mapcar #'(lambda (x) `(send r ,(intern (string-upcase x) *keyword-package*))) (send-all (remove-duplicates (append (send-all union-link-list :joint) joint-list)) :name))) ,(list 'quote av0)) (objects (list r)))
-		     command-args (nconc `(list . ,(mapcar #'(lambda (x) `(make-coords :pos ,(send x :worldpos) :rot ,(send x :worldrot))) target-coords)) (list :dump-command nil :debug-view t) print-args)
-		     command-id (let ((lt (unix::localtime))) (substitute #\0 #\  (format nil "~A-~04d-~02d-~02d-~02d-~02d-~02d" (send (class self) :name) (+ 1900 (elt lt 5)) (elt lt 4) (elt lt 3) (elt lt 2) (elt lt 1) (elt lt 0)))))
+		     command-args (nconc `(list . ,(mapcar #'(lambda (x) `(make-coords :pos ,(send x :worldpos) :rot ,(send x :worldrot))) target-coords)) (list :dump-command nil :debug-view t) print-args))
 	       (warn ";; command : ~a~%" `(let ((r ,command-init)) ,command-setup (send* r :inverse-kinematics ,command-args)))
-	       (warn ";; dump debug command to /~A/~A.l~%" dump-command command-id)
-	       (warn ";; (progn (load \"/~A/~A.l\")(ik-setup)(ik-check))~%" dump-command command-id)
+	       (warn ";; dump debug command to ~A/~A~%" command-directory command-filename)
+	       (warn ";; (progn (load \"~A\")(ik-setup)(ik-check))~%" command-directory command-filename)
 	       ;; dump
 	       (with-open-file
-		(f (format nil "/~A/~A.l" dump-command command-id) :direction :output)
-		(format f ";; ik ~A log at ~A on ~A~%~%" (if success "success" "fail") (string-trim '(10) (unix:asctime (unix:localtime))) lisp::lisp-implementation-version)
+		(f (format nil "~A/~A-failure.l" command-directory command-filename) :direction :output  :if-exists :append)
 		(format f "(defun ~A-setup () (let ((r ~A)) (setq *robot* r) ~A (objects (list *robot*))))~%" command-id command-init command-setup)
 		(format f "(defun ~A-check () (send* *robot* :inverse-kinematics ~A))~%" command-id command-args)
 		(format f "(defun ik-setup () (~A-setup))~%" command-id)