diff --git a/examples/batch_ik_demo.py b/examples/batch_ik_demo.py index 035612b6..46c41088 100755 --- a/examples/batch_ik_demo.py +++ b/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 = [ diff --git a/examples/robot_models.py b/examples/robot_models.py index 844a99ba..6b7d5cf0 100755 --- a/examples/robot_models.py +++ b/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(), ] diff --git a/skrobot/data/__init__.py b/skrobot/data/__init__.py index 82593135..e3709cb3 100644 --- a/skrobot/data/__init__.py +++ b/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') diff --git a/skrobot/interfaces/ros/__init__.py b/skrobot/interfaces/ros/__init__.py index 475932a4..e9072c10 100644 --- a/skrobot/interfaces/ros/__init__.py +++ b/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"] diff --git a/skrobot/interfaces/ros/nextage.py b/skrobot/interfaces/ros/nextage.py new file mode 100644 index 00000000..1581faf2 --- /dev/null +++ b/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) diff --git a/skrobot/models/__init__.py b/skrobot/models/__init__.py index 35fb1589..aaedbf90 100644 --- a/skrobot/models/__init__.py +++ b/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 diff --git a/skrobot/models/nextage.py b/skrobot/models/nextage.py new file mode 100644 index 00000000..283a988c --- /dev/null +++ b/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