Merge pull request #551 from Naoki-Hiraoka/fix-bag-of-extended-previe…

…w-controller fix bug of extended-preview-controller
euslisp · Aug 28, 2019 · ecfc50e · ecfc50e · k-okada · Aug 28, 2019
2 parents 9f64ae9 + 11c2611
commit ecfc50e
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 1 deletion.
diff --git a/irteus/irtdyna.l b/irteus/irtdyna.l
@@ -1446,7 +1446,7 @@
 	   (R+btPb-1bt (m* R+btPb-1 bt))
 	   (qt (scale-matrix Q ct)))
       (dotimes (i delay)
-	(let* ((qt (if (= i (1- delay)) (m* P qt) qt))
+	(let* ((qt (if (= i (1- delay)) (m* P ct) qt))
                (fa (m* R+btPb-1 (m* bt (m* gsi qt)))))
           (setq gsi (m* A-bKt gsi))
           (dotimes (ii input-dim)

diff --git a/irteus/test/irteus-demo.l b/irteus/test/irteus-demo.l
@@ -89,6 +89,13 @@
   (assert
    (every #'identity (test-preview-control-0 :preview-controller-class extended-preview-controller))))
 
+(deftest test-test-extended-preview-control-0-QR
+  (assert
+   (every #'identity (mapcar #'(lambda (x y) (and (eps= (cadr (memq :zmp x)) (cadr (memq :zmp y)))
+                                                  (eps= (cadr (memq :cog x)) (cadr (memq :cog y)))))
+                             (test-preview-control-0 :preview-controller-class extended-preview-controller :q 1 :r 1e-6)
+                             (test-preview-control-0 :preview-controller-class extended-preview-controller :q 1e1 :r 1e-5)))))
+
 (deftest test-test-preview-control-1
   (assert
    (every #'identity (test-preview-control-1))))