From afb0dbd2daee08758c32c8f53d8e1ad2ee1f4ac0 Mon Sep 17 00:00:00 2001
From: Ethan Gordon <ekgordon@cs.uw.edu>
Date: Wed, 23 Aug 2023 01:44:52 -0700
Subject: [PATCH] Add Kinova JACO + MICO definitions to robot (#32)

* Added Kinova JACO + MICO robot definition

* Match moveit groups to https://github.com/Kinovarobotics/kinova-ros/tree/noetic-devel/kinova_moveit

* Include gripper type and assistive/service designation in kinova prefix

* Added pre-commit hooks
---
 pymoveit2/robots/kinova.py | 51 ++++++++++++++++++++++++++++++++++++++
 1 file changed, 51 insertions(+)
 create mode 100644 pymoveit2/robots/kinova.py

diff --git a/pymoveit2/robots/kinova.py b/pymoveit2/robots/kinova.py
new file mode 100644
index 0000000..b0878dc
--- /dev/null
+++ b/pymoveit2/robots/kinova.py
@@ -0,0 +1,51 @@
+from typing import List
+
+# Documentation:
+# https://github.com/Kinovarobotics/kinova-ros/
+
+MOVE_GROUP_ARM: str = "arm"
+MOVE_GROUP_GRIPPER: str = "gripper"
+
+JACO_PREFIX: str = "j2"
+MICO_PREFIX: str = "m1"
+
+
+def get_prefix(
+    version_prefix: str = JACO_PREFIX,
+    arm_dof: int = 6,
+    hand_dof: int = 2,
+    spherical=False,
+    assistive=False,
+) -> str:
+    return (
+        version_prefix
+        + ("s" if spherical else "n")
+        + str(arm_dof)
+        + ("a" if assistive else "s")
+        + str(hand_dof)
+        + "00_"
+    )
+
+
+def joint_names(prefix: str = get_prefix()) -> List[str]:
+    res = []
+    arm_dof = int(prefix[3])
+    for i in range(arm_dof):
+        res.append(prefix + "joint_" + str(i + 1))
+    return res
+
+
+def base_link_name(prefix: str = get_prefix()) -> str:
+    return prefix + "link_base"
+
+
+def end_effector_name(prefix: str = get_prefix()) -> str:
+    return prefix + "end_effector"
+
+
+def gripper_joint_names(prefix: str = get_prefix()) -> List[str]:
+    res = []
+    hand_dof = int(prefix[5])
+    for i in range(hand_dof):
+        res.append(prefix + "joint_finger_" + str(i + 1))
+    return res