From 408bce5bf08d2a1677d9221b0c81138835a76101 Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Mon, 31 Jul 2023 19:46:32 +0900
Subject: [PATCH 1/2] [panda_eus] Change dual_panda's end-coords to the same as
 single panda

---
 jsk_panda_robot/README.md                              | 2 +-
 jsk_panda_robot/panda_eus/models/dual_panda.urdf.xacro | 8 ++++----
 jsk_panda_robot/panda_eus/models/dual_panda.yaml       | 8 ++++----
 3 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/jsk_panda_robot/README.md b/jsk_panda_robot/README.md
index eb353bde2b..d035983bce 100644
--- a/jsk_panda_robot/README.md
+++ b/jsk_panda_robot/README.md
@@ -107,7 +107,7 @@
       ```
       - Notice
         - `(send *ri* :recover-error)` is required every time when you press and release the black switch (`activated` -> `monitored stop` -> `activated`).
-        - `dual_panda`'s `end-coords`, `reset-pose`, and `reset-manip-pose` are different from single `panda`'s ones for historical reasons.
+        - `dual_panda`'s `reset-pose` and `reset-manip-pose` are different from single `panda`'s ones for historical reasons.
 #### Record/play rosbag
 ```bash
 roslaunch jsk_panda_startup dual_panda1_record.launch  # Or roslaunch jsk_panda_startup dual_panda2_record.launch
diff --git a/jsk_panda_robot/panda_eus/models/dual_panda.urdf.xacro b/jsk_panda_robot/panda_eus/models/dual_panda.urdf.xacro
index d786546287..12165163c6 100644
--- a/jsk_panda_robot/panda_eus/models/dual_panda.urdf.xacro
+++ b/jsk_panda_robot/panda_eus/models/dual_panda.urdf.xacro
@@ -109,17 +109,17 @@
   <link name="$(arg arm_id_1)_end_effector" >
   </link>
   <joint name="$(arg arm_id_1)_end_effector_joint" type="fixed">
-    <parent link="$(arg arm_id_1)_link7"/>
+    <parent link="$(arg arm_id_1)_link8"/>
     <child link="$(arg arm_id_1)_end_effector"/>
-    <origin xyz="0 0 0.2" rpy="0 -1.570796327 -0.785398163"/>
+    <origin xyz="0 0 0.1" rpy="0 -1.570796327 -0.785398163"/>
   </joint>
 
   <link name="$(arg arm_id_2)_end_effector" >
   </link>
   <joint name="$(arg arm_id_2)_end_effector_joint" type="fixed">
-    <parent link="$(arg arm_id_2)_link7"/>
+    <parent link="$(arg arm_id_2)_link8"/>
     <child link="$(arg arm_id_2)_end_effector"/>
-    <origin xyz="0 0 0.2" rpy="0 -1.570796327 -0.785398163"/>
+    <origin xyz="0 0 0.1" rpy="0 -1.570796327 -0.785398163"/>
   </joint>
 
 </robot>
diff --git a/jsk_panda_robot/panda_eus/models/dual_panda.yaml b/jsk_panda_robot/panda_eus/models/dual_panda.yaml
index 338af0958f..2190c285f4 100644
--- a/jsk_panda_robot/panda_eus/models/dual_panda.yaml
+++ b/jsk_panda_robot/panda_eus/models/dual_panda.yaml
@@ -20,12 +20,12 @@ larm:
 #  - pan_tilt_JOINT1 : head-neck-p
 
 rarm-end-coords:
-  parent: rarm_link7
-  translate: [0, 0, 0.2]
+  parent: rarm_link8
+  translate: [0, 0, 0.1]
   rotate : [-0.35740972270209, -0.86285515644477, -0.35740630816298, 98.42112728719206]
 larm-end-coords:
-  parent: larm_link7
-  translate: [0, 0, 0.2]
+  parent: larm_link8
+  translate: [0, 0, 0.1]
   rotate : [-0.35740972270209, -0.86285515644477, -0.35740630816298, 98.42112728719206]
 
 angle-vector:

From 5f3c67336fa19249e5a5320fc66572ac73b6599e Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Fri, 4 Aug 2023 13:11:04 +0900
Subject: [PATCH 2/2] [panda_eus] Move end-coords of single panda and
 dual_panda to the center of the fingers

