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: 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 @@ + diff --git a/hrpsys_ros_bridge_tutorials/CMakeLists.txt b/hrpsys_ros_bridge_tutorials/CMakeLists.txt index 5e5d8a00..dfd51bfb 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" @@ -521,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) @@ -532,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") @@ -540,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() @@ -566,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/euslisp/a0b-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/a0b-interface.l new file mode 100644 index 00000000..d42f2650 --- /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 :a0b "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/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 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..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: @@ -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..9298238a 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__": @@ -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,16 +43,16 @@ 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:' - for limb, eef_info in g_eef_info_list.items(): - print '%s: %s' % (limb, eef_info) + print('eef_infos param list:') + for limb, eef_info in list(g_eef_info_list.items()): + print('%s: %s' % (limb, eef_info)) while not rospy.is_shutdown(): publishEndEffectorAll() 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/hrp2_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py index 3bf0d267..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 @@ -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() @@ -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] @@ -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 91eb4d0f..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 @@ -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']] @@ -141,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 @@ -277,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] @@ -382,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] @@ -469,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] @@ -478,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 @@ -542,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] @@ -621,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] @@ -685,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* @@ -804,6 +807,12 @@ def tablisLoadTestPose (self): def tablisInitPose (self): return [0]*len(self.tablisResetPose()) + def a0bResetPose(self): + 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.a0bResetPose()) + # (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": @@ -890,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") diff --git a/nextage_tutorials/.gitignore b/nextage_tutorials/.gitignore new file mode 100644 index 00000000..e52fbf20 --- /dev/null +++ b/nextage_tutorials/.gitignore @@ -0,0 +1,5 @@ +nextage.dae +nextage.l +launch/ft_sensor_calib.launch +launch/manual_tf_calib.launch +scripts/tf_to_imu.py 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/jsk-nextage-init.l b/nextage_tutorials/euslisp/jsk-nextage-init.l new file mode 100644 index 00000000..9fd25cc2 --- /dev/null +++ b/nextage_tutorials/euslisp/jsk-nextage-init.l @@ -0,0 +1,16 @@ +#!/usr/bin/env roseus + +(require "package://nextage_tutorials/euslisp/nextage-utils.l") +(nextage-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-teleop.l b/nextage_tutorials/euslisp/jsk-nextage-teleop.l new file mode 100644 index 00000000..d858aa59 --- /dev/null +++ b/nextage_tutorials/euslisp/jsk-nextage-teleop.l @@ -0,0 +1,126 @@ +#!/usr/bin/env roseus + +(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-utils.l") + +;; Parameters +(setq *use-torso* nil) +(setq *use-hand* nil) +(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) + (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 left-phantom-cb (msg) + (setq *lpos* (send msg :pose :position)) + (setq *lrot* (send msg :pose :orientation)) + (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 *rclose* (send msg :close_gripper)) + (setq *rlocked* (send msg :locked)) + ) + +(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 60)) +(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) + + (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))) + (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)) + (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))) + (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)) + (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))) + + (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 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) + ) diff --git a/nextage_tutorials/euslisp/nextage-interface.l b/nextage_tutorials/euslisp/nextage-interface.l index 6c6e55ed..918e8577 100644 --- a/nextage_tutorials/euslisp/nextage-interface.l +++ b/nextage_tutorials/euslisp/nextage-interface.l @@ -1,18 +1,35 @@ (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")) + (defclass nextage-interface :super robot-interface :slots ()) + (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 @@ -41,6 +58,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..3f3c95d7 --- /dev/null +++ b/nextage_tutorials/euslisp/nextage-utils.l @@ -0,0 +1,22 @@ +(require :nextage "package://nextage_tutorials/nextage.l") +(require "package://nextage_tutorials/euslisp/nextage-interface.l") + +(defmethod NextageOpen-robot + (: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..5235608a --- /dev/null +++ b/nextage_tutorials/launch/d405_lhand.launch @@ -0,0 +1,151 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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_camera.launch b/nextage_tutorials/launch/elp_camera.launch new file mode 100644 index 00000000..3b0f1334 --- /dev/null +++ b/nextage_tutorials/launch/elp_camera.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/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_robot_state_publisher.launch b/nextage_tutorials/launch/jsk_robot_state_publisher.launch new file mode 100644 index 00000000..6e90e73a --- /dev/null +++ b/nextage_tutorials/launch/jsk_robot_state_publisher.launch @@ -0,0 +1,12 @@ + + + + + + + + + + diff --git a/nextage_tutorials/launch/l515_head.launch b/nextage_tutorials/launch/l515_head.launch new file mode 100644 index 00000000..ebf619bd --- /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..d676021f --- /dev/null +++ b/nextage_tutorials/launch/right_ft_sensor.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/nextage_tutorials/package.xml b/nextage_tutorials/package.xml index 6fc36136..43fb9161 100644 --- a/nextage_tutorials/package.xml +++ b/nextage_tutorials/package.xml @@ -12,7 +12,11 @@ pr2eus euscollada nextage_description - nextage_gazebo + robotiq_ft_sensor + usb_cam + dynamixel_detachable_hand + + diff --git a/nextage_tutorials/scripts/create_udev_rules.sh b/nextage_tutorials/scripts/create_udev_rules.sh new file mode 100755 index 00000000..41256ab2 --- /dev/null +++ b/nextage_tutorials/scripts/create_udev_rules.sh @@ -0,0 +1,16 @@ +#!/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 +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/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/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" diff --git a/nextage_tutorials/urdf/JSKNextageOpen.urdf b/nextage_tutorials/urdf/JSKNextageOpen.urdf new file mode 100644 index 00000000..46304fe9 --- /dev/null +++ b/nextage_tutorials/urdf/JSKNextageOpen.urdf @@ -0,0 +1,507 @@ + + + + + + + + Gazebo/${color} + + + + + + transmission_interface/SimpleTransmission + + EffortJointInterface + + + EffortJointInterface + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + / + gazebo_ros_control/DefaultRobotHWSim + + + +