From cadf60031f4083f5eb33d52618fb425e4f503cf8 Mon Sep 17 00:00:00 2001
From: mmurooka <murooka@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 23 Jul 2014 15:25:20 +0000
Subject: [PATCH] overwrite :inverse-kinematics and
 :fullbody-inverse-kinematics for hrp2jsknt,nts not to use toe joint as
 default.

---
 .../euslisp/hrp2jsknt-utils.l                     | 15 +++++++++++++++
 .../euslisp/hrp2jsknts-utils.l                    | 14 ++++++++++++++
 2 files changed, 29 insertions(+)

diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-utils.l b/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-utils.l
index afc2475a..34f1ed5c 100644
--- a/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-utils.l
+++ b/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-utils.l
@@ -6,3 +6,18 @@
 (eval
  `(defmethod hrp2jsknt-robot
     ,@(get-hrp2-with-hand-class-methods)))
+
+(defmethod hrp2jsknt-robot
+  (:inverse-kinematics
+   (target-coords
+    &rest args
+    &key (additional-weight-list (mapcar #'(lambda (l) (list (send self l :toe-p :child-link) 0)) '(:rleg :lleg)))
+    &allow-other-keys)
+   (send-super* :inverse-kinematics target-coords :additional-weight-list additional-weight-list args))
+  (:fullbody-inverse-kinematics
+   (target-coords
+    &rest args
+    &key (additional-weight-list (mapcar #'(lambda (l) (list (send self l :toe-p :child-link) 0)) '(:rleg :lleg)))
+    &allow-other-keys)
+   (send-super* :fullbody-inverse-kinematics target-coords :additional-weight-list additional-weight-list args))
+  )
diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-utils.l b/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-utils.l
index f0464780..94614ec8 100644
--- a/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-utils.l
+++ b/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-utils.l
@@ -7,3 +7,17 @@
  `(defmethod hrp2jsknts-robot
     ,@(get-hrp2-with-hand-class-methods)))
 
+(defmethod hrp2jsknts-robot
+  (:inverse-kinematics
+   (target-coords
+    &rest args
+    &key (additional-weight-list (mapcar #'(lambda (l) (list (send self l :toe-p :child-link) 0)) '(:rleg :lleg)))
+    &allow-other-keys)
+   (send-super* :inverse-kinematics target-coords :additional-weight-list additional-weight-list args))
+  (:fullbody-inverse-kinematics
+   (target-coords
+    &rest args
+    &key (additional-weight-list (mapcar #'(lambda (l) (list (send self l :toe-p :child-link) 0)) '(:rleg :lleg)))
+    &allow-other-keys)
+   (send-super* :fullbody-inverse-kinematics target-coords :additional-weight-list additional-weight-list args))
+  )