Add nextage model (#616)

* Add nextage robot model and interface

* Add nextage examples

Author: Michi-Tsubaki

examples/batch_ik_demo.py

@@ -8,6 +8,7 @@
 from skrobot.coordinates import Coordinates
 from skrobot.model.primitives import Axis
 from skrobot.models import Fetch
+from skrobot.models import Nextage
 from skrobot.models import Panda
 from skrobot.models import PR2
 from skrobot.models import R8_6
@@ -27,7 +28,7 @@ def parse_axis_constraint(axis_str):
 def main():
     parser = argparse.ArgumentParser(description='Advanced Batch IK Demo with axis constraints')
     parser.add_argument('--robot', type=str, default='pr2',
-                        choices=['fetch', 'pr2', 'panda', 'r8_6'],
+                        choices=['fetch', 'pr2', 'panda', 'r8_6', 'nextage'],
                         help='Robot model to use. Default: fetch')
     parser.add_argument('--rotation-axis', '--rotation_axis', '-r',
                         default='True',
@@ -84,6 +85,9 @@ def main():
     elif args.robot == 'r8_6':
         robot = R8_6()
         arm = robot.rarm
+    elif args.robot == 'nextage':
+        robot = Nextage()
+        arm = robot.rarm
 
     robot.reset_pose()
 
@@ -102,6 +106,23 @@ def main():
             Coordinates(pos=(0.46, -0.24, 0.89)).rotate(np.deg2rad(8), 'z').rotate(np.deg2rad(3), 'x'),
             Coordinates(pos=(0.56, -0.24, 0.89)),
         ]
+    elif args.robot == 'nextage':
+        target_poses = [
+            Coordinates(pos=(0.3, -0.25, -0.1)).rotate(
+                np.pi, 'y').rotate(np.deg2rad(-25), 'z'),
+            Coordinates(pos=(0.3, -0.25, -0.15)).rotate(
+                np.pi, 'y').rotate(np.deg2rad(-30), 'z'),
+            Coordinates(pos=(0.4, -0.2, -0.1)).rotate(
+                np.pi, 'y').rotate(np.deg2rad(15), 'y').rotate(
+                np.deg2rad(-15), 'z'),
+            Coordinates(pos=(0.28, -0.18, -0.08)).rotate(
+                np.pi, 'y').rotate(np.deg2rad(90), 'x'),
+            Coordinates(pos=(0.35, -0.2, -0.12)).rotate(
+                np.pi, 'y').rotate(np.deg2rad(45), 'x'),
+            Coordinates(pos=(0.35, -0.22, -0.08)).rotate(
+                np.pi, 'y').rotate(np.deg2rad(-30), 'y').rotate(
+                np.deg2rad(15), 'z'),
+        ]
     else:
         # Default target poses for Fetch/PR2/Panda
         target_poses = [

examples/robot_models.py

@@ -40,6 +40,7 @@ def main():
     robots = [
         skrobot.models.Kuka(),
         skrobot.models.Fetch(),
+        skrobot.models.Nextage(),
         skrobot.models.PR2(),
         skrobot.models.Panda(),
     ]

skrobot/data/__init__.py

@@ -83,6 +83,20 @@ def kuka_urdfpath():
     return osp.join(data_dir, 'kuka_description', 'kuka.urdf')
 
 
+def nextage_urdfpath():
+    path = osp.join(get_cache_dir(), 'nextage_description', 'urdf', 'NextageOpen.urdf')
+    if osp.exists(path):
+        return path
+    _download(
+        url='https://github.com/iory/scikit-robot-models/raw/refs/heads/master/nextage_description.tar.gz',  # NOQA
+        path=osp.join(get_cache_dir(), 'nextage_description.tar.gz'),
+        md5='9805ac9cd97b67056dde31aa88762ec7',
+        postprocess='extractall',
+        quiet=True,
+    )
+    return path
+
+
 def panda_urdfpath():
     path = osp.join(get_cache_dir(),
                     'franka_description', 'panda.urdf')

skrobot/interfaces/ros/__init__.py

@@ -1,9 +1,11 @@
 from skrobot._lazy_imports import LazyImportClass
 
 
+NextageROSRobotInterface = LazyImportClass(
+    ".nextage", "NextageROSRobotInterface", "skrobot.interfaces.ros")
 PandaROSRobotInterface = LazyImportClass(
     ".panda", "PandaROSRobotInterface", "skrobot.interfaces.ros")
 PR2ROSRobotInterface = LazyImportClass(
     ".pr2", "PR2ROSRobotInterface", "skrobot.interfaces.ros")
 
-__all__ = ["PandaROSRobotInterface", "PR2ROSRobotInterface"]
+__all__ = ["NextageROSRobotInterface", "PandaROSRobotInterface", "PR2ROSRobotInterface"]

skrobot/interfaces/ros/nextage.py

@@ -0,0 +1,59 @@
+import actionlib
+import control_msgs.msg
+
+from .base import ROSRobotInterfaceBase
+
+
+class NextageROSRobotInterface(ROSRobotInterfaceBase):
+
+    def __init__(self, *args, **kwargs):
+        super(NextageROSRobotInterface, self).__init__(*args, **kwargs)
+
+        self.rarm_move = actionlib.SimpleActionClient(
+            '/rarm_controller/follow_joint_trajectory_action',
+            control_msgs.msg.FollowJointTrajectoryAction
+        )
+        self.rarm_move.wait_for_server()
+
+        self.larm_move = actionlib.SimpleActionClient(
+            '/larm_controller/follow_joint_trajectory_action',
+            control_msgs.msg.FollowJointTrajectoryAction
+        )
+        self.larm_move.wait_for_server()
+
+    @property
+    def rarm_controller(self):
+        return dict(
+            controller_type='rarm_controller',
+            controller_action='/rarm_controller/follow_joint_trajectory_action',
+            controller_state='/rarm_controller/state',
+            action_type=control_msgs.msg.FollowJointTrajectoryAction,
+            joint_names=[j.name for j in self.robot.rarm.joint_list],
+        )
+
+    @property
+    def larm_controller(self):
+        return dict(
+            controller_type='larm_controller',
+            controller_action='/larm_controller/follow_joint_trajectory_action',
+            controller_state='/larm_controller/state',
+            action_type=control_msgs.msg.FollowJointTrajectoryAction,
+            joint_names=[j.name for j in self.robot.larm.joint_list],
+        )
+
+    def default_controller(self):
+        return [self.rarm_controller, self.larm_controller]
+
+    def move_arm(self, trajectory, arm='rarm', wait=True):
+        if arm == 'rarm':
+            self.send_trajectory(self.rarm_move, trajectory, wait)
+        elif arm == 'larm':
+            self.send_trajectory(self.larm_move, trajectory, wait)
+
+    def send_trajectory(self, client, trajectory, wait=True):
+        goal = control_msgs.msg.FollowJointTrajectoryGoal()
+        goal.trajectory = trajectory
+        if wait:
+            client.send_goal_and_wait(goal)
+        else:
+            client.send_goal(goal)

skrobot/models/__init__.py

@@ -2,6 +2,7 @@
 
 from skrobot.models.fetch import Fetch
 from skrobot.models.kuka import Kuka
+from skrobot.models.nextage import Nextage
 from skrobot.models.panda import Panda
 from skrobot.models.pr2 import PR2
 from skrobot.models.r8_6 import R8_6

skrobot/models/nextage.py

@@ -0,0 +1,99 @@
+from cached_property import cached_property
+import numpy as np
+
+from ..coordinates import CascadedCoords
+from ..data import nextage_urdfpath
+from ..model import RobotModel
+from .urdf import RobotModelFromURDF
+
+
+class Nextage(RobotModelFromURDF):
+    """
+    - Nextage Open Official Information.
+
+      https://nextage.kawadarobot.co.jp/open
+
+    - Nextage Open Robot Description
+
+      https://github.com/tork-a/rtmros_nextage/tree/indigo-devel/nextage_description/urdf
+    """
+
+    def __init__(self, *args, **kwargs):
+        super(Nextage, self).__init__(*args, **kwargs)
+
+        # End effector coordinates
+        self.rarm_end_coords = CascadedCoords(
+            pos=[-0.185, 0.0, -0.01],
+            parent=self.RARM_JOINT5_Link,
+            name='rarm_end_coords')
+        self.rarm_end_coords.rotate(-np.pi / 2.0, 'y')
+
+        self.larm_end_coords = CascadedCoords(
+            pos=[-0.185, 0.0, -0.01],
+            parent=self.LARM_JOINT5_Link,
+            name='larm_end_coords')
+        self.larm_end_coords.rotate(-np.pi / 2.0, 'y')
+
+        self.head_end_coords = CascadedCoords(
+            pos=[0.06, 0, 0.025],
+            parent=self.HEAD_JOINT1_Link,
+            name='head_end_coords')
+        self.head_end_coords.rotate(np.deg2rad(90), 'y')
+
+        self.reset_pose()
+
+    @cached_property
+    def default_urdf_path(self):
+        return nextage_urdfpath()
+
+    def reset_pose(self):
+        angle_vector = [
+            0.0,
+            0.0,
+            0.0,
+            np.deg2rad(0.6),
+            0.0,
+            np.deg2rad(-100),
+            np.deg2rad(-15.2),
+            np.deg2rad(9.4),
+            np.deg2rad(-3.2),
+            np.deg2rad(-0.6),
+            0.0,
+            np.deg2rad(-100),
+            np.deg2rad(15.2),
+            np.deg2rad(9.4),
+            np.deg2rad(3.2),
+        ]
+        self.angle_vector(angle_vector)
+        return self.angle_vector()
+
+    def reset_manip_pose(self):
+        """Reset robot to manipulation pose (same as reset_pose for Nextage)"""
+        return self.reset_pose()
+
+    @cached_property
+    def rarm(self):
+        link_names = ["RARM_JOINT{}_Link".format(i) for i in range(6)]
+        links = [getattr(self, n) for n in link_names]
+        joints = [l.joint for l in links]
+        model = RobotModel(link_list=links, joint_list=joints)
+        model.end_coords = self.rarm_end_coords
+        return model
+
+    @cached_property
+    def larm(self):
+        link_names = ["LARM_JOINT{}_Link".format(i) for i in range(6)]
+        links = [getattr(self, n) for n in link_names]
+        joints = [l.joint for l in links]
+        model = RobotModel(link_list=links, joint_list=joints)
+        model.end_coords = self.larm_end_coords
+        return model
+
+    @cached_property
+    def head(self):
+        link_names = ["HEAD_JOINT{}_Link".format(i) for i in range(2)]
+        links = [getattr(self, n) for n in link_names]
+        joints = [l.joint for l in links]
+        model = RobotModel(link_list=links, joint_list=joints)
+        model.end_coords = self.head_end_coords
+        return model