From 1f4ef9e3c453070beb59b78cece861f1377240cc Mon Sep 17 00:00:00 2001
From: Naoki-Hiraoka <hiraoka@jsk.imi.i.u-tokyo.ac.jp>
Date: Sun, 19 May 2019 09:14:06 +0900
Subject: [PATCH] [irtrobot.l test-irt-motion.l]fix sign and unit of moment in
 :calc-static-balance-point. add a test related to this.

---
 irteus/irtrobot.l             |  4 +--
 irteus/test/test-irt-motion.l | 48 +++++++++++++++++++++++++++++++++++
 2 files changed, 50 insertions(+), 2 deletions(-)

diff --git a/irteus/irtrobot.l b/irteus/irtrobot.l
index 4958e59ef..ae476dded 100644
--- a/irteus/irtrobot.l
+++ b/irteus/irtrobot.l
@@ -1017,7 +1017,7 @@
    "Calculate static balance point which is equivalent to static extended ZMP.
     The output is expressed by the world coordinates.
     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.
+    force-list [N] and moment-list [Nm] are list of force and moment that robot receives 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 update-mass-properties) (elt *g-vec* 2))))
@@ -1029,7 +1029,7 @@
                            (+ 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))))
+                              (* 1e3 (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))
diff --git a/irteus/test/test-irt-motion.l b/irteus/test/test-irt-motion.l
index 63e4049b4..753348cd0 100644
--- a/irteus/test/test-irt-motion.l
+++ b/irteus/test/test-irt-motion.l
@@ -871,6 +871,50 @@
                                     (send robot :calc-static-balance-point :force-list (list #f(10 20 0) #f(25 20 0))))))
     ))
 
+;; test for static balance point
+(defun fullbody-ik-with-forces-moments (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)
+         (force-list '(#f(10 20 0) #f(25 20 0)))
+         (moment-list '(#f(20 -10 0) #f(10 -20 0)))
+         (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 force-list :moment-list moment-list))
+                :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 force-list :moment-list moment-list)))
+         ;;check static balance
+         ;;The sum of force and moment that robot receives at target-points, ZMP and centroid should be almost zero (except yaw moment).
+         (> 1.0
+            (norm (subseq (transform (send robot :calc-grasp-matrix
+                                           (append (mapcar #'(lambda (x) (send robot x :end-coords :worldpos)) (list :rarm :larm))
+                                                   (list (send robot :centroid)
+                                                         (send (send robot :foot-midcoords) :worldpos))))
+                                     (concatenate float-vector
+                                                  (mapcan #'(lambda (f m) (concatenate cons f m))
+                                                          force-list moment-list)
+                                                  (v- (scale (* 1e-6 (send robot :weight)) *g-vec*)) #F(0 0 0)
+                                                  (v- (scale (* 1e-6 (send robot :weight)) *g-vec*) (reduce #'v+ force-list)) #F(0 0 0)))
+                          0 5))))
+    ))
+
 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
 ;; unit tests
 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
@@ -954,6 +998,10 @@
   (assert
    (fullbody-ik-with-forces *sample-robot*)))
 
+(deftest test-fullbody-ik-with-forces-moments-samplerobot
+  (assert
+   (fullbody-ik-with-forces-moments *sample-robot*)))
+
 (deftest test-fullbody-ik-draw-param-check-samplerobot
   (dotimes (i 5) (send *sample-robot* :move-centroid-on-foot :both '(:rleg :lleg)))
   (assert