Merge pull request #551 from Naoki-Hiraoka/fix-bag-of-extended-preview-controller

fix bug of extended-preview-controller

Author: k-okada

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)

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))))