From e3133eff077c229f57198480b2db87aa54bdbb9a Mon Sep 17 00:00:00 2001 From: Yuya Nagamatsu Date: Fri, 17 Dec 2021 17:42:31 +0900 Subject: [PATCH 01/33] [hrpsys_ros_bridge_tutorials/A0B] add A0B interface --- hrpsys_ros_bridge_tutorials/CMakeLists.txt | 9 +++++++ .../euslisp/a0b-interface.l | 15 +++++++++++ hrpsys_ros_bridge_tutorials/models/a0b.yaml | 25 +++++++++++++++++++ .../a0b_hrpsys_config.py | 12 +++++++++ .../urata_hrpsys_config.py | 13 ++++++++++ 5 files changed, 74 insertions(+) create mode 100644 hrpsys_ros_bridge_tutorials/euslisp/a0b-interface.l create mode 100644 hrpsys_ros_bridge_tutorials/models/a0b.yaml create mode 100755 hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/a0b_hrpsys_config.py diff --git a/hrpsys_ros_bridge_tutorials/CMakeLists.txt b/hrpsys_ros_bridge_tutorials/CMakeLists.txt index 5e5d8a00..a1e5b88f 100644 --- a/hrpsys_ros_bridge_tutorials/CMakeLists.txt +++ b/hrpsys_ros_bridge_tutorials/CMakeLists.txt @@ -444,6 +444,14 @@ compile_rbrain_model_for_closed_robots(TQLEG0 TQLEG0 TQLEG0 --conf-file-option "seq_optional_data_dim: 4" ) +# A0B +compile_rbrain_model_for_closed_robots(A0B A0B A0B + --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav" + --conf-file-option "end_effectors: rarm,RARM_JOINT5,WAIST,0.25375,0.0,0.0,0.0,0.0,0.0,0.0," + --conf-dt-option "0.002" + --simulation-timestep-option "0.002" + ) + if(EXISTS ${PROJECT_SOURCE_DIR}/models/TESTMDOFARM.wrl) compile_openhrp_model( ${PROJECT_SOURCE_DIR}/models/TESTMDOFARM.wrl @@ -502,6 +510,7 @@ generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(URATALEG generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(YSTLEG YSTLEG "--use-robot-hrpsys-config") generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(CHIDORI CHIDORI "--use-robot-hrpsys-config") generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(TQLEG0 TQLEG0 "--use-robot-hrpsys-config") +generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(A0B A0B "--use-robot-hrpsys-config") generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(TABLIS TABLIS "--use-robot-hrpsys-config") generate_default_launch_eusinterface_files( "$(find hrpsys_ros_bridge_tutorials)/models/TESTMDOFARM.wrl" diff --git a/hrpsys_ros_bridge_tutorials/euslisp/a0b-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/a0b-interface.l new file mode 100644 index 00000000..9a11ae89 --- /dev/null +++ b/hrpsys_ros_bridge_tutorials/euslisp/a0b-interface.l @@ -0,0 +1,15 @@ +(load "package://hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l") +(require :tqleg0 "package://hrpsys_ros_bridge_tutorials/models/a0b.l") + +(defclass a0b-interface + :super rtm-ros-robot-interface + :slots ()) +(defmethod a0b-interface + (:init (&rest args &key ((:controller-timeout ct) nil)) + (send-super* :init :robot a0b-robot :controller-timeout ct args))) + +(defun a0b-init (&rest args) + (if (not (boundp '*ri*)) + (setq *ri* (instance* a0b-interface :init args))) + (if (not (boundp '*a0b*)) + (setq *a0b* (instance a0b-robot :init)))) diff --git a/hrpsys_ros_bridge_tutorials/models/a0b.yaml b/hrpsys_ros_bridge_tutorials/models/a0b.yaml new file mode 100644 index 00000000..ca3e2c9f --- /dev/null +++ b/hrpsys_ros_bridge_tutorials/models/a0b.yaml @@ -0,0 +1,25 @@ +## +## - collada_joint_name : euslisp_joint_name (start with :) +## + +rarm: + - RARM_JOINT0 : rarm-joint0 + - RARM_JOINT1 : rarm-joint1 + - RARM_JOINT2 : rarm-joint2 + - RARM_JOINT3 : rarm-joint3 + - RARM_JOINT4 : rarm-joint4 + - RARM_JOINT5 : rarm-joint5 + +## +## end-coords +## +rarm-end-coords: + translate : [0.25375, 0, 0] + +## +## reset-pose +## +angle-vector: + zero-pose : [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + reset-pose : [60.0, -90.0, 90.0, -30.0, -90.0, 90.0] + reverse-reset-pose : [60.0, 90.0, -90.0, -30.0, 90.0, -90.0] diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/a0b_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/a0b_hrpsys_config.py new file mode 100755 index 00000000..fe9534ec --- /dev/null +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/a0b_hrpsys_config.py @@ -0,0 +1,12 @@ +#!/usr/bin/env python + +from urata_hrpsys_config import * + +if __name__ == '__main__': + hcf = URATAHrpsysConfigurator("A0B") + if len(sys.argv) > 2 : + hcf.init(sys.argv[1], sys.argv[2]) + elif len(sys.argv) > 1 : + hcf.init(sys.argv[1]) + else : + hcf.init() diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py index 91eb4d0f..6ff87cbf 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py @@ -56,6 +56,9 @@ def defJointGroups (self): head_group = ['head', ['HEAD_JOINT0', 'HEAD_JOINT1']] torso_group = ['torso', ['CHEST_JOINT0', 'CHEST_JOINT1', 'CHEST_JOINT2']] self.Groups = [rarm_group, larm_group, rleg_group, lleg_group, head_group, torso_group] + elif self.ROBOT_NAME == "A0B": + rarm_group = ['rarm', ['RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5']] + self.Groups = [rarm_group] else: rarm_group = ['rarm', ['RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5', 'RARM_JOINT6', 'RARM_JOINT7']] larm_group = ['larm', ['LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'LARM_JOINT6', 'LARM_JOINT7']] @@ -804,6 +807,12 @@ def tablisLoadTestPose (self): def tablisInitPose (self): return [0]*len(self.tablisResetPose()) + def a0bResetPose(self): + return [0, 0, 0, 0, 0, 0] + + def a0bInitPose (self): + return [0]*len(self.tqleg0ResetPose()) + # (mapcar #'deg2rad (concatenate cons (send *robot* :reset-landing-pose))) def jaxonResetLandingPose (self): return [0.004318,0.005074,-0.134838,1.18092,-0.803855,-0.001463,0.004313,0.005079,-0.133569,1.18206,-0.806262,-0.001469,0.003782,-0.034907,0.004684,0.0,0.0,0.0,0.698132,-0.349066,-0.087266,-1.39626,0.0,0.0,-0.349066,0.0,0.698132,0.349066,0.087266,-1.39626,0.0,0.0,-0.349066] @@ -830,6 +839,8 @@ def setResetPose(self): self.seq_svc.setJointAngles(self.tqleg0ResetPose(), 5.0) elif self.ROBOT_NAME == "TABLIS": self.seq_svc.setJointAngles(self.tablisResetPose(), 5.0) + elif self.ROBOT_NAME == "A0B": + self.seq_svc.setJointAngles(self.a0bResetPose(), 5.0) def setLoadTestPose(self): if self.ROBOT_NAME == "TABLIS": @@ -858,6 +869,8 @@ def setInitPose(self): self.seq_svc.setJointAngles(self.tqleg0InitPose(), 10.0) elif self.ROBOT_NAME == "TABLIS": self.seq_svc.setJointAngles(self.tablisInitPose(), 10.0) + elif self.ROBOT_NAME == "A0B": + self.seq_svc.setJointAngles(self.a0bInitPose(), 10.0) def setCollisionFreeInitPose(self): if self.ROBOT_NAME == "JAXON_BLUE": From 9693ff4bc94ec59b4326f34aa9a1b0acf1949833 Mon Sep 17 00:00:00 2001 From: wu Date: Wed, 5 Apr 2023 11:18:34 +0900 Subject: [PATCH 02/33] change name :tqleg0 to :a0b --- hrpsys_ros_bridge_tutorials/euslisp/a0b-interface.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hrpsys_ros_bridge_tutorials/euslisp/a0b-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/a0b-interface.l index 9a11ae89..d42f2650 100644 --- a/hrpsys_ros_bridge_tutorials/euslisp/a0b-interface.l +++ b/hrpsys_ros_bridge_tutorials/euslisp/a0b-interface.l @@ -1,5 +1,5 @@ (load "package://hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l") -(require :tqleg0 "package://hrpsys_ros_bridge_tutorials/models/a0b.l") +(require :a0b "package://hrpsys_ros_bridge_tutorials/models/a0b.l") (defclass a0b-interface :super rtm-ros-robot-interface From 3edcc9ee0fd1e61bf9a065746335b3ee9b0436b4 Mon Sep 17 00:00:00 2001 From: wu Date: Wed, 5 Apr 2023 11:20:10 +0900 Subject: [PATCH 03/33] change a0bResetPose list based on a0b.yaml, set a0bInitPose based on a0bResetPose length --- .../src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py index 6ff87cbf..a64c7eb8 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py @@ -808,10 +808,10 @@ def tablisInitPose (self): return [0]*len(self.tablisResetPose()) def a0bResetPose(self): - return [0, 0, 0, 0, 0, 0] + return [math.radians(60), math.radians(-90), math.radians(90), math.radians(-30), math.radians(-90), math.radians(90)] def a0bInitPose (self): - return [0]*len(self.tqleg0ResetPose()) + return [0]*len(self.a0bResetPose()) # (mapcar #'deg2rad (concatenate cons (send *robot* :reset-landing-pose))) def jaxonResetLandingPose (self): From 47ce3716cb8473f4af8944dfa67c5dbdb9765fe8 Mon Sep 17 00:00:00 2001 From: kirohy Date: Fri, 5 Apr 2024 16:11:48 +0900 Subject: [PATCH 04/33] [hrpsys_ros_bridge_tutorials] fix for building models with xacro in Noetic --- hrpsys_ros_bridge_tutorials/CMakeLists.txt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/hrpsys_ros_bridge_tutorials/CMakeLists.txt b/hrpsys_ros_bridge_tutorials/CMakeLists.txt index a1e5b88f..594a5e3e 100644 --- a/hrpsys_ros_bridge_tutorials/CMakeLists.txt +++ b/hrpsys_ros_bridge_tutorials/CMakeLists.txt @@ -530,8 +530,10 @@ generate_default_launch_eusinterface_files("$(find openhrp3)/share/OpenHRP-3.1/s find_package(xacro) if(EXISTS ${xacro_SOURCE_PREFIX}/xacro.py) set(xacro_exe ${xacro_SOURCE_PREFIX}/xacro.py) -else() +elseif(EXISTS ${xacro_PREFIX}/share/xacro/xacro.py) set(xacro_exe ${xacro_PREFIX}/share/xacro/xacro.py) +else() + set(xacro_exe ${xacro_PREFIX}/bin/xacro) # xacro.py is deprecated in Noetic endif() find_package(euscollada) From 19606b31ec54e1e203fd2ac7f50813747222b244 Mon Sep 17 00:00:00 2001 From: kirohy Date: Fri, 5 Apr 2024 23:46:21 +0900 Subject: [PATCH 05/33] [hrpsys_ros_bridge_tutorials] apply 2to3 -f print --- .../scripts/add-obj-to-hrpproject.py | 6 +++--- .../scripts/publish_end_effector_tf.py | 4 ++-- .../src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py | 4 ++-- .../samplerobot_hrpsys_config.py | 2 +- .../src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 4 ++-- 5 files changed, 10 insertions(+), 10 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/scripts/add-obj-to-hrpproject.py b/hrpsys_ros_bridge_tutorials/scripts/add-obj-to-hrpproject.py index d01fd43f..ca928472 100755 --- a/hrpsys_ros_bridge_tutorials/scripts/add-obj-to-hrpproject.py +++ b/hrpsys_ros_bridge_tutorials/scripts/add-obj-to-hrpproject.py @@ -118,9 +118,9 @@ def add_object_to_projectfile(objname, url, robot=False, add_collision=True, tra argc = len(argvs) if argc < 4: - print '### usage : .xml .xml objname,url,is_robot_q,add_collision_q,0,0,0,0,0,0,0' - print '### example : sample_in.xml sample_out.xml table,url_of_table,True,True,0.0,0.0,1.0,0.0,0.0,1.0,90.0' - print '### string,string ,bool,bool,trans<3> ,rotation<4>' + print('### usage : .xml .xml objname,url,is_robot_q,add_collision_q,0,0,0,0,0,0,0') + print('### example : sample_in.xml sample_out.xml table,url_of_table,True,True,0.0,0.0,1.0,0.0,0.0,1.0,90.0') + print('### string,string ,bool,bool,trans<3> ,rotation<4>') exit(0) infile = argvs[1] diff --git a/hrpsys_ros_bridge_tutorials/scripts/publish_end_effector_tf.py b/hrpsys_ros_bridge_tutorials/scripts/publish_end_effector_tf.py index f66c1448..4e0851c1 100755 --- a/hrpsys_ros_bridge_tutorials/scripts/publish_end_effector_tf.py +++ b/hrpsys_ros_bridge_tutorials/scripts/publish_end_effector_tf.py @@ -50,9 +50,9 @@ def publishEndEffectorAll(): if not g_eef_info_list[limb].has_key('parent'): g_eef_info_list[limb]['parent'] = parent_link_name # print for debug - print 'eef_infos param list:' + print('eef_infos param list:') for limb, eef_info in g_eef_info_list.items(): - print '%s: %s' % (limb, eef_info) + print('%s: %s' % (limb, eef_info)) while not rospy.is_shutdown(): publishEndEffectorAll() diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py index 3bf0d267..519fafcf 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py @@ -14,7 +14,7 @@ def getRTCList (self): return self.getRTCListUnstable() def init (self, robotname="Robot", url=""): HrpsysConfigurator.init(self, robotname, url) - print "initialize rtc parameters" + print("initialize rtc parameters") self.setStAbcParameters() self.loadForceMomentOffsetFile() @@ -380,7 +380,7 @@ def loadForceMomentOffsetFile (self): elif self.ROBOT_NAME == "HRP2JSK": self.rmfo_svc.loadForceMomentOffsetParams(rospkg.RosPack().get_path('hrpsys_ros_bridge_tutorials')+"/models/hand_force_calib_offset_thumb_60deg_HRP2JSK") else: - print "No force moment offset file" + print("No force moment offset file") def __init__(self, robotname=""): self.ROBOT_NAME = robotname diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/samplerobot_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/samplerobot_hrpsys_config.py index 96a19b26..e31b3d70 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/samplerobot_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/samplerobot_hrpsys_config.py @@ -12,7 +12,7 @@ def getRTCList (self): return self.getRTCListUnstable() def init (self, robotname="SampleRobot", url=""): HrpsysConfigurator.init(self, robotname, url) - print "initialize rtc parameters" + print("initialize rtc parameters") self.setStAbcParameters() def defJointGroups (self): diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py index a64c7eb8..2e2a4dc8 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py @@ -481,7 +481,7 @@ def setStAbcParametersURATALEG (self): self.st_svc.setParameter(stp) def setStAbcParametersYSTLEG (self): - print "Not implemented yet" + print("Not implemented yet") def setStAbcParametersCHIDORI (self): # abc setting @@ -903,4 +903,4 @@ def loadForceMomentOffsetFile (self): elif self.ROBOT_NAME == "JAXON_RED": self.rmfo_svc.loadForceMomentOffsetParams(rospkg.RosPack().get_path('hrpsys_ros_bridge_tutorials')+"/models/hand_force_calib_offset_JAXON_RED") else: - print "No force moment offset file" + print("No force moment offset file") From ed414b28db52a202d46910f7ae3753009071435f Mon Sep 17 00:00:00 2001 From: kirohy Date: Fri, 5 Apr 2024 23:47:15 +0900 Subject: [PATCH 06/33] [hrpsys_ros_bridge_tutorials] apply 2to3 -f map --- .../hrp2_hrpsys_config.py | 6 +++--- .../urata_hrpsys_config.py | 14 +++++++------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py index 519fafcf..16816e77 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py @@ -122,7 +122,7 @@ def setStAbcParametershrp2017c(self): OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])] rarm_vertices = rleg_vertices larm_vertices = lleg_vertices - stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices]) + stp.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices]] # tpcc st params stp.k_tpcc_p=[2.0, 2.0] stp.k_tpcc_x=[5.0, 5.0] @@ -219,7 +219,7 @@ def setStAbcParametershrp2016c (self): OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])] rarm_vertices = rleg_vertices larm_vertices = lleg_vertices - stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices]) + stp.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices]] # tpcc st params stp.k_tpcc_p=[2.0, 2.0] stp.k_tpcc_x=[5.0, 5.0] @@ -316,7 +316,7 @@ def setStAbcParametershrp2007c (self): OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])] rarm_vertices = rleg_vertices larm_vertices = lleg_vertices - stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices]) + stp.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices]] # tpcc st params stp.k_tpcc_p=[2.0, 2.0] stp.k_tpcc_x=[5.0, 5.0] diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py index 2e2a4dc8..4114a66f 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py @@ -144,7 +144,7 @@ def setStAbcParametersSTARO (self): OpenHRP.StabilizerService.TwoDimensionVertex(pos=[stp.eefm_leg_front_margin, -1*stp.eefm_leg_inside_margin]), OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, -1*stp.eefm_leg_inside_margin]), OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, stp.eefm_leg_outside_margin])] - stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices]) + stp.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [rleg_vertices, lleg_vertices]] stp.eefm_zmp_delay_time_const=[0.055, 0.055] #stp.eefm_cogvel_cutoff_freq = 3.18 #stp.eefm_cogvel_cutoff_freq = 6.18 # servooff+walk @@ -280,7 +280,7 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, stp.eefm_leg_outside_margin])] rarm_vertices = rleg_vertices larm_vertices = lleg_vertices - stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices]) + stp.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices]] stp.eefm_cogvel_cutoff_freq = 4.0 stp.eefm_k1=[-1.48412,-1.48412] stp.eefm_k2=[-0.486727,-0.486727] @@ -385,7 +385,7 @@ def setStAbcIcParametersJAXON_BLUE(self, foot="KAWADA"): OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, stp.eefm_leg_outside_margin])] rarm_vertices = rleg_vertices larm_vertices = lleg_vertices - stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices]) + stp.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices]] stp.eefm_cogvel_cutoff_freq = 4.0 # for only leg stp.eefm_k1=[-1.36334,-1.36334] @@ -472,7 +472,7 @@ def setStAbcParametersURATALEG (self): OpenHRP.StabilizerService.TwoDimensionVertex(pos=[stp.eefm_leg_front_margin, -1*stp.eefm_leg_inside_margin]), OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, -1*stp.eefm_leg_inside_margin]), OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, stp.eefm_leg_outside_margin])] - stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices]) + stp.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [rleg_vertices, lleg_vertices]] stp.eefm_zmp_delay_time_const=[0.055, 0.055] stp.eefm_cogvel_cutoff_freq = 4.25 stp.eefm_k1=[-1.38077, -1.38077] @@ -545,7 +545,7 @@ def setStAbcParametersCHIDORI (self): OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_inside_margin]), OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_inside_margin]), OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])] - stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices]) + stp.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [rleg_vertices, lleg_vertices]] # stp.eefm_zmp_delay_time_const=[0.055, 0.055] stp.eefm_cogvel_cutoff_freq = 4.0 stp.eefm_k1=[-1.48412,-1.48412] @@ -624,7 +624,7 @@ def setStAbcParametersTQLEG0 (self): OpenHRP.StabilizerService.TwoDimensionVertex(pos=[stp.eefm_leg_front_margin, -1*stp.eefm_leg_inside_margin]), OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, -1*stp.eefm_leg_inside_margin]), OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, stp.eefm_leg_outside_margin])] - stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices]) + stp.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [rleg_vertices, lleg_vertices]] stp.eefm_zmp_delay_time_const=[0.055, 0.055] stp.eefm_cogvel_cutoff_freq = 4.25 stp.eefm_k1=[-1.38077, -1.38077] @@ -688,7 +688,7 @@ def setStAbcParametersTABLIS(self): OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])] rarm_vertices = rleg_vertices larm_vertices = lleg_vertices - stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices]) + stp.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices]] # stp.eefm_zmp_delay_time_const=[0.055, 0.055] stp.eefm_cogvel_cutoff_freq = 4.0 # calculated by calculate-eefm-st-state-feedback-default-gain-from-robot *chidori* From 9895ef0604188b0e29cdbf7b527eccb450e89168 Mon Sep 17 00:00:00 2001 From: kirohy Date: Fri, 5 Apr 2024 23:47:31 +0900 Subject: [PATCH 07/33] [hrpsys_ros_bridge_tutorials] apply 2to3 -f dict --- hrpsys_ros_bridge_tutorials/scripts/add-obj-to-hrpproject.py | 2 +- .../scripts/publish_end_effector_tf.py | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/scripts/add-obj-to-hrpproject.py b/hrpsys_ros_bridge_tutorials/scripts/add-obj-to-hrpproject.py index ca928472..b94e2bf8 100755 --- a/hrpsys_ros_bridge_tutorials/scripts/add-obj-to-hrpproject.py +++ b/hrpsys_ros_bridge_tutorials/scripts/add-obj-to-hrpproject.py @@ -22,7 +22,7 @@ def fixed_writexml(self, writer, indent="", addindent="", newl=""): writer.write(indent+"<" + self.tagName) attrs = self._get_attributes() - a_names = attrs.keys() + a_names = list(attrs.keys()) a_names.sort() for a_name in a_names: diff --git a/hrpsys_ros_bridge_tutorials/scripts/publish_end_effector_tf.py b/hrpsys_ros_bridge_tutorials/scripts/publish_end_effector_tf.py index 4e0851c1..9cf027ef 100755 --- a/hrpsys_ros_bridge_tutorials/scripts/publish_end_effector_tf.py +++ b/hrpsys_ros_bridge_tutorials/scripts/publish_end_effector_tf.py @@ -13,7 +13,7 @@ def publishEndEffectorOne(eef_name, eef_info, stamp): stamp, eef_name, eef_info['parent']) def publishEndEffectorAll(): - for limb_name, eef_info in g_eef_info_list.items(): + for limb_name, eef_info in list(g_eef_info_list.items()): publishEndEffectorOne(limb_name+"_end_coords", eef_info, rospy.Time.now()) if __name__ == "__main__": @@ -51,7 +51,7 @@ def publishEndEffectorAll(): g_eef_info_list[limb]['parent'] = parent_link_name # print for debug print('eef_infos param list:') - for limb, eef_info in g_eef_info_list.items(): + for limb, eef_info in list(g_eef_info_list.items()): print('%s: %s' % (limb, eef_info)) while not rospy.is_shutdown(): From c3999c0b7e993c237bd908ad591a387650ccdf10 Mon Sep 17 00:00:00 2001 From: kirohy Date: Fri, 5 Apr 2024 23:47:50 +0900 Subject: [PATCH 08/33] [hrpsys_ros_bridge_tutorials] apply 2to3 -f has_key --- .../scripts/publish_end_effector_tf.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/scripts/publish_end_effector_tf.py b/hrpsys_ros_bridge_tutorials/scripts/publish_end_effector_tf.py index 9cf027ef..9298238a 100755 --- a/hrpsys_ros_bridge_tutorials/scripts/publish_end_effector_tf.py +++ b/hrpsys_ros_bridge_tutorials/scripts/publish_end_effector_tf.py @@ -33,7 +33,7 @@ def publishEndEffectorAll(): parent_link_name_next = parent_link_name while True: parent_link_name = parent_link_name_next - if robot_model.child_map.has_key(parent_link_name): + if parent_link_name in robot_model.child_map: parent_link_name_next = robot_model.child_map[parent_link_name][0][1] if not (parent_link_name_next.startswith(limb.upper()) or parent_link_name_next.startswith(limb.lower())): break @@ -43,11 +43,11 @@ def publishEndEffectorAll(): param_name = '~'+limb+'-end-coords' if rospy.has_param(param_name): g_eef_info_list[limb] = rospy.get_param(param_name) - if not g_eef_info_list[limb].has_key('translate'): + if 'translate' not in g_eef_info_list[limb]: g_eef_info_list[limb]['translate'] = [0, 0, 0, 0] - if not g_eef_info_list[limb].has_key('rotate'): + if 'rotate' not in g_eef_info_list[limb]: g_eef_info_list[limb]['rotate'] = [0, 0, 0, 0] - if not g_eef_info_list[limb].has_key('parent'): + if 'parent' not in g_eef_info_list[limb]: g_eef_info_list[limb]['parent'] = parent_link_name # print for debug print('eef_infos param list:') From e1460cd9b963ae6b1fc248261bbce0d0667d0273 Mon Sep 17 00:00:00 2001 From: y-iwata Date: Wed, 10 Jan 2024 15:32:18 +0900 Subject: [PATCH 09/33] change depth_registration value from false to true to fix the gap between XYZ and color --- hironx_tutorial/launch/hironxjsk_real.launch | 1 + 1 file changed, 1 insertion(+) diff --git a/hironx_tutorial/launch/hironxjsk_real.launch b/hironx_tutorial/launch/hironxjsk_real.launch index 09fa9d7d..9849669c 100644 --- a/hironx_tutorial/launch/hironxjsk_real.launch +++ b/hironx_tutorial/launch/hironxjsk_real.launch @@ -5,6 +5,7 @@ + From 8a071051fcfe4ecd3a9e06d82e5550ce17371137 Mon Sep 17 00:00:00 2001 From: y-iwata Date: Wed, 10 Jan 2024 21:16:58 +0900 Subject: [PATCH 10/33] add description of image transport plugin param for hw_registered --- .../hironxjsk_image_transport_plugins_params.yaml | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/hironx_tutorial/config/hironxjsk_image_transport_plugins_params.yaml b/hironx_tutorial/config/hironxjsk_image_transport_plugins_params.yaml index 57ae1271..91fd43d7 100644 --- a/hironx_tutorial/config/hironxjsk_image_transport_plugins_params.yaml +++ b/hironx_tutorial/config/hironxjsk_image_transport_plugins_params.yaml @@ -51,6 +51,19 @@ head_camera: - 'image_transport/theora' compressedDepth: png_level: 5 + hw_registered: + image_rect: + disable_pub_plugins: + - 'image_transport/compressed' + - 'image_transport/theora' + compressedDepth: + png_level: 5 + image_rect_raw: + disable_pub_plugins: + - 'image_transport/compressed' + - 'image_transport/theora' + compressedDepth: + png_level: 5 ir: image: disable_pub_plugins: From 0d724aeac1ed97d21cee5649a82b0b4a56fff1de Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Fri, 2 May 2025 22:21:52 +0900 Subject: [PATCH 11/33] [hrpsys_ros_bridge_tutorials] Enable to compile on pure Ubuntu Focal On Ubuntu Focal, "/usr/bin/env python" cannot be executed unless python-is-python2/3 is installed --- hrpsys_ros_bridge_tutorials/CMakeLists.txt | 12 ++++++++++-- .../models/gen_hand_attached_staro_model.sh | 15 +++++++++++++-- .../models/gen_sensor_attached_staro_model.sh | 17 ++++++++++++++--- 3 files changed, 37 insertions(+), 7 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/CMakeLists.txt b/hrpsys_ros_bridge_tutorials/CMakeLists.txt index 594a5e3e..dfd51bfb 100644 --- a/hrpsys_ros_bridge_tutorials/CMakeLists.txt +++ b/hrpsys_ros_bridge_tutorials/CMakeLists.txt @@ -543,6 +543,14 @@ else () set(euscollada_PACKAGE_PATH ${euscollada_PREFIX}/share/euscollada) endif() +# enable to execute with correct python version +# cf. https://github.com/tork-a/openrtm_aist_python-release/pull/5 +find_package(PythonInterp QUIET) +if(NOT PYTHONINTERP_FOUND) + find_package(Python REQUIRED) + set(PYTHON_EXECUTABLE ${Python_EXECUTABLE}) +endif() + macro (generate_hand_attached_hrp2_model _robot_name) set(_model_dir "${PROJECT_SOURCE_DIR}/models/") set(_in_urdf_file "${_model_dir}/${_robot_name}.urdf") @@ -551,7 +559,7 @@ macro (generate_hand_attached_hrp2_model _robot_name) string(TOLOWER ${_robot_name} _srobot_name) message("generate hand_attached_hrp2_model for ${_robot_name}") add_custom_command(OUTPUT ${_out_urdf_file} - COMMAND ${_script_file} + COMMAND ${PYTHON_EXECUTABLE} ${_script_file} LARM_LINK6 RARM_LINK6 ${_in_urdf_file} ${_out_urdf_file} DEPENDS ${_in_urdf_file} ${_script_file} ${_srobot_name}_${PROJECT_NAME}_compile_urdf) endmacro() @@ -577,7 +585,7 @@ macro (attach_sensor_and_endeffector_to_hrp2jsk_urdf set(_out_urdf_file "${_model_dir}/${_out_file}") set(_script_file ${euscollada_PACKAGE_PATH}/scripts/add_sensor_to_collada.py) add_custom_command(OUTPUT ${_out_urdf_file} - COMMAND ${_script_file} + COMMAND ${PYTHON_EXECUTABLE} ${_script_file} ${_in_urdf_file} -O ${_out_urdf_file} -C ${_in_yaml_file} DEPENDS ${_in_urdf_file} ${_in_yaml_file} ${_script_file}) list(APPEND compile_urdf_robots ${_out_urdf_file}) diff --git a/hrpsys_ros_bridge_tutorials/models/gen_hand_attached_staro_model.sh b/hrpsys_ros_bridge_tutorials/models/gen_hand_attached_staro_model.sh index ce349681..ff0bb4ac 100755 --- a/hrpsys_ros_bridge_tutorials/models/gen_hand_attached_staro_model.sh +++ b/hrpsys_ros_bridge_tutorials/models/gen_hand_attached_staro_model.sh @@ -10,7 +10,18 @@ INPUT_FILE=$2 EUSCOLLADA_PATH=$3 BODY_FILE=${INPUT_FILE//.urdf/_body.urdf} +# enable to execute with correct python version +# cf. https://github.com/tork-a/openrtm_aist_python-release/pull/5 +if command -v python3 &>/dev/null; then + PYTHON_EXECUTABLE=$(command -v python3) +elif command -v python &>/dev/null; then + PYTHON_EXECUTABLE=$(command -v python) +else + echo "Cannot find python executable" + exit 1 +fi + tmp1=`mktemp` tmp2=`mktemp` -${EUSCOLLADA_PATH}/scripts/remove_sensor_from_urdf.py LARM_LINK7 $INPUT_FILE $tmp1 -${EUSCOLLADA_PATH}/scripts/remove_sensor_from_urdf.py RARM_LINK7 $tmp1 $BODY_FILE +${PYTHON_EXECUTABLE} ${EUSCOLLADA_PATH}/scripts/remove_sensor_from_urdf.py LARM_LINK7 $INPUT_FILE $tmp1 +${PYTHON_EXECUTABLE} ${EUSCOLLADA_PATH}/scripts/remove_sensor_from_urdf.py RARM_LINK7 $tmp1 $BODY_FILE diff --git a/hrpsys_ros_bridge_tutorials/models/gen_sensor_attached_staro_model.sh b/hrpsys_ros_bridge_tutorials/models/gen_sensor_attached_staro_model.sh index 26d2c55a..6e8163f5 100755 --- a/hrpsys_ros_bridge_tutorials/models/gen_sensor_attached_staro_model.sh +++ b/hrpsys_ros_bridge_tutorials/models/gen_sensor_attached_staro_model.sh @@ -9,11 +9,22 @@ input_urdf=$3 output_urdf=$4 yaml_file=$5 +# enable to execute with correct python version +# cf. https://github.com/tork-a/openrtm_aist_python-release/pull/5 +if command -v python3 &>/dev/null; then + PYTHON_EXECUTABLE=$(command -v python3) +elif command -v python &>/dev/null; then + PYTHON_EXECUTABLE=$(command -v python) +else + echo "Cannot find python executable" + exit 1 +fi + # generating model with sensors function add_sensor_to_tmp_urdf() { tmp_model=`mktemp` - ${eusurdf_path}/scripts/add_sensor_to_urdf.py $@ $tmp_model + ${PYTHON_EXECUTABLE} ${eusurdf_path}/scripts/add_sensor_to_urdf.py $@ $tmp_model if [ $? == 0 ]; then echo $tmp_model else @@ -24,7 +35,7 @@ function add_sensor_to_tmp_urdf() function add_eef_to_tmp_urdf() { tmp_model=`mktemp` - ${eusurdf_path}/scripts/add_eef_to_urdf.py $@ $tmp_model + ${PYTHON_EXECUTABLE} ${eusurdf_path}/scripts/add_eef_to_urdf.py $@ $tmp_model if [ $? == 0 ]; then echo $tmp_model else @@ -59,7 +70,7 @@ function generate_staro_model() # # tmp_model11=$(add_sensor_to_tmp_urdf 0 0.2 -0.2 0 0 1.57 LARM_LINK6 LARM_cb_jig $tmp_model10) # # tmp_model12=$(add_sensor_to_tmp_urdf 0 -0.2 -0.2 0 0 1.57 RARM_LINK6 RARM_cb_jig $tmp_model11) # cp $tmp_model4 $2 - ${eusurdf_path}/scripts/add_sensor_to_collada.py $1 -O $2 -C $3 + ${PYTHON_EXECUTABLE} ${eusurdf_path}/scripts/add_sensor_to_collada.py $1 -O $2 -C $3 } generate_staro_model $input_urdf $output_urdf $yaml_file From ab83adbb9030978b473368fbe8601366f7dfc168 Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Tue, 21 Oct 2025 18:47:48 +0900 Subject: [PATCH 12/33] Set up head_camera, larm_camera, ft_sensor and teleop --- nextage_tutorials/.gitignore | 2 + nextage_tutorials/CMakeLists.txt | 11 +- nextage_tutorials/config/ft_calib_data.yaml | 16 + nextage_tutorials/euslisp/nextage-teleope.l | 173 ++++++ nextage_tutorials/launch/dual_touch.launch | 26 + nextage_tutorials/launch/elp_head.launch | 47 ++ .../launch/ft_sensor_calib.launch | 33 ++ nextage_tutorials/launch/l515_head.launch | 151 ++++++ .../launch/right_ft_sensor.launch | 7 + nextage_tutorials/launch/slave.launch | 27 + nextage_tutorials/package.xml | 5 +- .../scripts/create_udev_rules.sh | 14 + nextage_tutorials/scripts/tf_to_imu.py | 29 + nextage_tutorials/udev/99-elp-camera.rules | 1 + nextage_tutorials/urdf/JSKNextageOpen.urdf | 512 ++++++++++++++++++ 15 files changed, 1052 insertions(+), 2 deletions(-) create mode 100644 nextage_tutorials/.gitignore create mode 100644 nextage_tutorials/config/ft_calib_data.yaml create mode 100644 nextage_tutorials/euslisp/nextage-teleope.l create mode 100644 nextage_tutorials/launch/dual_touch.launch create mode 100644 nextage_tutorials/launch/elp_head.launch create mode 100644 nextage_tutorials/launch/ft_sensor_calib.launch create mode 100644 nextage_tutorials/launch/l515_head.launch create mode 100644 nextage_tutorials/launch/right_ft_sensor.launch create mode 100644 nextage_tutorials/launch/slave.launch create mode 100755 nextage_tutorials/scripts/create_udev_rules.sh create mode 100644 nextage_tutorials/scripts/tf_to_imu.py create mode 100644 nextage_tutorials/udev/99-elp-camera.rules create mode 100644 nextage_tutorials/urdf/JSKNextageOpen.urdf diff --git a/nextage_tutorials/.gitignore b/nextage_tutorials/.gitignore new file mode 100644 index 00000000..b7e8bda4 --- /dev/null +++ b/nextage_tutorials/.gitignore @@ -0,0 +1,2 @@ +nextage.dae +nextage.l \ No newline at end of file diff --git a/nextage_tutorials/CMakeLists.txt b/nextage_tutorials/CMakeLists.txt index 0a8d8182..bd7c4fc5 100644 --- a/nextage_tutorials/CMakeLists.txt +++ b/nextage_tutorials/CMakeLists.txt @@ -38,8 +38,17 @@ endif() ############# ## Install ## ############# +catkin_install_python(PROGRAMS + scripts/tf_to_imu.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) -install(DIRECTORY config euslisp launch model sample +install(PROGRAMS + scripts/create_udev_rules.sh + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY config euslisp launch model sample udev urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS) diff --git a/nextage_tutorials/config/ft_calib_data.yaml b/nextage_tutorials/config/ft_calib_data.yaml new file mode 100644 index 00000000..aee443a1 --- /dev/null +++ b/nextage_tutorials/config/ft_calib_data.yaml @@ -0,0 +1,16 @@ +bias: + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - -0.08200792164801718 + - 0.0 +gripper_mass: 0.8180173125445871 +gripper_com_pose: + - 0.002033092466916677 + - -0.002542349869443385 + - 0.08879802276111172 + - 0 + - 0 + - 0 +gripper_com_frame_id: lwr_7_link \ No newline at end of file diff --git a/nextage_tutorials/euslisp/nextage-teleope.l b/nextage_tutorials/euslisp/nextage-teleope.l new file mode 100644 index 00000000..37c77962 --- /dev/null +++ b/nextage_tutorials/euslisp/nextage-teleope.l @@ -0,0 +1,173 @@ +#!/usr/bin/env roseus + +;; Load and Initialize roseus program +(ros::load-ros-manifest "roseus") +(ros::roseus "surgical-nextage-manekin") + +;; Import msgs. +(ros::roseus-add-msgs "geometry_msgs") +(ros::roseus-add-msgs "sensor_msgs") + +;; Import Robot Interface +(require "package://jsk_surgical_nextage_robot/euslisp/surgical-nextage-interface.l") + +(defun apply-deadband (value threshold) + (if (< (abs value) threshold) + 0.0 + value)) + +(defun apply-deadband-to-axes (axes threshold) + (let ((result (copy-seq axes))) + (dotimes (i (length axes)) + (setf (elt result i) (apply-deadband (elt axes i) threshold))) + result)) + +;; Callback for left spacenav +(defun left-cb (msg) + (setq *ljoy-raw* (send msg :axes)) + (setq *ljoy* (apply-deadband-to-axes *ljoy-raw* 0.00)) + (setq *lbot* (send msg :buttons)) + ) + +;; Callback for right spacenav +(defun right-cb (msg) + (setq *rjoy-raw* (send msg :axes)) + (setq *rjoy* (apply-deadband-to-axes *rjoy-raw* 0.05)) + (setq *rbot* (send msg :buttons)) + ) + +;; Initialization +(nextage-init) +(objects (list *nextage*)) + +;; Global params for left arm +(setq *l-forceps-rotation* 2.4) ;; This param is to be fixed. +(setq *l-grasp-position* t) + +;; Global params for right arm +(setq *r-forceps-rotation* -2.4) ;; This param is to be fixed. +(setq *r-grasp-position* t) + +;; Reset pose +(send *nextage* :reset-pose) +(send *nextage* :head :angle-vector #f(0 30)) +(send *ri* :angle-vector (send *nextage* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + +;; Initialize joystick values +(setq *ljoy-raw* #f(0 0 0 0 0 0)) +(setq *ljoy* #f(0 0 0 0 0 0)) +(setq *lbot* #f(0 0)) +(setq *rjoy-raw* #f(0 0 0 0 0 0)) +(setq *rjoy* #f(0 0 0 0 0 0)) +(setq *rbot* #f(0 0)) + +;; Subscribe to spacenav topics +(ros::subscribe "spacenav/joy" sensor_msgs::Joy #'left-cb) +(ros::subscribe "/right_manip/spacenav/joy" sensor_msgs::Joy #'right-cb) + +;; Common scales for both arms +(setq *scale* 15) +(setq *scale-pitch* 0.02) +(setq *scale-yaw* 1) +(setq *deadband-threshold* 0.04) + +(do-until-key + (ros::spin-once) + + (format t "Left Joy Raw: ~A~%" *ljoy-raw*) + (format t "Left Joy Filtered: ~A~%" *ljoy*) + (format t "Right Joy Raw: ~A~%" *rjoy-raw*) + (format t "Right Joy Filtered: ~A~%" *rjoy*) + + ;; Left arm control + (setq *ljoyx* (* (elt *ljoy* 0) *scale*)) + (setq *ljoyy* (* (elt *ljoy* 1) *scale*)) + (setq *ljoyz* (* (elt *ljoy* 2) *scale*)) + (setq *lroll* (* (elt *ljoy* 3) *scale-pitch*)) + (setq *lpitch* (* (elt *ljoy* 4) *scale-pitch*)) + (setq *lyaw* (* (elt *ljoy* 5) *scale-yaw*)) + (setq *lbot1* (elt *lbot* 0)) + (setq *lbot2* (elt *lbot* 1)) + + ;; Right arm control + (setq *rjoyx* (* (elt *rjoy* 0) *scale*)) + (setq *rjoyy* (* (elt *rjoy* 1) *scale*)) + (setq *rjoyz* (* (elt *rjoy* 2) *scale*)) + (setq *rroll* (* (elt *rjoy* 3) *scale-pitch*)) + (setq *rpitch* (* (elt *rjoy* 4) *scale-pitch*)) + (setq *ryaw* (* (elt *rjoy* 5) *scale-yaw*)) + (setq *rbot1* (elt *rbot* 0)) + (setq *rbot2* (elt *rbot* 1)) + + (if (or (not (= *ljoyx* 0)) (not (= *ljoyy* 0)) (not (= *ljoyz* 0)) + (not (= *lroll* 0)) (not (= *lpitch* 0))) + (progn + (setq *larm-coords* (send (send *nextage* :larm :end-coords) :copy-worldcoords)) + (send *larm-coords* :translate (float-vector *ljoyz* *ljoyy* (- 0 *ljoyx*))) + (send *larm-coords* :rotate *lpitch* :y) + (send *larm-coords* :rotate *lroll* :x) + (send *nextage* :larm :inverse-kinematics *larm-coords* :rotation-axis t :stop 3 :revert-if-fail nil))) + + (if (or (not (= *rjoyx* 0)) (not (= *rjoyy* 0)) (not (= *rjoyz* 0)) + (not (= *rroll* 0)) (not (= *rpitch* 0))) + (progn + (setq *rarm-coords* (send (send *nextage* :rarm :end-coords) :copy-worldcoords)) + (send *rarm-coords* :translate (float-vector *rjoyz* *rjoyy* (- 0 *rjoyx*))) + (send *rarm-coords* :rotate *rpitch* :y) + (send *rarm-coords* :rotate *rroll* :x) + (send *nextage* :rarm :inverse-kinematics *rarm-coords* :rotation-axis t :stop 3 :revert-if-fail nil))) + + ;; Send angle vector for both arms + (send *ri* :angle-vector (send *nextage* :angle-vector) 500) + + ;; Left forceps control + (if (or (= *lbot1* 1) (= *lbot2* 1)) + (progn + (if (> (abs *lyaw*) 0) + (if (> *lyaw* 0) + (setq *l-forceps-rotation* (+ *l-forceps-rotation* 0.2)) + (setq *l-forceps-rotation* (- *l-forceps-rotation* 0.2)))) + ;;(setq *l-forceps-rotation* (pi_cycle *l-forceps-rotation*)) + + (if (= *lbot1* 1) + (progn + (send *ri* :lforceps :grasp t :rotate *l-forceps-rotation*) + (setq *l-grasp-position* t))) + + (if (= *lbot2* 1) + (progn + (send *ri* :lforceps :grasp nil :rotate *l-forceps-rotation*) + (setq *l-grasp-position* nil))) + + (if (and (= *lbot1* 1) (= *lbot2* 1)) + (send *ri* :lforceps :grasp *l-grasp-position* :rotate *l-forceps-rotation*)))) + + ;; Right forceps control + (if (or (= *rbot1* 1) (= *rbot2* 1)) + (progn + (if (> (abs *ryaw*) 0) + (if (> *ryaw* 0) + (setq *r-forceps-rotation* (+ *r-forceps-rotation* 0.2)) + (setq *r-forceps-rotation* (- *r-forceps-rotation* 0.2)))) + + ;;(setq *r-forceps-rotation* (pi_cycle *r-forceps-rotation*)) + + (if (= *rbot1* 1) + (progn + (send *ri* :rforceps :grasp t :rotate *r-forceps-rotation*) + (setq *r-grasp-position* t))) + + (if (= *rbot2* 1) + (progn + (send *ri* :rforceps :grasp nil :rotate *r-forceps-rotation*) + (setq *r-grasp-position* nil))) + + (if (and (= *rbot1* 1) (= *rbot2* 1)) + (send *ri* :rforceps :grasp *r-grasp-position* :rotate *r-forceps-rotation*)))) + + (send *irtviewer* :draw-objects) + ;;(send *ri* :wait-interpolation) + (ros::duration-sleep 0.5) + (ros::spin-once)) \ No newline at end of file diff --git a/nextage_tutorials/launch/dual_touch.launch b/nextage_tutorials/launch/dual_touch.launch new file mode 100644 index 00000000..ea277d13 --- /dev/null +++ b/nextage_tutorials/launch/dual_touch.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nextage_tutorials/launch/elp_head.launch b/nextage_tutorials/launch/elp_head.launch new file mode 100644 index 00000000..4c8e052c --- /dev/null +++ b/nextage_tutorials/launch/elp_head.launch @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + + intrinsic_controls: + focus_auto: true + exposure_auto_priority: false + exposure_auto: 1 + white_balance_temperature_auto: false + power_line_frequency: 1 + brightness: 200 + ignore: + - contrast + - saturation + - sharpness + - backlight_compensation + - white_balance_temperature + - pan_absolute + - tilt_absolute + - focus_absolute + - zoom_absolute + + + + + + + + diff --git a/nextage_tutorials/launch/ft_sensor_calib.launch b/nextage_tutorials/launch/ft_sensor_calib.launch new file mode 100644 index 00000000..c40e5b57 --- /dev/null +++ b/nextage_tutorials/launch/ft_sensor_calib.launch @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nextage_tutorials/launch/l515_head.launch b/nextage_tutorials/launch/l515_head.launch new file mode 100644 index 00000000..ebac7dd0 --- /dev/null +++ b/nextage_tutorials/launch/l515_head.launch @@ -0,0 +1,151 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nextage_tutorials/launch/right_ft_sensor.launch b/nextage_tutorials/launch/right_ft_sensor.launch new file mode 100644 index 00000000..cf2f1820 --- /dev/null +++ b/nextage_tutorials/launch/right_ft_sensor.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/nextage_tutorials/launch/slave.launch b/nextage_tutorials/launch/slave.launch new file mode 100644 index 00000000..6f77f2fd --- /dev/null +++ b/nextage_tutorials/launch/slave.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nextage_tutorials/package.xml b/nextage_tutorials/package.xml index 6fc36136..da9a24b3 100644 --- a/nextage_tutorials/package.xml +++ b/nextage_tutorials/package.xml @@ -12,7 +12,10 @@ pr2eus euscollada nextage_description - nextage_gazebo + robotiq_ft_sensor + usb_cam + gravity_compensation + diff --git a/nextage_tutorials/scripts/create_udev_rules.sh b/nextage_tutorials/scripts/create_udev_rules.sh new file mode 100755 index 00000000..8539d184 --- /dev/null +++ b/nextage_tutorials/scripts/create_udev_rules.sh @@ -0,0 +1,14 @@ +create_udev_rules.sh +#!/bin/bash + +echo "" +echo "This scripts copies udev rules for dual hands to /etc/udev/rules.d" +echo "" + +sudo cp `rospack find nextage_tutorials`/udev/99-elp-camera.rules /etc/udev/rules.d + +echo "" +echo "Restarting udev" +echo "" +sudo service udev reload +sudo service udev restart diff --git a/nextage_tutorials/scripts/tf_to_imu.py b/nextage_tutorials/scripts/tf_to_imu.py new file mode 100644 index 00000000..8c097cd7 --- /dev/null +++ b/nextage_tutorials/scripts/tf_to_imu.py @@ -0,0 +1,29 @@ +#!/usr/bin/env python3 + +import rospy +import tf2_ros +from sensor_msgs.msg import Imu + +if __name__ == '__main__': + rospy.init_node('tf_to_imu') + + base_frame = rospy.get_param("~base_frame", "CHEST_JOINT0_Link") + sensor_frame = rospy.get_param("~sensor_frame", "RARM_JOINT5_Link") + imu_topic = rospy.get_param("~imu_topic", "/imu/data") + + tf_buffer = tf2_ros.Buffer() + listener = tf2_ros.TransformListener(tf_buffer) + pub = rospy.Publisher(imu_topic, Imu, queue_size=10) + + rate = rospy.Rate(100) + while not rospy.is_shutdown(): + try: + trans = tf_buffer.lookup_transform(base_frame, sensor_frame, rospy.Time(0)) + imu_msg = Imu() + imu_msg.header.stamp = rospy.Time.now() + imu_msg.header.frame_id = sensor_frame + imu_msg.orientation = trans.transform.rotation + pub.publish(imu_msg) + except Exception as e: + rospy.logwarn_throttle(5.0, f"TF not available yet: {e}") + rate.sleep() diff --git a/nextage_tutorials/udev/99-elp-camera.rules b/nextage_tutorials/udev/99-elp-camera.rules new file mode 100644 index 00000000..0f8700c2 --- /dev/null +++ b/nextage_tutorials/udev/99-elp-camera.rules @@ -0,0 +1 @@ +SUBSYSTEM=="video4linux", ATTRS{idVendor}=="32e4", ATTRS{idProduct}=="4802", ATTRS{serial}=="2023081002", SYMLINK+="elp01", MODE="0666" \ No newline at end of file diff --git a/nextage_tutorials/urdf/JSKNextageOpen.urdf b/nextage_tutorials/urdf/JSKNextageOpen.urdf new file mode 100644 index 00000000..f21fe630 --- /dev/null +++ b/nextage_tutorials/urdf/JSKNextageOpen.urdf @@ -0,0 +1,512 @@ + + + + + + + Gazebo/${color} + + + + + + transmission_interface/SimpleTransmission + + EffortJointInterface + + + EffortJointInterface + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + / + gazebo_ros_control/DefaultRobotHWSim + + + + From e45a48cde328b091677ea59352d566b07f987fcf Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Fri, 24 Oct 2025 17:53:47 +0900 Subject: [PATCH 13/33] Create teleope program --- .../euslisp/jsk-nextage-teleope.l | 143 ++++++++++++++++++ 1 file changed, 143 insertions(+) create mode 100644 nextage_tutorials/euslisp/jsk-nextage-teleope.l diff --git a/nextage_tutorials/euslisp/jsk-nextage-teleope.l b/nextage_tutorials/euslisp/jsk-nextage-teleope.l new file mode 100644 index 00000000..68531ae8 --- /dev/null +++ b/nextage_tutorials/euslisp/jsk-nextage-teleope.l @@ -0,0 +1,143 @@ +#!/usr/bin/env roseus +;; Load packages +(ros::load-ros-manifest "roseus") +(ros::roseus "nextage-phantom") +(ros::roseus-add-msgs "geometry_msgs") +(ros::roseus-add-msgs "omni_msgs") +(load "models/arrow-object.l") +(require "package://nextage_tutorials/euslisp/nextage-interface.l") +(require "package://dynamixel_detachable_hand/euslisp/lhand-interface.l") +(require "package://dynamixel_detachable_hand/euslisp/rhand-interface.l") + +;; Parameters +(setq *use-torso* nil) +(setq *use-hand* nil) +(setq *base-offset-l* (float-vector 380 200 0)) +(setq *base-offset-r* (float-vector 380 -200 0)) +(setq *ratio* 3.0) ;; control ratio +(setq *send-time* 100) ;; ms +(ros::rate 10) ;; Hz *send-time* and rate should be balanced. + +(defun initialize-variables() + (setq *lpos* nil) + (setq *lrot* nil) + (setq *ltf* nil) + (setq *lcoords* nil) + (setq *lclose* nil) + (setq *llocked* nil) + (setq *rpos* nil) + (setq *rrot* nil) + (setq *rtf* nil) + (setq *rcoords* nil) + (setq *rclose* nil) + (setq *rlocked* nil) + ) + +(defun check-work-limit() + ) + +(defun check-collision() + ) + +(defun left-phantom-cb (msg) + (setq *lpos* (send msg :pose :position)) + (setq *lrot* (send msg :pose :orientation)) +;; (setq *ltf* (ros::tf-pose->coords (send msg :pose))) +;; (setq *lcoords* (make-coords :rot (copy-object (send *ltf* :rot)))) +;; (send *lcoords* :rotate pi/2 :x :world) +;; (send *lcoords* :locate (send *ltf* :worldpos) :world) + (setq *lclose* (send msg :close_gripper)) + (setq *llocked* (send msg :locked)) + ) + +(defun right-phantom-cb (msg) + (setq *rpos* (send msg :pose :position)) + (setq *rrot* (send msg :pose :orientation)) +;; (setq *rtf* (ros::tf-pose->coords (send msg :pose))) +;; (setq *rcoords* (make-coords :rot (copy-object (send *rtf* :rot)))) +;; (send *rcoords* :rotate pi/2 :x :world) +;; (send *rcoords* :locate (send *rtf* :worldpos) :world) + (setq *rclose* (send msg :close_gripper)) + (setq *rlocked* (send msg :locked)) + ) + + +;; Initialize +(ros::rate 10) +(ros::subscribe "/left_device/phantom/state" omni_msgs::OmniState #'left-phantom-cb) +(ros::subscribe "/right_device/phantom/state" omni_msgs::OmniState #'right-phantom-cb) + +(initialize-variables) +(nextage-init) +(lhand-init) +(rhand-init) +(setq *lc* (make-cube 10 10 10)) ;; left-center +(send *lc* :translate *base-offset-l*) +(send *lc* :set-color :yellow) +(setq *rc* (make-cube 10 10 10)) ;; right-center +(send *rc* :translate *base-offset-r*) +(send *rc* :set-color :yellow) +(setq *robot* *nextage*) +(setq *la-target* (arrow)) +(setq *la-end* (arrow)) +(setq *ra-target* (arrow)) +(setq *ra-end* (arrow)) +(objects (list *nextage* *lc* *rc* *la-target* *la-end*)) + +;; Reset robot +(send *nextage* :reset-pose) +(send *nextage* :head :angle-vector #f(0 10)) +(send *ri* :angle-vector (send *nextage* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + +;; Constants +(setq R_map (matrix (float-vector 0 -1 0) + (float-vector 1 0 0) + (float-vector 0 0 1))) + +;; Main +(do-until-key + (ros::spin-once) + + (when (and *lcoords* (not *llocked*)) + (setq *lx* (+ (* (send *lpos* :y) *ratio*) (elt *base-offset-l* 0))) + (setq *ly* (+ (* (- 0 (send *lpos* :x)) *ratio*) (elt *base-offset-l* 1))) + (setq *lz* (+ (* (send *lpos* :z) *ratio*) (elt *base-offset-l* 2))) + (setq lR_src (quaternion2matrix (float-vector (send *lrot* :w) + (send *lrot* :x) + (send *lrot* :y) + (send *lrot* :z)))) + (setq lR_dst (m* R_map lR_src)) + (setq *larm-target* (make-coords :pos (float-vector *lx* *ly* *lz*) :rot lR_dst)) + (send *la-target* :move-to *larm-target* :world) + (send *la-end* :move-to (send (send *nextage* :larm :end-coords) :copy-worldcoords) :world) + (send *nextage* :larm :inverse-kinematics *larm-target* :use-torso *use-torso* :rotation-axis t :stop 4 :revert-if-fail nil)) + + (when (and *rpos* *rrot* (not *rlocked*)) + (setq *rx* (+ (* (send *rpos* :y) *ratio*) (elt *base-offset-r* 0))) + (setq *ry* (+ (* (- 0 (send *rpos* :x)) *ratio*) (elt *base-offset-r* 1))) + (setq *rz* (+ (* (send *rpos* :z) *ratio*) (elt *base-offset-r* 2))) + (setq rR_src (quaternion2matrix (float-vector (send *rrot* :w) + (send *rrot* :x) + (send *rrot* :y) + (send *rrot* :z)))) + (setq rR_dst (m* R_map rR_src)) + (setq *rarm-target* (make-coords :pos (float-vector *rx* *ry* *rz*) :rot rR_dst)) + (send *nextage* :rarm :inverse-kinematics *rarm-target* :use-torso *use-torso* :rotation-axis t :stop 4 :revert-if-fail nil)) + + (send *ri* :angle-vector (send *nextage* :angle-vector) 100) + + (when (and *lclose* (not *llocked*)) + (if *lclose* + (send *lhand* :close) + (send *lhand* :open))) + + (when (and *rclose* (not *rlocked*)) + (if *rclose* + (send *rhand* :close-holder) + (send *rhand* :open-holder))) + + (send *irtviewer* :draw-objects) + (ros::sleep) + ) From 89d4a9ea33e7253fcc6954da22f70dcb11c7db5d Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Fri, 24 Oct 2025 19:03:45 +0900 Subject: [PATCH 14/33] Create udev rules and se up launch files --- .../euslisp/jsk-nextage-teleope.l | 5 + nextage_tutorials/euslisp/nextage-teleope.l | 173 ------------------ .../{elp_head.launch => elp_camera.launch} | 4 +- .../launch/jsk_nextageopen.launch | 25 +++ .../launch/jsk_robot_state_publisher.launch | 12 ++ .../launch/right_ft_sensor.launch | 2 +- nextage_tutorials/launch/slave.launch | 27 --- .../scripts/create_udev_rules.sh | 6 +- nextage_tutorials/udev/99-rftsensor.rules | 1 + 9 files changed, 50 insertions(+), 205 deletions(-) delete mode 100644 nextage_tutorials/euslisp/nextage-teleope.l rename nextage_tutorials/launch/{elp_head.launch => elp_camera.launch} (93%) create mode 100644 nextage_tutorials/launch/jsk_nextageopen.launch create mode 100644 nextage_tutorials/launch/jsk_robot_state_publisher.launch delete mode 100644 nextage_tutorials/launch/slave.launch create mode 100644 nextage_tutorials/udev/99-rftsensor.rules diff --git a/nextage_tutorials/euslisp/jsk-nextage-teleope.l b/nextage_tutorials/euslisp/jsk-nextage-teleope.l index 68531ae8..5938f446 100644 --- a/nextage_tutorials/euslisp/jsk-nextage-teleope.l +++ b/nextage_tutorials/euslisp/jsk-nextage-teleope.l @@ -141,3 +141,8 @@ (send *irtviewer* :draw-objects) (ros::sleep) ) + +#| +Memo: Using the tf conversion method descripted in https://github.com/YUKINA-3252/jsk_hironx_teleop/issues/2, we cannot control the ration of :translate, +So I don't use it intentionally. +=# diff --git a/nextage_tutorials/euslisp/nextage-teleope.l b/nextage_tutorials/euslisp/nextage-teleope.l deleted file mode 100644 index 37c77962..00000000 --- a/nextage_tutorials/euslisp/nextage-teleope.l +++ /dev/null @@ -1,173 +0,0 @@ -#!/usr/bin/env roseus - -;; Load and Initialize roseus program -(ros::load-ros-manifest "roseus") -(ros::roseus "surgical-nextage-manekin") - -;; Import msgs. -(ros::roseus-add-msgs "geometry_msgs") -(ros::roseus-add-msgs "sensor_msgs") - -;; Import Robot Interface -(require "package://jsk_surgical_nextage_robot/euslisp/surgical-nextage-interface.l") - -(defun apply-deadband (value threshold) - (if (< (abs value) threshold) - 0.0 - value)) - -(defun apply-deadband-to-axes (axes threshold) - (let ((result (copy-seq axes))) - (dotimes (i (length axes)) - (setf (elt result i) (apply-deadband (elt axes i) threshold))) - result)) - -;; Callback for left spacenav -(defun left-cb (msg) - (setq *ljoy-raw* (send msg :axes)) - (setq *ljoy* (apply-deadband-to-axes *ljoy-raw* 0.00)) - (setq *lbot* (send msg :buttons)) - ) - -;; Callback for right spacenav -(defun right-cb (msg) - (setq *rjoy-raw* (send msg :axes)) - (setq *rjoy* (apply-deadband-to-axes *rjoy-raw* 0.05)) - (setq *rbot* (send msg :buttons)) - ) - -;; Initialization -(nextage-init) -(objects (list *nextage*)) - -;; Global params for left arm -(setq *l-forceps-rotation* 2.4) ;; This param is to be fixed. -(setq *l-grasp-position* t) - -;; Global params for right arm -(setq *r-forceps-rotation* -2.4) ;; This param is to be fixed. -(setq *r-grasp-position* t) - -;; Reset pose -(send *nextage* :reset-pose) -(send *nextage* :head :angle-vector #f(0 30)) -(send *ri* :angle-vector (send *nextage* :angle-vector) 1000) -(send *ri* :wait-interpolation) -(send *irtviewer* :draw-objects) - -;; Initialize joystick values -(setq *ljoy-raw* #f(0 0 0 0 0 0)) -(setq *ljoy* #f(0 0 0 0 0 0)) -(setq *lbot* #f(0 0)) -(setq *rjoy-raw* #f(0 0 0 0 0 0)) -(setq *rjoy* #f(0 0 0 0 0 0)) -(setq *rbot* #f(0 0)) - -;; Subscribe to spacenav topics -(ros::subscribe "spacenav/joy" sensor_msgs::Joy #'left-cb) -(ros::subscribe "/right_manip/spacenav/joy" sensor_msgs::Joy #'right-cb) - -;; Common scales for both arms -(setq *scale* 15) -(setq *scale-pitch* 0.02) -(setq *scale-yaw* 1) -(setq *deadband-threshold* 0.04) - -(do-until-key - (ros::spin-once) - - (format t "Left Joy Raw: ~A~%" *ljoy-raw*) - (format t "Left Joy Filtered: ~A~%" *ljoy*) - (format t "Right Joy Raw: ~A~%" *rjoy-raw*) - (format t "Right Joy Filtered: ~A~%" *rjoy*) - - ;; Left arm control - (setq *ljoyx* (* (elt *ljoy* 0) *scale*)) - (setq *ljoyy* (* (elt *ljoy* 1) *scale*)) - (setq *ljoyz* (* (elt *ljoy* 2) *scale*)) - (setq *lroll* (* (elt *ljoy* 3) *scale-pitch*)) - (setq *lpitch* (* (elt *ljoy* 4) *scale-pitch*)) - (setq *lyaw* (* (elt *ljoy* 5) *scale-yaw*)) - (setq *lbot1* (elt *lbot* 0)) - (setq *lbot2* (elt *lbot* 1)) - - ;; Right arm control - (setq *rjoyx* (* (elt *rjoy* 0) *scale*)) - (setq *rjoyy* (* (elt *rjoy* 1) *scale*)) - (setq *rjoyz* (* (elt *rjoy* 2) *scale*)) - (setq *rroll* (* (elt *rjoy* 3) *scale-pitch*)) - (setq *rpitch* (* (elt *rjoy* 4) *scale-pitch*)) - (setq *ryaw* (* (elt *rjoy* 5) *scale-yaw*)) - (setq *rbot1* (elt *rbot* 0)) - (setq *rbot2* (elt *rbot* 1)) - - (if (or (not (= *ljoyx* 0)) (not (= *ljoyy* 0)) (not (= *ljoyz* 0)) - (not (= *lroll* 0)) (not (= *lpitch* 0))) - (progn - (setq *larm-coords* (send (send *nextage* :larm :end-coords) :copy-worldcoords)) - (send *larm-coords* :translate (float-vector *ljoyz* *ljoyy* (- 0 *ljoyx*))) - (send *larm-coords* :rotate *lpitch* :y) - (send *larm-coords* :rotate *lroll* :x) - (send *nextage* :larm :inverse-kinematics *larm-coords* :rotation-axis t :stop 3 :revert-if-fail nil))) - - (if (or (not (= *rjoyx* 0)) (not (= *rjoyy* 0)) (not (= *rjoyz* 0)) - (not (= *rroll* 0)) (not (= *rpitch* 0))) - (progn - (setq *rarm-coords* (send (send *nextage* :rarm :end-coords) :copy-worldcoords)) - (send *rarm-coords* :translate (float-vector *rjoyz* *rjoyy* (- 0 *rjoyx*))) - (send *rarm-coords* :rotate *rpitch* :y) - (send *rarm-coords* :rotate *rroll* :x) - (send *nextage* :rarm :inverse-kinematics *rarm-coords* :rotation-axis t :stop 3 :revert-if-fail nil))) - - ;; Send angle vector for both arms - (send *ri* :angle-vector (send *nextage* :angle-vector) 500) - - ;; Left forceps control - (if (or (= *lbot1* 1) (= *lbot2* 1)) - (progn - (if (> (abs *lyaw*) 0) - (if (> *lyaw* 0) - (setq *l-forceps-rotation* (+ *l-forceps-rotation* 0.2)) - (setq *l-forceps-rotation* (- *l-forceps-rotation* 0.2)))) - ;;(setq *l-forceps-rotation* (pi_cycle *l-forceps-rotation*)) - - (if (= *lbot1* 1) - (progn - (send *ri* :lforceps :grasp t :rotate *l-forceps-rotation*) - (setq *l-grasp-position* t))) - - (if (= *lbot2* 1) - (progn - (send *ri* :lforceps :grasp nil :rotate *l-forceps-rotation*) - (setq *l-grasp-position* nil))) - - (if (and (= *lbot1* 1) (= *lbot2* 1)) - (send *ri* :lforceps :grasp *l-grasp-position* :rotate *l-forceps-rotation*)))) - - ;; Right forceps control - (if (or (= *rbot1* 1) (= *rbot2* 1)) - (progn - (if (> (abs *ryaw*) 0) - (if (> *ryaw* 0) - (setq *r-forceps-rotation* (+ *r-forceps-rotation* 0.2)) - (setq *r-forceps-rotation* (- *r-forceps-rotation* 0.2)))) - - ;;(setq *r-forceps-rotation* (pi_cycle *r-forceps-rotation*)) - - (if (= *rbot1* 1) - (progn - (send *ri* :rforceps :grasp t :rotate *r-forceps-rotation*) - (setq *r-grasp-position* t))) - - (if (= *rbot2* 1) - (progn - (send *ri* :rforceps :grasp nil :rotate *r-forceps-rotation*) - (setq *r-grasp-position* nil))) - - (if (and (= *rbot1* 1) (= *rbot2* 1)) - (send *ri* :rforceps :grasp *r-grasp-position* :rotate *r-forceps-rotation*)))) - - (send *irtviewer* :draw-objects) - ;;(send *ri* :wait-interpolation) - (ros::duration-sleep 0.5) - (ros::spin-once)) \ No newline at end of file diff --git a/nextage_tutorials/launch/elp_head.launch b/nextage_tutorials/launch/elp_camera.launch similarity index 93% rename from nextage_tutorials/launch/elp_head.launch rename to nextage_tutorials/launch/elp_camera.launch index 4c8e052c..3b0f1334 100644 --- a/nextage_tutorials/launch/elp_head.launch +++ b/nextage_tutorials/launch/elp_camera.launch @@ -4,14 +4,14 @@ - + - + diff --git a/nextage_tutorials/launch/jsk_nextageopen.launch b/nextage_tutorials/launch/jsk_nextageopen.launch new file mode 100644 index 00000000..e1c9348e --- /dev/null +++ b/nextage_tutorials/launch/jsk_nextageopen.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/nextage_tutorials/launch/jsk_robot_state_publisher.launch b/nextage_tutorials/launch/jsk_robot_state_publisher.launch new file mode 100644 index 00000000..1e22a63d --- /dev/null +++ b/nextage_tutorials/launch/jsk_robot_state_publisher.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/nextage_tutorials/launch/right_ft_sensor.launch b/nextage_tutorials/launch/right_ft_sensor.launch index cf2f1820..d676021f 100644 --- a/nextage_tutorials/launch/right_ft_sensor.launch +++ b/nextage_tutorials/launch/right_ft_sensor.launch @@ -1,5 +1,5 @@ - + diff --git a/nextage_tutorials/launch/slave.launch b/nextage_tutorials/launch/slave.launch deleted file mode 100644 index 6f77f2fd..00000000 --- a/nextage_tutorials/launch/slave.launch +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/nextage_tutorials/scripts/create_udev_rules.sh b/nextage_tutorials/scripts/create_udev_rules.sh index 8539d184..41256ab2 100755 --- a/nextage_tutorials/scripts/create_udev_rules.sh +++ b/nextage_tutorials/scripts/create_udev_rules.sh @@ -1,4 +1,3 @@ -create_udev_rules.sh #!/bin/bash echo "" @@ -6,9 +5,12 @@ echo "This scripts copies udev rules for dual hands to /etc/udev/rules.d" echo "" sudo cp `rospack find nextage_tutorials`/udev/99-elp-camera.rules /etc/udev/rules.d - +sudo cp `rospack find nextage_tutorials`/udev/99-rftsensor.rules /etc/udev/rules.d echo "" echo "Restarting udev" echo "" sudo service udev reload sudo service udev restart + +sudo udevadm control --reload-rules +sudo udevadm trigger diff --git a/nextage_tutorials/udev/99-rftsensor.rules b/nextage_tutorials/udev/99-rftsensor.rules new file mode 100644 index 00000000..68a113e5 --- /dev/null +++ b/nextage_tutorials/udev/99-rftsensor.rules @@ -0,0 +1 @@ +SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6015", ATTRS{serial}=="DAK2EOTK", SYMLINK+="rftsensor", MODE="0666" From 41b588188d2f9d366cc338b8d25f7ddbe5c7ee22 Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Fri, 24 Oct 2025 21:04:42 +0900 Subject: [PATCH 15/33] Set up nextage-interface.l and nextage-utils.l --- nextage_tutorials/.gitignore | 5 +- nextage_tutorials/euslisp/jsk-nextage-init.l | 18 +++ ...nextage-teleope.l => jsk-nextage-teleop.l} | 70 +++----- nextage_tutorials/euslisp/nextage-interface.l | 49 ++++++ nextage_tutorials/euslisp/nextage-utils.l | 52 ++++++ nextage_tutorials/launch/d405_lhand.launch | 151 ++++++++++++++++++ .../launch/jsk_nextage_bringup.launch | 15 ++ .../launch/jsk_nextageopen.launch | 25 --- .../launch/jsk_robot_state_publisher.launch | 4 +- nextage_tutorials/launch/l515_head.launch | 6 +- .../launch/manual_tf_calib.launch | 18 +++ nextage_tutorials/package.xml | 3 +- nextage_tutorials/urdf/JSKNextageOpen.urdf | 21 +-- 13 files changed, 346 insertions(+), 91 deletions(-) create mode 100644 nextage_tutorials/euslisp/jsk-nextage-init.l rename nextage_tutorials/euslisp/{jsk-nextage-teleope.l => jsk-nextage-teleop.l} (59%) create mode 100644 nextage_tutorials/euslisp/nextage-utils.l create mode 100644 nextage_tutorials/launch/d405_lhand.launch create mode 100644 nextage_tutorials/launch/jsk_nextage_bringup.launch delete mode 100644 nextage_tutorials/launch/jsk_nextageopen.launch create mode 100644 nextage_tutorials/launch/manual_tf_calib.launch diff --git a/nextage_tutorials/.gitignore b/nextage_tutorials/.gitignore index b7e8bda4..e52fbf20 100644 --- a/nextage_tutorials/.gitignore +++ b/nextage_tutorials/.gitignore @@ -1,2 +1,5 @@ nextage.dae -nextage.l \ No newline at end of file +nextage.l +launch/ft_sensor_calib.launch +launch/manual_tf_calib.launch +scripts/tf_to_imu.py diff --git a/nextage_tutorials/euslisp/jsk-nextage-init.l b/nextage_tutorials/euslisp/jsk-nextage-init.l new file mode 100644 index 00000000..f8405f54 --- /dev/null +++ b/nextage_tutorials/euslisp/jsk-nextage-init.l @@ -0,0 +1,18 @@ +#!/usr/bin/env roseus + +(require "package://nextage_tutorials/euslisp/nextage-utils.l") +(nextage-init) +(lhand-init) +(rhand-init) + +(setq *robot* *nextage*) +(objects (list *robot*)) +(send *robot* :reset-pose) +(send *robot* :head :angle-vector #f(0 60)) +(send *robot* :larm :move-end-pos #f(0 0 100)) +(send *robot* :rarm :move-end-pos #f(0 0 100)) +(send *ri* :angle-vector (send *nextage* :angle-vector) 2000) +(send *ri* :wait-interpolation) +(send *ri* :close-holder :rarm) +(send *ri* :open-forceps :larm) +(send *irtviewer* :draw-objects) diff --git a/nextage_tutorials/euslisp/jsk-nextage-teleope.l b/nextage_tutorials/euslisp/jsk-nextage-teleop.l similarity index 59% rename from nextage_tutorials/euslisp/jsk-nextage-teleope.l rename to nextage_tutorials/euslisp/jsk-nextage-teleop.l index 5938f446..d858aa59 100644 --- a/nextage_tutorials/euslisp/jsk-nextage-teleope.l +++ b/nextage_tutorials/euslisp/jsk-nextage-teleop.l @@ -1,22 +1,20 @@ #!/usr/bin/env roseus -;; Load packages -(ros::load-ros-manifest "roseus") -(ros::roseus "nextage-phantom") + +(ros::roseus "jsk-nextage-teleop") (ros::roseus-add-msgs "geometry_msgs") (ros::roseus-add-msgs "omni_msgs") + (load "models/arrow-object.l") -(require "package://nextage_tutorials/euslisp/nextage-interface.l") -(require "package://dynamixel_detachable_hand/euslisp/lhand-interface.l") -(require "package://dynamixel_detachable_hand/euslisp/rhand-interface.l") +(require "package://nextage_tutorials/euslisp/nextage-utils.l") ;; Parameters (setq *use-torso* nil) (setq *use-hand* nil) -(setq *base-offset-l* (float-vector 380 200 0)) -(setq *base-offset-r* (float-vector 380 -200 0)) -(setq *ratio* 3.0) ;; control ratio -(setq *send-time* 100) ;; ms -(ros::rate 10) ;; Hz *send-time* and rate should be balanced. +(setq *base-offset-l* (float-vector 400 100 50)) +(setq *base-offset-r* (float-vector 400 -100 50)) +(setq *send-time* 200) +(setq *ratio* 0.5) +(ros::rate 5) (defun initialize-variables() (setq *lpos* nil) @@ -36,16 +34,9 @@ (defun check-work-limit() ) -(defun check-collision() - ) - (defun left-phantom-cb (msg) (setq *lpos* (send msg :pose :position)) (setq *lrot* (send msg :pose :orientation)) -;; (setq *ltf* (ros::tf-pose->coords (send msg :pose))) -;; (setq *lcoords* (make-coords :rot (copy-object (send *ltf* :rot)))) -;; (send *lcoords* :rotate pi/2 :x :world) -;; (send *lcoords* :locate (send *ltf* :worldpos) :world) (setq *lclose* (send msg :close_gripper)) (setq *llocked* (send msg :locked)) ) @@ -53,17 +44,10 @@ (defun right-phantom-cb (msg) (setq *rpos* (send msg :pose :position)) (setq *rrot* (send msg :pose :orientation)) -;; (setq *rtf* (ros::tf-pose->coords (send msg :pose))) -;; (setq *rcoords* (make-coords :rot (copy-object (send *rtf* :rot)))) -;; (send *rcoords* :rotate pi/2 :x :world) -;; (send *rcoords* :locate (send *rtf* :worldpos) :world) (setq *rclose* (send msg :close_gripper)) (setq *rlocked* (send msg :locked)) ) - -;; Initialize -(ros::rate 10) (ros::subscribe "/left_device/phantom/state" omni_msgs::OmniState #'left-phantom-cb) (ros::subscribe "/right_device/phantom/state" omni_msgs::OmniState #'right-phantom-cb) @@ -86,7 +70,7 @@ ;; Reset robot (send *nextage* :reset-pose) -(send *nextage* :head :angle-vector #f(0 10)) +(send *nextage* :head :angle-vector #f(0 60)) (send *ri* :angle-vector (send *nextage* :angle-vector) 1000) (send *ri* :wait-interpolation) (send *irtviewer* :draw-objects) @@ -100,7 +84,9 @@ (do-until-key (ros::spin-once) - (when (and *lcoords* (not *llocked*)) + (setq ik-success t) + + (when (and *lpos* *lrot* (not *llocked*)) (setq *lx* (+ (* (send *lpos* :y) *ratio*) (elt *base-offset-l* 0))) (setq *ly* (+ (* (- 0 (send *lpos* :x)) *ratio*) (elt *base-offset-l* 1))) (setq *lz* (+ (* (send *lpos* :z) *ratio*) (elt *base-offset-l* 2))) @@ -110,9 +96,8 @@ (send *lrot* :z)))) (setq lR_dst (m* R_map lR_src)) (setq *larm-target* (make-coords :pos (float-vector *lx* *ly* *lz*) :rot lR_dst)) - (send *la-target* :move-to *larm-target* :world) - (send *la-end* :move-to (send (send *nextage* :larm :end-coords) :copy-worldcoords) :world) - (send *nextage* :larm :inverse-kinematics *larm-target* :use-torso *use-torso* :rotation-axis t :stop 4 :revert-if-fail nil)) + (unless (send *nextage* :larm :inverse-kinematics *larm-target* :use-torso *use-torso* :rotation-axis t :stop 10 :revert-if-fail t) + (setq ik-success nil))) (when (and *rpos* *rrot* (not *rlocked*)) (setq *rx* (+ (* (send *rpos* :y) *ratio*) (elt *base-offset-r* 0))) @@ -124,25 +109,18 @@ (send *rrot* :z)))) (setq rR_dst (m* R_map rR_src)) (setq *rarm-target* (make-coords :pos (float-vector *rx* *ry* *rz*) :rot rR_dst)) - (send *nextage* :rarm :inverse-kinematics *rarm-target* :use-torso *use-torso* :rotation-axis t :stop 4 :revert-if-fail nil)) + (unless (send *nextage* :rarm :inverse-kinematics *rarm-target* :use-torso *use-torso* :rotation-axis t :stop 10 :revert-if-fail t) + (setq ik-success nil))) - (send *ri* :angle-vector (send *nextage* :angle-vector) 100) + (when *lclose* (send *ri* :close-forceps :larm)) + (unless *lclose* (send *ri* :open-forceps :larm)) + (when *rclose* (send *ri* :close :rarm)) + (unless *rclose* (send *ri* :open-holder :rarm)) - (when (and *lclose* (not *llocked*)) - (if *lclose* - (send *lhand* :close) - (send *lhand* :open))) - - (when (and *rclose* (not *rlocked*)) - (if *rclose* - (send *rhand* :close-holder) - (send *rhand* :open-holder))) + (when (and ik-success (not (send *nextage* :self-collision-check))) + (send *ri* :angle-vector (send *nextage* :angle-vector) *send-time* nil 0 :min-time 0) + ) (send *irtviewer* :draw-objects) (ros::sleep) ) - -#| -Memo: Using the tf conversion method descripted in https://github.com/YUKINA-3252/jsk_hironx_teleop/issues/2, we cannot control the ration of :translate, -So I don't use it intentionally. -=# diff --git a/nextage_tutorials/euslisp/nextage-interface.l b/nextage_tutorials/euslisp/nextage-interface.l index 6c6e55ed..7bc9b00d 100644 --- a/nextage_tutorials/euslisp/nextage-interface.l +++ b/nextage_tutorials/euslisp/nextage-interface.l @@ -1,9 +1,14 @@ (load "package://pr2eus/robot-interface.l") (require :nextage "package://nextage_tutorials/nextage.l") +(when (ros::resolve-ros-path "package://dynamixel_detachable_hand/euslisp/lhand-interface.l") + (load "package://dynamixel_detachable_hand/euslisp/lhand-interface.l") + (load "package://dynamixel_detachable_hand/euslisp/rhand-interface.l")) + (defclass nextage-interface :super robot-interface :slots ()) + (defmethod nextage-interface (:init (&rest args) (send-super :init :robot nextageopen-robot)) @@ -41,6 +46,50 @@ (cons :controller-state "/head_controller/fstate") (cons :action-type control_msgs::FollowJointTrajectoryAction) (cons :joint-names (send-all (send robot :head :joint-list) :name))))) + (:lhand-init () + (unless (boundp '*lhand*) + (setq *lhand* (instance lhand-interface :init))) + *lhand*) + (:rhand-init () + (unless (boundp '*rhand*) + (setq *rhand* (instance rhand-interface :init))) + *rhand*) + (:start-grasp (&optional (arm :arms) &rest args) + (case arm + (:larm (send* (send self :lhand-init) :close args)) + (:rarm (send* (send self :rhand-init) :close args)) + (:arms (send* (send self :lhand-init) :close args) + (send* (send self :rhand-init) :close args)))) + (:stop-grasp (&optional (arm :arms) &rest args) + (case arm + (:larm (send* (send self :lhand-init) :open args)) + (:rarm (send* (send self :rhand-init) :open args)) + (:arms (send* (send self :lhand-init) :open args) + (send* (send self :rhand-init) :open args)))) + (:open-forceps (&optional (arm :arms) &rest args) + (case arm + (:larm (send* (send self :lhand-init) :open-forceps args)) + (:rarm (send* (send self :rhand-init) :open-forceps args)) + (:arms (send* (send self :lhand-init) :open-forceps args) + (send* (send self :rhand-init) :open-forceps args)))) + (:close-forceps (&optional (arm :arms) &rest args) + (case arm + (:larm (send* (send self :lhand-init) :close-forceps args)) + (:rarm (send* (send self :rhand-init) :close-forceps args)) + (:arms (send* (send self :lhand-init) :close-forceps args) + (send* (send self :rhand-init) :close-forceps args)))) + (:open-holder (&optional (arm :arms) &rest args) + (case arm + (:larm (send* (send self :lhand-init) :open-holder args)) + (:rarm (send* (send self :rhand-init) :open-holder args)) + (:arms (send* (send self :lhand-init) :open-holder args) + (send* (send self :rhand-init) :open-holder args)))) + (:close-holder (&optional (arm :arms) &rest args) + (case arm + (:larm (send* (send self :lhand-init) :close-holder args)) + (:rarm (send* (send self :rhand-init) :close-holder args)) + (:arms (send* (send self :lhand-init) :close-holder args) + (send* (send self :rhand-init) :close-holder args)))) ) (defun nextage-init () diff --git a/nextage_tutorials/euslisp/nextage-utils.l b/nextage_tutorials/euslisp/nextage-utils.l new file mode 100644 index 00000000..4030690c --- /dev/null +++ b/nextage_tutorials/euslisp/nextage-utils.l @@ -0,0 +1,52 @@ +(require :nextage "package://nextage_tutorials/nextage.l") +(require "package://nextage_tutorials/euslisp/nextage-interface.l") + +(defmethod NextageOpen-robot + (:inverse-kinematics + (target-coords &rest args &key (avoid-collision-distance 5) &allow-other-keys) + (let ((r) (original-av (send self :angle-vector))) + (setq r (send-super* :inverse-kinematics target-coords :avoid-collision-distance avoid-collision-distance :warnp nil :dump-command nil args)) + (unless r + (format *error-output* "; failed for normal ik, try to start from ik-friendly position~%") + (let* ((move-joints (send-all (cadr (memq :link-list args)) :joint)) + (av-after-first-ik (send self :angle-vector))) + (send self :angle-vector original-av) + (send self :reset-pose) + (send self :head :angle-vector #f(0 60)) + (send self :larm :move-end-pos #f(0 0 50)) + (send self :rarm :move-end-pos #f(0 0 50)) + (mapcar #'(lambda (j a) + (if (not (memq j move-joints)) + (send j :joint-angle a))) + (send self :joint-list) original-av) + (setq r (send-super* :inverse-kinematics target-coords :avoid-collision-distance avoid-collision-distance :warnp nil :dump-command nil args)) + (unless r + (if (and (memq :revert-if-fail args) (null (cadr (memq :revert-if-fail args)))) + (send self :angle-vector original-av) + (send self :angle-vector av-after-first-ik))))) + r)) + (:l/r-reverse + (av) + (let ((av-tmp (copy-list av))) + (setf (elt av-tmp 0) (- (elt av-tmp 0))) + (setf (elt av-tmp 3) (- (elt av-tmp 3))) + (setf (elt av-tmp 5) (- (elt av-tmp 5))) + av-tmp)) + (:self-collision-check (&rest args) + (let ((ret (send-super* :self-collision-check args))) + (when ret + (setq ret (remove-if #'(lambda (l) + (or (and (eq (car l) (send self :link "CHEST_JOINT0_Link")) + (eq (cdr l) (send self :link "HEAD_JOINT1_Link"))) + (and (eq (cdr l) (send self :link "CHEST_JOINT0_Link")) + (eq (car l) (send self :link "HEAD_JOINT1_Link"))) + (and (eq (car l) (send self :link "LARM_JOINT4_Link")) + (eq (cdr l) (send self :link "LARM_JOINT5_Link"))) + (and (eq (cdr l) (send self :link "LARM_JOINT4_Link")) + (eq (car l) (send self :link "LARM_JOINT5_Link"))) + (and (eq (car l) (send self :link "RARM_JOINT4_Link")) + (eq (cdr l) (send self :link "RARM_JOINT5_Link"))) + (and (eq (cdr l) (send self :link "RARM_JOINT4_Link")) + (eq (car l) (send self :link "RARM_JOINT5_Link"))))) + ret))) + ret))) diff --git a/nextage_tutorials/launch/d405_lhand.launch b/nextage_tutorials/launch/d405_lhand.launch new file mode 100644 index 00000000..2c8d425d --- /dev/null +++ b/nextage_tutorials/launch/d405_lhand.launch @@ -0,0 +1,151 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nextage_tutorials/launch/jsk_nextage_bringup.launch b/nextage_tutorials/launch/jsk_nextage_bringup.launch new file mode 100644 index 00000000..3abd354e --- /dev/null +++ b/nextage_tutorials/launch/jsk_nextage_bringup.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/nextage_tutorials/launch/jsk_nextageopen.launch b/nextage_tutorials/launch/jsk_nextageopen.launch deleted file mode 100644 index e1c9348e..00000000 --- a/nextage_tutorials/launch/jsk_nextageopen.launch +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/nextage_tutorials/launch/jsk_robot_state_publisher.launch b/nextage_tutorials/launch/jsk_robot_state_publisher.launch index 1e22a63d..6e90e73a 100644 --- a/nextage_tutorials/launch/jsk_robot_state_publisher.launch +++ b/nextage_tutorials/launch/jsk_robot_state_publisher.launch @@ -5,8 +5,8 @@ - + diff --git a/nextage_tutorials/launch/l515_head.launch b/nextage_tutorials/launch/l515_head.launch index ebac7dd0..ebf619bd 100644 --- a/nextage_tutorials/launch/l515_head.launch +++ b/nextage_tutorials/launch/l515_head.launch @@ -3,7 +3,7 @@ - + @@ -49,8 +49,8 @@ - - + + diff --git a/nextage_tutorials/launch/manual_tf_calib.launch b/nextage_tutorials/launch/manual_tf_calib.launch new file mode 100644 index 00000000..4c3f656a --- /dev/null +++ b/nextage_tutorials/launch/manual_tf_calib.launch @@ -0,0 +1,18 @@ + + + + object_type: cube + frame_id: LARM_JOINT5_Link + publish_tf: true + tf_frame: d405_lhand_link + object_x: 0.1 + object_y: 0.1 + object_z: 0.1 + initial_x: -0.06389 + initial_y: -0.04099 + initial_z: -0.0397 + initial_orientation: [-0.32028598357632193, -0.4779897040147819, -0.26448673000322787, 0.7739441202261368] + + + diff --git a/nextage_tutorials/package.xml b/nextage_tutorials/package.xml index da9a24b3..43fb9161 100644 --- a/nextage_tutorials/package.xml +++ b/nextage_tutorials/package.xml @@ -14,7 +14,8 @@ nextage_description robotiq_ft_sensor usb_cam - gravity_compensation + dynamixel_detachable_hand + diff --git a/nextage_tutorials/urdf/JSKNextageOpen.urdf b/nextage_tutorials/urdf/JSKNextageOpen.urdf index f21fe630..ff610a5f 100644 --- a/nextage_tutorials/urdf/JSKNextageOpen.urdf +++ b/nextage_tutorials/urdf/JSKNextageOpen.urdf @@ -1,3 +1,4 @@ + - - - - - - @@ -336,12 +331,6 @@ - - - - - - @@ -356,6 +345,7 @@ + @@ -386,7 +376,7 @@ - + @@ -442,6 +432,11 @@ + + + + + From 77360bcc8e88680866f12cd04798eb9ef33cd52e Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Tue, 11 Nov 2025 14:52:02 +0900 Subject: [PATCH 16/33] Remove lhand-init, rhand-init --- nextage_tutorials/euslisp/jsk-nextage-init.l | 2 -- 1 file changed, 2 deletions(-) diff --git a/nextage_tutorials/euslisp/jsk-nextage-init.l b/nextage_tutorials/euslisp/jsk-nextage-init.l index f8405f54..9fd25cc2 100644 --- a/nextage_tutorials/euslisp/jsk-nextage-init.l +++ b/nextage_tutorials/euslisp/jsk-nextage-init.l @@ -2,8 +2,6 @@ (require "package://nextage_tutorials/euslisp/nextage-utils.l") (nextage-init) -(lhand-init) -(rhand-init) (setq *robot* *nextage*) (objects (list *robot*)) From 2753a0c346095ef8b0890edeceaf3b59be46b333 Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Tue, 11 Nov 2025 17:08:11 +0900 Subject: [PATCH 17/33] Publish d405 TF --- nextage_tutorials/launch/d405_lhand.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nextage_tutorials/launch/d405_lhand.launch b/nextage_tutorials/launch/d405_lhand.launch index 2c8d425d..5235608a 100644 --- a/nextage_tutorials/launch/d405_lhand.launch +++ b/nextage_tutorials/launch/d405_lhand.launch @@ -52,7 +52,7 @@ - + From 9387927445b2720833f137b6e20321d1a7366f2b Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Tue, 11 Nov 2025 17:08:32 +0900 Subject: [PATCH 18/33] Fix d405 link --- nextage_tutorials/urdf/JSKNextageOpen.urdf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nextage_tutorials/urdf/JSKNextageOpen.urdf b/nextage_tutorials/urdf/JSKNextageOpen.urdf index ff610a5f..4052eff9 100644 --- a/nextage_tutorials/urdf/JSKNextageOpen.urdf +++ b/nextage_tutorials/urdf/JSKNextageOpen.urdf @@ -435,7 +435,7 @@ - + From 4b10aea7fd11d8b02f56a552c69aa6c133dd852d Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Tue, 11 Nov 2025 17:09:15 +0900 Subject: [PATCH 19/33] Remove calib file --- .../launch/manual_tf_calib.launch | 18 ------------------ 1 file changed, 18 deletions(-) delete mode 100644 nextage_tutorials/launch/manual_tf_calib.launch diff --git a/nextage_tutorials/launch/manual_tf_calib.launch b/nextage_tutorials/launch/manual_tf_calib.launch deleted file mode 100644 index 4c3f656a..00000000 --- a/nextage_tutorials/launch/manual_tf_calib.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - object_type: cube - frame_id: LARM_JOINT5_Link - publish_tf: true - tf_frame: d405_lhand_link - object_x: 0.1 - object_y: 0.1 - object_z: 0.1 - initial_x: -0.06389 - initial_y: -0.04099 - initial_z: -0.0397 - initial_orientation: [-0.32028598357632193, -0.4779897040147819, -0.26448673000322787, 0.7739441202261368] - - - From 9026d2cb5af7dfa5860117181342692a8764380d Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Wed, 12 Nov 2025 13:08:57 +0900 Subject: [PATCH 20/33] Refix d405 link --- nextage_tutorials/urdf/JSKNextageOpen.urdf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nextage_tutorials/urdf/JSKNextageOpen.urdf b/nextage_tutorials/urdf/JSKNextageOpen.urdf index 4052eff9..46304fe9 100644 --- a/nextage_tutorials/urdf/JSKNextageOpen.urdf +++ b/nextage_tutorials/urdf/JSKNextageOpen.urdf @@ -435,7 +435,7 @@ - + From 175955121bec2c0148020037f48c55d3f241827d Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Wed, 12 Nov 2025 13:10:36 +0900 Subject: [PATCH 21/33] Use full-body-controller in descripted in robot-interface when using real robot and use each limb controller when using gazebo simulator --- nextage_tutorials/euslisp/nextage-interface.l | 26 ++++++++++++++----- 1 file changed, 19 insertions(+), 7 deletions(-) diff --git a/nextage_tutorials/euslisp/nextage-interface.l b/nextage_tutorials/euslisp/nextage-interface.l index 7bc9b00d..918e8577 100644 --- a/nextage_tutorials/euslisp/nextage-interface.l +++ b/nextage_tutorials/euslisp/nextage-interface.l @@ -1,6 +1,8 @@ (load "package://pr2eus/robot-interface.l") +;; (load "package://hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l") (require :nextage "package://nextage_tutorials/nextage.l") +;; If dynamixel hand is available (when (ros::resolve-ros-path "package://dynamixel_detachable_hand/euslisp/lhand-interface.l") (load "package://dynamixel_detachable_hand/euslisp/lhand-interface.l") (load "package://dynamixel_detachable_hand/euslisp/rhand-interface.l")) @@ -11,13 +13,23 @@ (defmethod nextage-interface (:init (&rest args) - (send-super :init :robot nextageopen-robot)) - (:default-controller () - (append - (send self :larm-controller) - (send self :rarm-controller) - (send self :torso-controller) - (send self :head-controller))) + (send-super :init :robot nextageopen-robot) + (setq on-gazebo-ros-control + (and (ros::get-param "/gazebo/time_step" nil) + ;; rtm-ros-bridge does not have type parametrs + (ros::get-param "/torso_controller/type" nil))) + (when on-gazebo-ros-control + (ros::ros-warn "Found Gazebo/ros_control environment")) + ) + (when on-gazebo-ros-control + (:default-controller () + (append + (send self :larm-controller) + (send self :rarm-controller) + (send self :torso-controller) + (send self :head-controller)) + ) + ) (:larm-controller () (list (list From 2398ce5325c6a3773326710a8b1907f9cc660be1 Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Mon, 17 Nov 2025 19:45:28 +0900 Subject: [PATCH 22/33] Purge override ik --- nextage_tutorials/euslisp/nextage-utils.l | 30 ----------------------- 1 file changed, 30 deletions(-) diff --git a/nextage_tutorials/euslisp/nextage-utils.l b/nextage_tutorials/euslisp/nextage-utils.l index 4030690c..3f3c95d7 100644 --- a/nextage_tutorials/euslisp/nextage-utils.l +++ b/nextage_tutorials/euslisp/nextage-utils.l @@ -2,36 +2,6 @@ (require "package://nextage_tutorials/euslisp/nextage-interface.l") (defmethod NextageOpen-robot - (:inverse-kinematics - (target-coords &rest args &key (avoid-collision-distance 5) &allow-other-keys) - (let ((r) (original-av (send self :angle-vector))) - (setq r (send-super* :inverse-kinematics target-coords :avoid-collision-distance avoid-collision-distance :warnp nil :dump-command nil args)) - (unless r - (format *error-output* "; failed for normal ik, try to start from ik-friendly position~%") - (let* ((move-joints (send-all (cadr (memq :link-list args)) :joint)) - (av-after-first-ik (send self :angle-vector))) - (send self :angle-vector original-av) - (send self :reset-pose) - (send self :head :angle-vector #f(0 60)) - (send self :larm :move-end-pos #f(0 0 50)) - (send self :rarm :move-end-pos #f(0 0 50)) - (mapcar #'(lambda (j a) - (if (not (memq j move-joints)) - (send j :joint-angle a))) - (send self :joint-list) original-av) - (setq r (send-super* :inverse-kinematics target-coords :avoid-collision-distance avoid-collision-distance :warnp nil :dump-command nil args)) - (unless r - (if (and (memq :revert-if-fail args) (null (cadr (memq :revert-if-fail args)))) - (send self :angle-vector original-av) - (send self :angle-vector av-after-first-ik))))) - r)) - (:l/r-reverse - (av) - (let ((av-tmp (copy-list av))) - (setf (elt av-tmp 0) (- (elt av-tmp 0))) - (setf (elt av-tmp 3) (- (elt av-tmp 3))) - (setf (elt av-tmp 5) (- (elt av-tmp 5))) - av-tmp)) (:self-collision-check (&rest args) (let ((ret (send-super* :self-collision-check args))) (when ret From 51bc2f6f8bdb3aae4275c95f1dc70b5e297d2c59 Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Thu, 4 Dec 2025 17:17:49 +0900 Subject: [PATCH 23/33] Recalibrate --- nextage_tutorials/urdf/JSKNextageOpen.urdf | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nextage_tutorials/urdf/JSKNextageOpen.urdf b/nextage_tutorials/urdf/JSKNextageOpen.urdf index 46304fe9..f1de7427 100644 --- a/nextage_tutorials/urdf/JSKNextageOpen.urdf +++ b/nextage_tutorials/urdf/JSKNextageOpen.urdf @@ -376,7 +376,7 @@ - + @@ -435,7 +435,7 @@ - + From 5491dc5d880b88bd83c9097621c438950e781a59 Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Thu, 4 Dec 2025 17:18:34 +0900 Subject: [PATCH 24/33] Abolish robotiq FT300 --- nextage_tutorials/launch/jsk_nextage_bringup.launch | 3 --- 1 file changed, 3 deletions(-) diff --git a/nextage_tutorials/launch/jsk_nextage_bringup.launch b/nextage_tutorials/launch/jsk_nextage_bringup.launch index 3abd354e..689b60f4 100644 --- a/nextage_tutorials/launch/jsk_nextage_bringup.launch +++ b/nextage_tutorials/launch/jsk_nextage_bringup.launch @@ -6,9 +6,6 @@ - - - From ff161200e2ecb7f757c8057c1355cfacb41f9bed Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Thu, 4 Dec 2025 18:13:09 +0900 Subject: [PATCH 25/33] Use leptrino from slave --- .../launch/jsk_nextage_slave.launch | 19 +++++++++++++++++++ .../scripts/create_udev_rules.sh | 2 +- nextage_tutorials/udev/91-force_sensors.rules | 3 +++ nextage_tutorials/udev/99-rftsensor.rules | 1 - 4 files changed, 23 insertions(+), 2 deletions(-) create mode 100644 nextage_tutorials/launch/jsk_nextage_slave.launch create mode 100644 nextage_tutorials/udev/91-force_sensors.rules delete mode 100644 nextage_tutorials/udev/99-rftsensor.rules diff --git a/nextage_tutorials/launch/jsk_nextage_slave.launch b/nextage_tutorials/launch/jsk_nextage_slave.launch new file mode 100644 index 00000000..e324d50d --- /dev/null +++ b/nextage_tutorials/launch/jsk_nextage_slave.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/nextage_tutorials/scripts/create_udev_rules.sh b/nextage_tutorials/scripts/create_udev_rules.sh index 41256ab2..eeb3377b 100755 --- a/nextage_tutorials/scripts/create_udev_rules.sh +++ b/nextage_tutorials/scripts/create_udev_rules.sh @@ -5,7 +5,7 @@ echo "This scripts copies udev rules for dual hands to /etc/udev/rules.d" echo "" sudo cp `rospack find nextage_tutorials`/udev/99-elp-camera.rules /etc/udev/rules.d -sudo cp `rospack find nextage_tutorials`/udev/99-rftsensor.rules /etc/udev/rules.d +sudo cp `rospack find nextage_tutorials`/udev/91-force_sensors.rules /etc/udev/rules.d echo "" echo "Restarting udev" echo "" diff --git a/nextage_tutorials/udev/91-force_sensors.rules b/nextage_tutorials/udev/91-force_sensors.rules new file mode 100644 index 00000000..c02ff181 --- /dev/null +++ b/nextage_tutorials/udev/91-force_sensors.rules @@ -0,0 +1,3 @@ +SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", ATTRS{serial}=="2510085", SYMLINK+="left_ft_sensor", MODE="0666" + +SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", ATTRS{serial}=="2510087", SYMLINK+="right_ft_sensor", MODE="0666" diff --git a/nextage_tutorials/udev/99-rftsensor.rules b/nextage_tutorials/udev/99-rftsensor.rules deleted file mode 100644 index 68a113e5..00000000 --- a/nextage_tutorials/udev/99-rftsensor.rules +++ /dev/null @@ -1 +0,0 @@ -SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6015", ATTRS{serial}=="DAK2EOTK", SYMLINK+="rftsensor", MODE="0666" From 33b2d8f7e32cd9e42a9df68fb61ca5a071ba3986 Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Fri, 5 Dec 2025 13:55:48 +0900 Subject: [PATCH 26/33] Maintain force sensor static calibration --- nextage_tutorials/CMakeLists.txt | 18 ++-- nextage_tutorials/config/ft_calib_data.yaml | 16 --- nextage_tutorials/config/ft_left.yaml | 5 + nextage_tutorials/config/ft_right.yaml | 5 + .../launch/force_sensor_calib.launch | 24 +++++ .../launch/ft_sensor_calib.launch | 33 ------- ...ingup.launch => jsk_nextage_master.launch} | 4 +- .../launch/jsk_nextage_slave.launch | 22 +++++ .../launch/right_ft_sensor.launch | 7 -- nextage_tutorials/scripts/ft_gravity_calib.py | 99 +++++++++++++++++++ .../scripts/ft_gravity_compensate.py | 71 +++++++++++++ nextage_tutorials/scripts/tf_to_imu.py | 29 ------ 12 files changed, 239 insertions(+), 94 deletions(-) delete mode 100644 nextage_tutorials/config/ft_calib_data.yaml create mode 100644 nextage_tutorials/config/ft_left.yaml create mode 100644 nextage_tutorials/config/ft_right.yaml create mode 100644 nextage_tutorials/launch/force_sensor_calib.launch delete mode 100644 nextage_tutorials/launch/ft_sensor_calib.launch rename nextage_tutorials/launch/{jsk_nextage_bringup.launch => jsk_nextage_master.launch} (75%) delete mode 100644 nextage_tutorials/launch/right_ft_sensor.launch create mode 100644 nextage_tutorials/scripts/ft_gravity_calib.py create mode 100644 nextage_tutorials/scripts/ft_gravity_compensate.py delete mode 100644 nextage_tutorials/scripts/tf_to_imu.py diff --git a/nextage_tutorials/CMakeLists.txt b/nextage_tutorials/CMakeLists.txt index bd7c4fc5..3ad99d7e 100644 --- a/nextage_tutorials/CMakeLists.txt +++ b/nextage_tutorials/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(nextage_tutorials) find_package(catkin REQUIRED @@ -38,14 +38,16 @@ endif() ############# ## Install ## ############# -catkin_install_python(PROGRAMS - scripts/tf_to_imu.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +file(GLOB_RECURSE SCRIPTS_FILES *.py) +catkin_install_python( + PROGRAMS ${SCRIPTS_FILES} + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/scripts ) - -install(PROGRAMS - scripts/create_udev_rules.sh - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(DIRECTORY scripts + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS + PATTERN "*" + PATTERN "*/*.py" EXCLUDE ) install(DIRECTORY config euslisp launch model sample udev urdf diff --git a/nextage_tutorials/config/ft_calib_data.yaml b/nextage_tutorials/config/ft_calib_data.yaml deleted file mode 100644 index aee443a1..00000000 --- a/nextage_tutorials/config/ft_calib_data.yaml +++ /dev/null @@ -1,16 +0,0 @@ -bias: - - 0.0 - - 0.0 - - 0.0 - - 0.0 - - -0.08200792164801718 - - 0.0 -gripper_mass: 0.8180173125445871 -gripper_com_pose: - - 0.002033092466916677 - - -0.002542349869443385 - - 0.08879802276111172 - - 0 - - 0 - - 0 -gripper_com_frame_id: lwr_7_link \ No newline at end of file diff --git a/nextage_tutorials/config/ft_left.yaml b/nextage_tutorials/config/ft_left.yaml new file mode 100644 index 00000000..c5aea572 --- /dev/null +++ b/nextage_tutorials/config/ft_left.yaml @@ -0,0 +1,5 @@ +bias: +- -1.529194312431732 +- 0.16152044014872324 +- 3.6313240850868413 +mass: -0.04809517084814864 diff --git a/nextage_tutorials/config/ft_right.yaml b/nextage_tutorials/config/ft_right.yaml new file mode 100644 index 00000000..fc2c94da --- /dev/null +++ b/nextage_tutorials/config/ft_right.yaml @@ -0,0 +1,5 @@ +bias: +- 0.03389291443670828 +- -0.5215328703469484 +- 2.877995258470721 +mass: 0.006041227854279071 diff --git a/nextage_tutorials/launch/force_sensor_calib.launch b/nextage_tutorials/launch/force_sensor_calib.launch new file mode 100644 index 00000000..3b83513a --- /dev/null +++ b/nextage_tutorials/launch/force_sensor_calib.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + diff --git a/nextage_tutorials/launch/ft_sensor_calib.launch b/nextage_tutorials/launch/ft_sensor_calib.launch deleted file mode 100644 index c40e5b57..00000000 --- a/nextage_tutorials/launch/ft_sensor_calib.launch +++ /dev/null @@ -1,33 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/nextage_tutorials/launch/jsk_nextage_bringup.launch b/nextage_tutorials/launch/jsk_nextage_master.launch similarity index 75% rename from nextage_tutorials/launch/jsk_nextage_bringup.launch rename to nextage_tutorials/launch/jsk_nextage_master.launch index 689b60f4..6e27ec87 100644 --- a/nextage_tutorials/launch/jsk_nextage_bringup.launch +++ b/nextage_tutorials/launch/jsk_nextage_master.launch @@ -8,5 +8,7 @@ - + + + diff --git a/nextage_tutorials/launch/jsk_nextage_slave.launch b/nextage_tutorials/launch/jsk_nextage_slave.launch index e324d50d..52f2b1c2 100644 --- a/nextage_tutorials/launch/jsk_nextage_slave.launch +++ b/nextage_tutorials/launch/jsk_nextage_slave.launch @@ -14,6 +14,28 @@ + + + + + + + + + + + + + + + + diff --git a/nextage_tutorials/launch/right_ft_sensor.launch b/nextage_tutorials/launch/right_ft_sensor.launch deleted file mode 100644 index d676021f..00000000 --- a/nextage_tutorials/launch/right_ft_sensor.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/nextage_tutorials/scripts/ft_gravity_calib.py b/nextage_tutorials/scripts/ft_gravity_calib.py new file mode 100644 index 00000000..5e69df20 --- /dev/null +++ b/nextage_tutorials/scripts/ft_gravity_calib.py @@ -0,0 +1,99 @@ +#!/usr/bin/env python + +import rospy +import tf +import numpy as np +import yaml +from geometry_msgs.msg import WrenchStamped + + +class FTGravityCalib: + def __init__(self): + self.ft_topic = rospy.get_param("~ft_topic", "/left_ft/left_ft/force_torque") + self.sensor_frame = rospy.get_param("~sensor_frame", "LARM_JOINT5_Link") + self.base_frame = rospy.get_param("~base_frame", "WAIST") + + self.output_yaml = rospy.get_param("~yaml_path", "ft_gravity.yaml") + + self.listener = tf.TransformListener() + + self.ft_data = [] + self.gravity = np.array([0, 0, -9.81]) + + rospy.Subscriber(self.ft_topic, WrenchStamped, self.cb_ft) + rospy.on_shutdown(self.solve) + + rospy.loginfo("=== FT Gravity Calibration Started ===") + rospy.loginfo("Collecting FT data... (Ctrl+C to finish)") + rospy.loginfo("Will save to: %s" % self.output_yaml) + + def cb_ft(self, msg): + """Record FT and gravity direction vector.""" + F = np.array([ + msg.wrench.force.x, + msg.wrench.force.y, + msg.wrench.force.z + ]) + + try: + (trans, rot) = self.listener.lookupTransform( + self.sensor_frame, # target + self.base_frame, # source + rospy.Time(0) + ) + except (tf.LookupException, tf.ExtrapolationException): + return + + R = tf.transformations.quaternion_matrix(rot)[:3, :3] + + g_sensor = R.dot(self.gravity) + + self.ft_data.append((F, g_sensor)) + + if len(self.ft_data) % 50 == 0: + rospy.loginfo("Collected samples: %d" % len(self.ft_data)) + + def solve(self): + if len(self.ft_data) == 0: + rospy.logwarn("No data collected. YAML not saved.") + return + + rospy.loginfo("Solving least squares...") + + # F = m*g + bias + A = [] + b = [] + + for F, g in self.ft_data: + A.append([g[0], 1, 0, 0]) + A.append([g[1], 0, 1, 0]) + A.append([g[2], 0, 0, 1]) + b.extend(F) + + A = np.array(A, dtype=float) + b = np.array(b, dtype=float) + + x, _, _, _ = np.linalg.lstsq(A, b, rcond=None) + + mass = float(x[0]) + bias = x[1:4].tolist() + + rospy.loginfo("Estimated mass: %.4f kg" % mass) + rospy.loginfo("Estimated bias: %s" % bias) + + calib = {"mass": mass, "bias": bias} + + try: + import os + os.makedirs(os.path.dirname(self.output_yaml), exist_ok=True) + except: + pass + with open(self.output_yaml, "w") as f: + yaml.safe_dump(calib, f, default_flow_style=False) + rospy.loginfo("Saved calibration → %s" % self.output_yaml) + + +if __name__ == "__main__": + rospy.init_node("ft_gravity_calib") + FTGravityCalib() + rospy.spin() diff --git a/nextage_tutorials/scripts/ft_gravity_compensate.py b/nextage_tutorials/scripts/ft_gravity_compensate.py new file mode 100644 index 00000000..e825aa58 --- /dev/null +++ b/nextage_tutorials/scripts/ft_gravity_compensate.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python +import rospy +import tf +import numpy as np +from geometry_msgs.msg import WrenchStamped + + +class FTGravityCompensate: + def __init__(self): + self.ft_topic = rospy.get_param("~input_topic") + self.out_topic = rospy.get_param("~output_topic") + self.sensor_frame = rospy.get_param("~sensor_frame") + self.base_frame = rospy.get_param("~base_frame") + + self.mass = rospy.get_param("~mass") + self.bias = np.array(rospy.get_param("~bias")) # [bx, by, bz] + + self.gravity = np.array([0, 0, -9.81]) + + self.listener = tf.TransformListener() + self.pub = rospy.Publisher(self.out_topic, WrenchStamped, queue_size=10) + + rospy.Subscriber(self.ft_topic, WrenchStamped, self.cb) + + rospy.loginfo("=== FT Gravity Compensation Started ===") + rospy.loginfo("input_topic: %s" % self.ft_topic) + rospy.loginfo("output_topic: %s" % self.out_topic) + rospy.loginfo("mass: %.4f" % self.mass) + rospy.loginfo("bias: %s" % self.bias.tolist()) + + def cb(self, msg): + F = np.array([ + msg.wrench.force.x, + msg.wrench.force.y, + msg.wrench.force.z + ]) + + try: + (_, rot) = self.listener.lookupTransform( + self.sensor_frame, # target + self.base_frame, # source + rospy.Time(0) + ) + except (tf.LookupException, tf.ExtrapolationException): + return + + R = tf.transformations.quaternion_matrix(rot)[:3, :3] + + g_sensor = R.dot(self.gravity) + + Fg = self.mass * g_sensor + + Fcorr = F - Fg - self.bias + + out = WrenchStamped() + out.header = msg.header + out.wrench.force.x = Fcorr[0] + out.wrench.force.y = Fcorr[1] + out.wrench.force.z = Fcorr[2] + + out.wrench.torque.x = msg.wrench.torque.x + out.wrench.torque.y = msg.wrench.torque.y + out.wrench.torque.z = msg.wrench.torque.z + + self.pub.publish(out) + + +if __name__ == "__main__": + rospy.init_node("ft_gravity_compensate") + node = FTGravityCompensate() + rospy.spin() diff --git a/nextage_tutorials/scripts/tf_to_imu.py b/nextage_tutorials/scripts/tf_to_imu.py deleted file mode 100644 index 8c097cd7..00000000 --- a/nextage_tutorials/scripts/tf_to_imu.py +++ /dev/null @@ -1,29 +0,0 @@ -#!/usr/bin/env python3 - -import rospy -import tf2_ros -from sensor_msgs.msg import Imu - -if __name__ == '__main__': - rospy.init_node('tf_to_imu') - - base_frame = rospy.get_param("~base_frame", "CHEST_JOINT0_Link") - sensor_frame = rospy.get_param("~sensor_frame", "RARM_JOINT5_Link") - imu_topic = rospy.get_param("~imu_topic", "/imu/data") - - tf_buffer = tf2_ros.Buffer() - listener = tf2_ros.TransformListener(tf_buffer) - pub = rospy.Publisher(imu_topic, Imu, queue_size=10) - - rate = rospy.Rate(100) - while not rospy.is_shutdown(): - try: - trans = tf_buffer.lookup_transform(base_frame, sensor_frame, rospy.Time(0)) - imu_msg = Imu() - imu_msg.header.stamp = rospy.Time.now() - imu_msg.header.frame_id = sensor_frame - imu_msg.orientation = trans.transform.rotation - pub.publish(imu_msg) - except Exception as e: - rospy.logwarn_throttle(5.0, f"TF not available yet: {e}") - rate.sleep() From b3f5f2fb84896337a393c625f1ef275edc9dd219 Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Fri, 19 Dec 2025 13:56:34 +0900 Subject: [PATCH 27/33] Add go to power off pose --- nextage_tutorials/euslisp/jsk-nextage-go-to-off-pose.l | 8 ++++++++ 1 file changed, 8 insertions(+) create mode 100644 nextage_tutorials/euslisp/jsk-nextage-go-to-off-pose.l diff --git a/nextage_tutorials/euslisp/jsk-nextage-go-to-off-pose.l b/nextage_tutorials/euslisp/jsk-nextage-go-to-off-pose.l new file mode 100644 index 00000000..d637b70a --- /dev/null +++ b/nextage_tutorials/euslisp/jsk-nextage-go-to-off-pose.l @@ -0,0 +1,8 @@ +#!/usr/bin/env roseus + +(load "nextage-utils.l") +(nextage-init) +(send *nextage* :angle-vector #f(0.0 0.0 -140.0 -158.0 0.0 0.0 0.0 0.0 -140.0 -158.0 0.0 0.0 0.0 0.0 0.0)) +(send *ri* :angle-vector (send *nextage* :angle-vector) 15000) + +;; TODO : Slave側でhrpsysとopenrtmがbuildできていないので，(send *ri* :servo-off) するためのserviceに送れない． From 9a063cf4eb8c99271f9e064109e5a290d34229e3 Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Fri, 19 Dec 2025 15:47:22 +0900 Subject: [PATCH 28/33] Update sensor offset --- nextage_tutorials/config/ft_left.yaml | 8 ++++---- nextage_tutorials/config/ft_right.yaml | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/nextage_tutorials/config/ft_left.yaml b/nextage_tutorials/config/ft_left.yaml index c5aea572..5c0b0f12 100644 --- a/nextage_tutorials/config/ft_left.yaml +++ b/nextage_tutorials/config/ft_left.yaml @@ -1,5 +1,5 @@ bias: -- -1.529194312431732 -- 0.16152044014872324 -- 3.6313240850868413 -mass: -0.04809517084814864 +- -0.8155106901261817 +- -0.7125817164245061 +- 3.132602122664439 +mass: -0.0232952914366634 diff --git a/nextage_tutorials/config/ft_right.yaml b/nextage_tutorials/config/ft_right.yaml index fc2c94da..fdb77d56 100644 --- a/nextage_tutorials/config/ft_right.yaml +++ b/nextage_tutorials/config/ft_right.yaml @@ -1,5 +1,5 @@ bias: -- 0.03389291443670828 -- -0.5215328703469484 -- 2.877995258470721 -mass: 0.006041227854279071 +- 1.8482932369101124 +- 0.43075233493892795 +- 2.5646278298134493 +mass: -0.02404630150911229 From 9b0d4d27af18412df78f0723744a08546b6778aa Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Fri, 19 Dec 2025 23:34:37 +0900 Subject: [PATCH 29/33] Update go to init pose time --- nextage_tutorials/euslisp/jsk-nextage-init.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nextage_tutorials/euslisp/jsk-nextage-init.l b/nextage_tutorials/euslisp/jsk-nextage-init.l index 9fd25cc2..cdd58c56 100644 --- a/nextage_tutorials/euslisp/jsk-nextage-init.l +++ b/nextage_tutorials/euslisp/jsk-nextage-init.l @@ -9,7 +9,7 @@ (send *robot* :head :angle-vector #f(0 60)) (send *robot* :larm :move-end-pos #f(0 0 100)) (send *robot* :rarm :move-end-pos #f(0 0 100)) -(send *ri* :angle-vector (send *nextage* :angle-vector) 2000) +(send *ri* :angle-vector (send *nextage* :angle-vector) 10000) (send *ri* :wait-interpolation) (send *ri* :close-holder :rarm) (send *ri* :open-forceps :larm) From 401584ed2036173f24165f18f641de68ebdf6113 Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Sat, 10 Jan 2026 23:02:44 +0900 Subject: [PATCH 30/33] Add dynamxiel connection monitoring --- nextage_tutorials/launch/jsk_nextage_slave.launch | 3 +++ 1 file changed, 3 insertions(+) diff --git a/nextage_tutorials/launch/jsk_nextage_slave.launch b/nextage_tutorials/launch/jsk_nextage_slave.launch index 52f2b1c2..fdfe3149 100644 --- a/nextage_tutorials/launch/jsk_nextage_slave.launch +++ b/nextage_tutorials/launch/jsk_nextage_slave.launch @@ -38,4 +38,7 @@ + + From d00d8cea0b3964066c08757b0481fc7c8863a306 Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Sat, 10 Jan 2026 23:03:08 +0900 Subject: [PATCH 31/33] Change left hand to usb_cam --- .../launch/jsk_nextage_master.launch | 6 +- nextage_tutorials/launch/lhand_camera.launch | 55 +++++++++++++++++++ 2 files changed, 58 insertions(+), 3 deletions(-) create mode 100644 nextage_tutorials/launch/lhand_camera.launch diff --git a/nextage_tutorials/launch/jsk_nextage_master.launch b/nextage_tutorials/launch/jsk_nextage_master.launch index 6e27ec87..54b64242 100644 --- a/nextage_tutorials/launch/jsk_nextage_master.launch +++ b/nextage_tutorials/launch/jsk_nextage_master.launch @@ -1,9 +1,9 @@ - + + diff --git a/nextage_tutorials/launch/lhand_camera.launch b/nextage_tutorials/launch/lhand_camera.launch new file mode 100644 index 00000000..d47bf4a4 --- /dev/null +++ b/nextage_tutorials/launch/lhand_camera.launch @@ -0,0 +1,55 @@ + + + + + + start_service_name: "start_capture" + stop_service_name: "stop_capture" + + video_device: /dev/lhand_camera + io_method: mmap + pixel_format: mjpeg + color_format: yuv422p + create_suspended: false + full_ffmpeg_log: false + + camera_name: lhand_camera + camera_frame_id: d405_lhand + camera_transport_suffix: image_raw + camera_info_url: "" + + image_width: 640 + image_height: 480 + framerate: 30 + + intrinsic_controls: + focus_auto: true + exposure_auto_priority: true + exposure_auto: 3 + white_balance_temperature_auto: true + power_line_frequency: 1 + ignore: + - brightness + - contrast + - saturation + - gain + - sharpness + - backlight_compensation + - white_balance_temperature + - exposure_absolute + - pan_absolute + - tilt_absolute + - focus_absolute + - zoom_absolute + + + + + + + + From 62ab664c2ffac92e9d4ab77c5c32bb92fb9bdd4e Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Sat, 10 Jan 2026 23:04:24 +0900 Subject: [PATCH 32/33] Add hand interface for scissors --- nextage_tutorials/euslisp/nextage-interface.l | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/nextage_tutorials/euslisp/nextage-interface.l b/nextage_tutorials/euslisp/nextage-interface.l index 918e8577..5705e087 100644 --- a/nextage_tutorials/euslisp/nextage-interface.l +++ b/nextage_tutorials/euslisp/nextage-interface.l @@ -102,6 +102,18 @@ (:rarm (send* (send self :rhand-init) :close-holder args)) (:arms (send* (send self :lhand-init) :close-holder args) (send* (send self :rhand-init) :close-holder args)))) + (:open-scissors (&optional (arm :arms) &rest args) + (case arm + (:larm (send* (send self :lhand-init) :open-scissors args)) + (:rarm (send* (send self :rhand-init) :open-scissors args)) + (:arms (send* (send self :lhand-init) :open-scissors args) + (send* (send self :rhand-init) :open-scissors args)))) + (:close-scissors (&optional (arm :arms) &rest args) + (case arm + (:larm (send* (send self :lhand-init) :close-scissors args)) + (:rarm (send* (send self :rhand-init) :close-scissors args)) + (:arms (send* (send self :lhand-init) :close-scissors args) + (send* (send self :rhand-init) :close-scissors args)))) ) (defun nextage-init () From 6371be098cb92a1fa47c4cc4d5bf1c0d6004c2a9 Mon Sep 17 00:00:00 2001 From: Michitoshi Tsubaki Date: Sat, 10 Jan 2026 23:04:57 +0900 Subject: [PATCH 33/33] Add new camera --- nextage_tutorials/udev/99-elp-camera.rules | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/nextage_tutorials/udev/99-elp-camera.rules b/nextage_tutorials/udev/99-elp-camera.rules index 0f8700c2..5b8f507f 100644 --- a/nextage_tutorials/udev/99-elp-camera.rules +++ b/nextage_tutorials/udev/99-elp-camera.rules @@ -1 +1,3 @@ -SUBSYSTEM=="video4linux", ATTRS{idVendor}=="32e4", ATTRS{idProduct}=="4802", ATTRS{serial}=="2023081002", SYMLINK+="elp01", MODE="0666" \ No newline at end of file +SUBSYSTEM=="video4linux", ATTRS{idVendor}=="32e4", ATTRS{idProduct}=="4802", ATTRS{serial}=="2023081002", SYMLINK+="elp01", MODE="0666" + +SUBSYSTEM=="video4linux", ATTR{index}=="0", ATTRS{idVendor}=="0bda", ATTRS{idProduct}=="5846", ATTRS{serial}=="200901010001", SYMLINK+="lhand_camera" MODE="0666"