From a77c3f97f0f52bd6b1970f276c839fe3bbf6daeb Mon Sep 17 00:00:00 2001
From: Kei Okada <kei.okada@gmail.com>
Date: Tue, 24 May 2016 21:02:46 +0900
Subject: [PATCH] irtmodel.l manipulability need to take into account weight
 value

---
 irteus/irtmodel.l | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/irteus/irtmodel.l b/irteus/irtmodel.l
index d7095cf4b..89e29fd70 100644
--- a/irteus/irtmodel.l
+++ b/irteus/irtmodel.l
@@ -1053,7 +1053,7 @@
     &allow-other-keys)
    (let (jacobi# m m2 (k 0))
      ;; m : manipulability
-     (setq m (manipulability jacobi tmp-mrr tmat))
+     (setq m (manipulability (if weight (m* jacobi (diagonal weight)) jacobi) tmp-mrr tmat))
      (if (< m ml) (setq k (* mg (expt (- 1.0 (/ m  ml)) 2))))
      (when (and debug-view (not (memq :no-message debug-view)))
        (warn "k     :~7,3f (manipulability:~7,3f, gain:~7,3f, limit:~7,3f, len:~d)~%" k m mg ml (cadr (array-dimensions jacobi))))