Add body inverse kinematics to control spot pose in euslisp

[708yamaguchi]

sample movie
https://user-images.githubusercontent.com/19769486/141134905-9945f8e5-5d96-4e19-9d25-b4f5e96d9592.mp4

sample code:

(load "package://spoteus/spot-utils.l")

(spot)
(objects (list *spot*))

(dotimes (i 100)
  (send *spot* :body-inverse-kinematics
        (make-coords :pos (float-vector 0 (* 100 (cos (* pi i 0.02))) (* 100 (sin (* pi i 0.02))))))
  (unix:usleep 20000)
  (send *irtviewer* :redraw))

(dotimes (i 100)
  (send *spot* :body-inverse-kinematics
        (make-coords :pos (float-vector 0 0 (* 20 (sin (* pi i 0.02))))
                     :rpy (float-vector (* 0.2 (sin (* pi i 0.02))) (* 0.2 (sin (* pi i 0.02))) (* 0.2 (sin (* pi i 0.02))))))
  (unix:usleep 20000)
  (send *irtviewer* :redraw))

[708yamaguchi]

@sktometometo

[sktometometo]

@708yamaguchi Please add this also to the develop branch (develop/spot)