From 7958ca44fafdbc363e5e9d897a25d07211d2a331 Mon Sep 17 00:00:00 2001 From: Michi-Tsubaki Date: Fri, 24 Oct 2025 00:03:46 +0900 Subject: [PATCH 1/8] Fix tile shape --- examples/robot_models.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/robot_models.py b/examples/robot_models.py index c0a15748..844a99ba 100755 --- a/examples/robot_models.py +++ b/examples/robot_models.py @@ -49,7 +49,7 @@ def main(): for i in range(nrow): for j in range(ncol): try: - robot = robots[i * nrow + j] + robot = robots[i * ncol + j] except IndexError: break plane = skrobot.model.Box(extents=(row - 0.01, col - 0.01, 0.01)) From 05d13b60eb999c439977fc7912559c22dc6eb5df Mon Sep 17 00:00:00 2001 From: Michi-Tsubaki Date: Thu, 23 Oct 2025 23:59:15 +0900 Subject: [PATCH 2/8] Add nextage robot model and interface --- skrobot/data/__init__.py | 14 ++++++ skrobot/interfaces/ros/__init__.py | 5 +- skrobot/interfaces/ros/nextage.py | 59 ++++++++++++++++++++++ skrobot/models/__init__.py | 1 + skrobot/models/nextage.py | 80 ++++++++++++++++++++++++++++++ 5 files changed, 157 insertions(+), 2 deletions(-) create mode 100644 skrobot/interfaces/ros/nextage.py create mode 100644 skrobot/models/nextage.py diff --git a/skrobot/data/__init__.py b/skrobot/data/__init__.py index 82593135..e24df0f7 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/Michi-Tsubaki/scikit-robot-models/raw/refs/heads/add-nextage/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..afb1d526 100644 --- a/skrobot/interfaces/ros/__init__.py +++ b/skrobot/interfaces/ros/__init__.py @@ -1,9 +1,10 @@ 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..97565809 --- /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..c75b8506 --- /dev/null +++ b/skrobot/models/nextage.py @@ -0,0 +1,80 @@ +from cached_property import cached_property + +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) + self.reset_pose() + + @cached_property + def default_urdf_path(self): + return nextage_urdfpath() + + def reset_pose(self): + import numpy as np + angle_vector = [ + 0.0, + -0.5, 0.0, np.deg2rad(-100), np.deg2rad(15.2), np.deg2rad(9.4), np.deg2rad(3.2), + 0.5, 0.0, np.deg2rad(-100), np.deg2rad(-15.2), np.deg2rad(9.4), np.deg2rad(-3.2), + 0.0, 0.0 + ] + 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 + + @cached_property + def rarm_end_coords(self): + return self.RARM_JOINT5_Link + + @cached_property + def larm_end_coords(self): + return self.LARM_JOINT5_Link + + @cached_property + def head_end_coords(self): + return self.HEAD_JOINT1_Link From 72dbacb5f25a20b4a7234d4168c420f124d2d893 Mon Sep 17 00:00:00 2001 From: Michi-Tsubaki Date: Fri, 24 Oct 2025 00:11:26 +0900 Subject: [PATCH 3/8] Add nextage examples --- examples/batch_ik_demo.py | 15 ++++++++++++++- examples/robot_models.py | 1 + 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/examples/batch_ik_demo.py b/examples/batch_ik_demo.py index 0c5111b2..2888817a 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', @@ -80,6 +81,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() @@ -98,6 +102,15 @@ 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.2, -0.3, 0.0)).rotate(np.deg2rad(-25), 'z'), + Coordinates(pos=(0.2, -0.3, -0.05)).rotate(np.deg2rad(-30), 'z'), + Coordinates(pos=(0.35, -0.25, 0.05)).rotate(np.deg2rad(35), 'y').rotate(np.deg2rad(-20), 'z'), + Coordinates(pos=(0.1, -0.1, 0.1)).rotate(np.deg2rad(90), 'x'), + Coordinates(pos=(0.2, -0.1, 0.03)).rotate(np.deg2rad(45), 'x'), + Coordinates(pos=(0.35, -0.15, 0.1)).rotate(np.deg2rad(-80), '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(), ] From 67d4334300727713aea95705478619807f6ade1c Mon Sep 17 00:00:00 2001 From: Michi-Tsubaki Date: Fri, 24 Oct 2025 10:33:09 +0900 Subject: [PATCH 4/8] Fix the url for nextage_description --- skrobot/data/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/skrobot/data/__init__.py b/skrobot/data/__init__.py index e24df0f7..e3709cb3 100644 --- a/skrobot/data/__init__.py +++ b/skrobot/data/__init__.py @@ -88,7 +88,7 @@ def nextage_urdfpath(): if osp.exists(path): return path _download( - url='https://github.com/Michi-Tsubaki/scikit-robot-models/raw/refs/heads/add-nextage/nextage_description.tar.gz', # NOQA + 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', From b28edd65341d38a0ec2902b9dd6eecfbc9806639 Mon Sep 17 00:00:00 2001 From: Michi-Tsubaki Date: Fri, 24 Oct 2025 10:37:10 +0900 Subject: [PATCH 5/8] Fix ruff linting errors --- skrobot/interfaces/ros/__init__.py | 1 + skrobot/interfaces/ros/nextage.py | 2 +- skrobot/models/nextage.py | 12 ++++++------ 3 files changed, 8 insertions(+), 7 deletions(-) diff --git a/skrobot/interfaces/ros/__init__.py b/skrobot/interfaces/ros/__init__.py index afb1d526..e9072c10 100644 --- a/skrobot/interfaces/ros/__init__.py +++ b/skrobot/interfaces/ros/__init__.py @@ -1,5 +1,6 @@ from skrobot._lazy_imports import LazyImportClass + NextageROSRobotInterface = LazyImportClass( ".nextage", "NextageROSRobotInterface", "skrobot.interfaces.ros") PandaROSRobotInterface = LazyImportClass( diff --git a/skrobot/interfaces/ros/nextage.py b/skrobot/interfaces/ros/nextage.py index 97565809..1581faf2 100644 --- a/skrobot/interfaces/ros/nextage.py +++ b/skrobot/interfaces/ros/nextage.py @@ -8,7 +8,7 @@ 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 diff --git a/skrobot/models/nextage.py b/skrobot/models/nextage.py index c75b8506..48837452 100644 --- a/skrobot/models/nextage.py +++ b/skrobot/models/nextage.py @@ -28,7 +28,7 @@ def default_urdf_path(self): def reset_pose(self): import numpy as np angle_vector = [ - 0.0, + 0.0, -0.5, 0.0, np.deg2rad(-100), np.deg2rad(15.2), np.deg2rad(9.4), np.deg2rad(3.2), 0.5, 0.0, np.deg2rad(-100), np.deg2rad(-15.2), np.deg2rad(9.4), np.deg2rad(-3.2), 0.0, 0.0 @@ -48,7 +48,7 @@ def rarm(self): 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)] @@ -57,7 +57,7 @@ def larm(self): 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)] @@ -66,15 +66,15 @@ def head(self): model = RobotModel(link_list=links, joint_list=joints) model.end_coords = self.head_end_coords return model - + @cached_property def rarm_end_coords(self): return self.RARM_JOINT5_Link - + @cached_property def larm_end_coords(self): return self.LARM_JOINT5_Link - + @cached_property def head_end_coords(self): return self.HEAD_JOINT1_Link From e6c46730b4ac53cf296db11d5ba3ac238f540741 Mon Sep 17 00:00:00 2001 From: Michi-Tsubaki Date: Sat, 25 Oct 2025 10:47:17 +0900 Subject: [PATCH 6/8] Fix endcoords and reset_pose --- skrobot/models/nextage.py | 41 +++++++++++++++++++++++++++++---------- 1 file changed, 31 insertions(+), 10 deletions(-) diff --git a/skrobot/models/nextage.py b/skrobot/models/nextage.py index 48837452..ae5dc5f8 100644 --- a/skrobot/models/nextage.py +++ b/skrobot/models/nextage.py @@ -1,12 +1,13 @@ from cached_property import cached_property +import numpy as np +from ..coordinates import Coordinates from ..data import nextage_urdfpath from ..model import RobotModel from .urdf import RobotModelFromURDF class Nextage(RobotModelFromURDF): - """ - Nextage Open Official Information. @@ -26,12 +27,22 @@ def default_urdf_path(self): return nextage_urdfpath() def reset_pose(self): - import numpy as np angle_vector = [ 0.0, - -0.5, 0.0, np.deg2rad(-100), np.deg2rad(15.2), np.deg2rad(9.4), np.deg2rad(3.2), - 0.5, 0.0, np.deg2rad(-100), np.deg2rad(-15.2), np.deg2rad(9.4), np.deg2rad(-3.2), - 0.0, 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() @@ -42,7 +53,7 @@ def reset_manip_pose(self): @cached_property def rarm(self): - link_names = ['RARM_JOINT{}_Link'.format(i) for i in range(6)] + 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) @@ -51,7 +62,7 @@ def rarm(self): @cached_property def larm(self): - link_names = ['LARM_JOINT{}_Link'.format(i) for i in range(6)] + 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) @@ -60,7 +71,7 @@ def larm(self): @cached_property def head(self): - link_names = ['HEAD_JOINT{}_Link'.format(i) for i in range(2)] + 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) @@ -69,11 +80,21 @@ def head(self): @cached_property def rarm_end_coords(self): - return self.RARM_JOINT5_Link + end_coords = Coordinates() + self.RARM_JOINT5_Link.assoc(end_coords) + end_coords.newcoords(self.RARM_JOINT5_Link.copy_worldcoords()) + end_coords.translate([-0.185, 0.0, -0.01]) + end_coords.rotate(-np.pi / 2.0, 'y') + return end_coords @cached_property def larm_end_coords(self): - return self.LARM_JOINT5_Link + end_coords = Coordinates() + self.LARM_JOINT5_Link.assoc(end_coords) + end_coords.newcoords(self.LARM_JOINT5_Link.copy_worldcoords()) + end_coords.translate([-0.185, 0.0, -0.01]) + end_coords.rotate(-np.pi / 2.0, 'y') + return end_coords @cached_property def head_end_coords(self): From 43f523de33df60b55c1430383c286d754968ffd3 Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Sat, 25 Oct 2025 13:20:12 +0900 Subject: [PATCH 7/8] Fixed end coords and batch_ik_demo's target --- examples/batch_ik_demo.py | 12 +++++------ skrobot/models/nextage.py | 43 +++++++++++++++++++-------------------- 2 files changed, 27 insertions(+), 28 deletions(-) diff --git a/examples/batch_ik_demo.py b/examples/batch_ik_demo.py index f9c5ec48..256c85ec 100755 --- a/examples/batch_ik_demo.py +++ b/examples/batch_ik_demo.py @@ -108,12 +108,12 @@ def main(): ] elif args.robot == 'nextage': target_poses = [ - Coordinates(pos=(0.2, -0.3, 0.0)).rotate(np.deg2rad(-25), 'z'), - Coordinates(pos=(0.2, -0.3, -0.05)).rotate(np.deg2rad(-30), 'z'), - Coordinates(pos=(0.35, -0.25, 0.05)).rotate(np.deg2rad(35), 'y').rotate(np.deg2rad(-20), 'z'), - Coordinates(pos=(0.1, -0.1, 0.1)).rotate(np.deg2rad(90), 'x'), - Coordinates(pos=(0.2, -0.1, 0.03)).rotate(np.deg2rad(45), 'x'), - Coordinates(pos=(0.35, -0.15, 0.1)).rotate(np.deg2rad(-80), 'y').rotate(np.deg2rad(15), 'z'), + 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 diff --git a/skrobot/models/nextage.py b/skrobot/models/nextage.py index ae5dc5f8..ad99b320 100644 --- a/skrobot/models/nextage.py +++ b/skrobot/models/nextage.py @@ -1,7 +1,7 @@ from cached_property import cached_property import numpy as np -from ..coordinates import Coordinates +from ..coordinates import CascadedCoords from ..data import nextage_urdfpath from ..model import RobotModel from .urdf import RobotModelFromURDF @@ -20,6 +20,26 @@ class Nextage(RobotModelFromURDF): 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 @@ -78,24 +98,3 @@ def head(self): model.end_coords = self.head_end_coords return model - @cached_property - def rarm_end_coords(self): - end_coords = Coordinates() - self.RARM_JOINT5_Link.assoc(end_coords) - end_coords.newcoords(self.RARM_JOINT5_Link.copy_worldcoords()) - end_coords.translate([-0.185, 0.0, -0.01]) - end_coords.rotate(-np.pi / 2.0, 'y') - return end_coords - - @cached_property - def larm_end_coords(self): - end_coords = Coordinates() - self.LARM_JOINT5_Link.assoc(end_coords) - end_coords.newcoords(self.LARM_JOINT5_Link.copy_worldcoords()) - end_coords.translate([-0.185, 0.0, -0.01]) - end_coords.rotate(-np.pi / 2.0, 'y') - return end_coords - - @cached_property - def head_end_coords(self): - return self.HEAD_JOINT1_Link From b661c14668681edb6814d835550a626fdd131975 Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Sat, 25 Oct 2025 13:28:52 +0900 Subject: [PATCH 8/8] Fix batch_ik_demo for Nextage robot - Adjust target poses to match Nextage end effector workspace - Add base rotation (180deg around y-axis) to match end_coords orientation - Split long lines to comply with line length limit --- examples/batch_ik_demo.py | 20 ++++++++++++++------ skrobot/models/nextage.py | 1 - 2 files changed, 14 insertions(+), 7 deletions(-) diff --git a/examples/batch_ik_demo.py b/examples/batch_ik_demo.py index 256c85ec..46c41088 100755 --- a/examples/batch_ik_demo.py +++ b/examples/batch_ik_demo.py @@ -108,12 +108,20 @@ def main(): ] 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'), + 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 diff --git a/skrobot/models/nextage.py b/skrobot/models/nextage.py index ad99b320..283a988c 100644 --- a/skrobot/models/nextage.py +++ b/skrobot/models/nextage.py @@ -97,4 +97,3 @@ def head(self): model = RobotModel(link_list=links, joint_list=joints) model.end_coords = self.head_end_coords return model -