Cacher is the code snippet organizer for pro developers

;; vim: set ft=lisp:
(require "package://jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/baxter.l")
; (require "package://jsk_2016_01_baxter_apc/robots/baxter.l")

(setq base->kinect2-torso
      (make-coords
        :pos (float-vector (* 1000 0.21) (* 1000 0.075) (* 1000 0.45))
        :rpy (float-vector -0.01 -1.57 3.14)
        )
      )

(setq *baxter* (instance baxter-robot :init))
(setq *robot* *baxter*)

(defmethod baxter-robot
  (:ik-prepared-poses
    ()
    (list
      :reset-pose
      :untuck-pose
      :fold-pose-upper
      :fold-pose-middle
      :fold-pose-lower
      :fold-pose-back
      :fold-to-keep-object
      )
    )
  )

(setq base (send *robot* :base_lk :worldcoords))

(setq kinect2-torso (send *robot* :base_lk :worldcoords :copy-worlcoords))
(send kinect2-torso :transform base->kinect2-torso)

(setq kinect2-torso->larm-goal
      (make-coords
        :pos (float-vector (* 1000 0.1) (* 1000 -0.3) (* 1000 0.6))
        :rpy (float-vector 0 0 0)
        )
      )
(setq larm-goal (send kinect2-torso :copy-worldcoords))
(send larm-goal :transform kinect2-torso->larm-goal)

(setq kinect2-torso->rarm-goal
      (make-coords
        :pos (float-vector (* 1000 -0.1) (* 1000 0.3) (* 1000 0.6))
        :rpy (float-vector 0 0 0)
        )
      )
(setq rarm-goal (send kinect2-torso :copy-worldcoords))
(send rarm-goal :transform kinect2-torso->rarm-goal)


(objects
  (list
    *robot*
    base
    kinect2-torso
    ; (send *robot* :larm :end-coords :worldcoords)
    ; (send *robot* :rarm :end-coords :worldcoords)
    ; larm-goal
    ; rarm-goal
    )
  )

;; -------------------------------------------------------------------------------
;; step-z
;; -------------------------------------------------------------------------------
(defun run-z ()
  (setq step-z 16)
  (dotimes (sz step-z)
    (let ((tc (send rarm-goal :copy-worldcoords))
          (mt (send *robot* :rarm :end-coords)))
      ; (send tc :rotate (+ -pi (* sz (/ pi step-z))) :z :local)
      (send tc :rotate (* sz (/ (* 2 pi) step-z)) :z :local)
      (setq av
            (send *robot* :rarm :inverse-kinematics tc
                  :use-gripper t
                  :rotation-axis t
                  :translation-axis t
                  :revert-if-fail nil)
            )
      ; (setq av
      ;       (send *robot* :rarm :inverse-kinematics tc
      ;             ;; fit the rotation
      ;             :use-gripper t :translation-axis nil :thre 10 ; :debug-view :no-message
      ;             ;; second task with null-space, fit the position
      ;             :additional-check #'(lambda () (> 100 (norm (send mt :difference-position tc))))
      ;             :null-space #'(lambda ()
      ;                             (scale 0.3
      ;                                    (transform
      ;                                      (send *robot* :calc-inverse-jacobian
      ;                                            (send *robot* :calc-jacobian-from-link-list
      ;                                                  (send *robot* :link-list
      ;                                                        (send mt :parent)
      ;                                                        (send *robot* :rarm :root-link))
      ;                                                  :move-target mt
      ;                                                  :translation-axis t :rotation-axis nil))
      ;                                      (send mt :difference-position tc))))
      ;             )
      ;       )
      (format t "av: ~a, rotation: ~a ~a, distance: ~a~%"
              av
              (send tc :worldrot)
              (send *robot* :rarm :end-coords :worldrot)
              (norm (v- (send *robot* :rarm :end-coords :worldpos)
                        (send kinect2-torso :worldpos)))
              )
      (objects (list *robot* base kinect2-torso tc))
      (unix::sleep 1)
      )
    )
  )

;; -------------------------------------------------------------------------------
;; step-x
;; -------------------------------------------------------------------------------
(defun run-x ()
  (setq step-x 8)
  (dotimes (sx step-x)
    (let ((tc (send rarm-goal :copy-worldcoords)))
      (send tc :rotate (+ -pi/2 (* sx (/ pi step-x))) :x :local)
      (send *robot* :rarm :inverse-kinematics tc
            :use-gripper t
            :translation-axis t
            :rotation-axis t
            :revert-if-fail nil
            )
      (objects (list *robot* base kinect2-torso tc))
      (unix::sleep 1)
      )
    )
  )

;; -------------------------------------------------------------------------------
;; step-y
;; -------------------------------------------------------------------------------
(defun run-y ()
  (setq step-y 8)
  (dotimes (sy step-y)
    (let ((tc (send rarm-goal :copy-worldcoords)))
      (send tc :rotate (+ -pi/2 (* sy (/ pi step-y))) :y :local)
      (send *robot* :rarm :inverse-kinematics tc
            :use-gripper t
            :translation-axis t
            :rotation-axis t
            :revert-if-fail nil
            )
      (objects (list *robot* base kinect2-torso tc))
      (unix::sleep 1)
      )
    )
  )

;; -------------------------------------------------------------------------------
;; view-hand-pose
;; -------------------------------------------------------------------------------

#|
(dolist (pose (list
                :view-hand-pose-b
                :view-hand-pose-c
                :view-hand-pose-e
                :view-hand-pose-f
                :view-hand-pose-h
                :view-hand-pose-i
                :view-hand-pose-k
                :view-hand-pose-l))
  (send *robot* pose :rarm)
  (setq distance
        (norm
          (v-
            (send *robot* :rarm :end-coords :worldpos)
            (send kinect2-torso :worldpos)
            )
          )
        )
  (format t "pose: ~a, distance: ~a~%" pose distance)
  )
|#