Cacher is the code snippet organizer for pro developers

(load "baxter.l")

(defun hand-look-at-ik
  (&key (camera-axis :z))
  ;; initialize robot
  (send *irtviewer* :title "look-at-ik")
  (unless (boundp '*robot*)
    (setq *robot* (instance baxter-robot :init)))
  (send *robot* :reset-pose)
  (send *robot* :update-descendants)
  ;; generate object model
  (setq *obj* (make-cube 200 50 50))
  (send *obj* :move-to kinect2-torso)
  (send *obj* :set-color :blue)
  (objects (list *obj* *robot*))
  ;; ik codes
  (let* ((look-at-face (elt (send *obj* :faces) 1))
         (result
           (send *robot* :larm :inverse-kinematics
                (orient-coords-to-axis
                  (make-coords :pos (send *obj* :worldpos)
                               :rot (copy-object (send *robot* :larm :end-coords :worldrot)))
                  (send look-at-face :normal)
                  camera-axis)
                :move-target (send *robot* :larm :end-coords)
                :link-list (send *robot* :link-list (send *robot* :larm :end-coords :parent))
                :rotation-axis camera-axis
                :translation-axis camera-axis
                :debug-view :no-message
                :centroid-thre 20
                :stop 1000)))
    (send *irtviewer* :draw-objects)
    (send look-at-face :draw-on :flush t :color #f(1 0 0) :width 2)
    result))

(setq *irtviewer* (make-irtviewer))
(setq *robot* (instance baxter-robot :init))
(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 kinect2-torso (send *robot* :base_lk :worldcoords :copy-worlcoords))
(send kinect2-torso :transform base->kinect2-torso)
(hand-look-at-ik)