Add static balance point method and test for it

irteus/irtrobot.l

@@ -939,6 +939,35 @@
      (setq links (append links (list l0 l1 l2)))
      (setq joint-list (append joint-list (list j0 j1 j2)))
      ))
+  (:calc-static-balance-point
+   (&key (target-points
+          (mapcar #'(lambda (tmp-arm)
+                      (send (send self tmp-arm :end-coords) :worldpos))
+                  '(:rarm :larm)))
+         (force-list (make-list (length target-points) :initial-element (float-vector 0 0 0)))
+         (moment-list (make-list (length target-points) :initial-element (float-vector 0 0 0)))
+         (static-balance-point-height (elt (apply #'midpoint 0.5 (send self :legs :end-coords :worldpos)) 2))
+         (update-mass-properties t))
+   "Calculate static balance point which is equivalent to static extended ZMP.
+    target-points are end-effector points on which force-list and moment-list apply.
+    force-list [N] and moment-list [Nm] are list of force and moment at target-points.
+    static-balance-point-height is height of static balance point [mm]."
+   (let* ((sbp (float-vector 0 0 static-balance-point-height))
+          (mg (* 1e-6 (send self :weight nil) (elt *g-vec* 2))))
+     (dotimes (idx 2)
+       (let ((denom mg)
+             (nume (* mg (elt (send self :centroid update-mass-properties) idx))))
+         (mapcar #'(lambda (f m tp)
+                     (setq nume
+                           (+ nume
+                              (- (* (- (elt tp 2) static-balance-point-height) (elt f idx))
+                                 (* (elt tp idx) (elt f 2)))
+                              (case idx (0 (- (elt m 1))) (1 (elt m 0))))
+                           denom (- denom (elt f 2))))
+                 force-list moment-list target-points)
+         (setf (elt sbp idx) (/ nume denom))
+         ))
+     sbp))
   )
 (in-package "GEOMETRY")
 

irteus/test/test-irt-motion.l

@@ -726,6 +726,35 @@
       )
     ))
 
+;; test for static balance point
+(defun fullbody-ik-with-forces (robot &key (debug-view :no-message))
+  (send robot :newcoords (make-coords))
+  (send robot :reset-pose)
+  (send robot :fix-leg-to-coords (make-coords))
+  (send robot :legs :move-end-pos (float-vector 0 0 50))
+  (send robot :fix-leg-to-coords (make-coords))
+  (objects (list robot))
+  (let* ((centroid-thre 10)
+         (result
+          (send robot :fullbody-inverse-kinematics
+                (list (send robot :rleg :end-coords :copy-worldcoords)
+                      (send robot :lleg :end-coords :copy-worldcoords)
+                      (make-coords :pos #f(150 -300 600))
+                      (make-coords :pos #f(150 300 600)))
+                :move-target (mapcar #'(lambda (x)
+                                         (send robot x :end-coords))
+                                     (list :rleg :lleg :rarm :larm))
+                :link-list (mapcar #'(lambda (x)
+                                       (send robot :link-list (send robot x :end-coords :parent)))
+                                   (list :rleg :lleg :rarm :larm))
+                :centroid-thre centroid-thre
+                :centroid-offset-func #'(lambda () (send robot :calc-static-balance-point :force-list (list #f(10 20 0) #f(25 20 0))))
+                :debug-view debug-view)))
+    (and result
+         (> centroid-thre (distance (apply #'midpoint 0.5 (send robot :legs :end-coords :worldpos))
+                                    (send robot :calc-static-balance-point :force-list (list #f(10 20 0) #f(25 20 0))))))
+    ))
+
 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
 ;; unit tests
 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
@@ -795,5 +824,9 @@
   (assert
    (test-self-collision-check-IK)))
 
+(deftest test-fullbody-ik-with-forces-samplerobot
+  (assert
+   (fullbody-ik-with-forces *sample-robot*)))
+
 (run-all-tests)
 (exit 0)