---
 .../panda_eus/models/dual_panda.urdf.xacro           |  8 ++++----
 jsk_panda_robot/panda_eus/models/dual_panda.yaml     | 12 ++++++------
 jsk_panda_robot/panda_eus/models/panda.yaml          |  6 +++---
 3 files changed, 13 insertions(+), 13 deletions(-)

diff --git a/jsk_panda_robot/panda_eus/models/dual_panda.urdf.xacro b/jsk_panda_robot/panda_eus/models/dual_panda.urdf.xacro
index 12165163c6..f47eb6c525 100644
--- a/jsk_panda_robot/panda_eus/models/dual_panda.urdf.xacro
+++ b/jsk_panda_robot/panda_eus/models/dual_panda.urdf.xacro
@@ -109,17 +109,17 @@
   <link name="$(arg arm_id_1)_end_effector" >
   </link>
   <joint name="$(arg arm_id_1)_end_effector_joint" type="fixed">
-    <parent link="$(arg arm_id_1)_link8"/>
+    <parent link="$(arg arm_id_1)_hand_tcp"/>
     <child link="$(arg arm_id_1)_end_effector"/>
-    <origin xyz="0 0 0.1" rpy="0 -1.570796327 -0.785398163"/>
+    <origin xyz="0 0 0" rpy="0 -1.570796327 0"/>
   </joint>
 
   <link name="$(arg arm_id_2)_end_effector" >
   </link>
   <joint name="$(arg arm_id_2)_end_effector_joint" type="fixed">
-    <parent link="$(arg arm_id_2)_link8"/>
+    <parent link="$(arg arm_id_2)_hand_tcp"/>
     <child link="$(arg arm_id_2)_end_effector"/>
-    <origin xyz="0 0 0.1" rpy="0 -1.570796327 -0.785398163"/>
+    <origin xyz="0 0 0" rpy="0 -1.570796327 0"/>
   </joint>
 
 </robot>
diff --git a/jsk_panda_robot/panda_eus/models/dual_panda.yaml b/jsk_panda_robot/panda_eus/models/dual_panda.yaml
index 2190c285f4..351154873a 100644
--- a/jsk_panda_robot/panda_eus/models/dual_panda.yaml
+++ b/jsk_panda_robot/panda_eus/models/dual_panda.yaml
@@ -20,13 +20,13 @@ larm:
 #  - pan_tilt_JOINT1 : head-neck-p
 
 rarm-end-coords:
-  parent: rarm_link8
-  translate: [0, 0, 0.1]
-  rotate : [-0.35740972270209, -0.86285515644477, -0.35740630816298, 98.42112728719206]
+  parent: rarm_hand_tcp
+  translate: [0, 0, 0]
+  rotate : [0, -1, 0, 90]
 larm-end-coords:
-  parent: larm_link8
-  translate: [0, 0, 0.1]
-  rotate : [-0.35740972270209, -0.86285515644477, -0.35740630816298, 98.42112728719206]
+  parent: larm_hand_tcp
+  translate: [0, 0, 0]
+  rotate : [0, -1, 0, 90]
 
 angle-vector:
   reset-pose: [ 0.0, 15.0, 0.0, -135.0, 0.0, 150.0, 45.0,
diff --git a/jsk_panda_robot/panda_eus/models/panda.yaml b/jsk_panda_robot/panda_eus/models/panda.yaml
index 03d3457795..08b55ee3f2 100644
--- a/jsk_panda_robot/panda_eus/models/panda.yaml
+++ b/jsk_panda_robot/panda_eus/models/panda.yaml
@@ -8,9 +8,9 @@ rarm:
   - panda_joint7 : rarm-wrist-y
 
 rarm-end-coords:
-  parent: panda_link8
-  translate: [0, 0, 0.1]
-  rotate : [-0.35740972270209, -0.86285515644477, -0.35740630816298, 98.42112728719206]
+  parent: panda_hand_tcp
+  translate: [0, 0, 0]
+  rotate : [0, -1, 0, 90]
 
 angle-vector:
   reset-pose: [ 0.0, -45.0, 0.0, -135.0, 0.0, 90.0, 45.0,