From ed8c0fa3d972538e3f0cf8b01b73167af1bb7dfc Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Mon, 12 Jun 2017 15:48:44 +0900 Subject: [PATCH 01/24] Add baxter model with right gripper-v6 --- jsk_arc2017_baxter/.gitignore | 3 + jsk_arc2017_baxter/robots/baxter.xacro | 15 + jsk_arc2017_baxter/robots/baxter.yaml | 75 ++++ .../robots/right_vacuum_gripper.xacro | 385 ++++++++++++++++++ 4 files changed, 478 insertions(+) create mode 100644 jsk_arc2017_baxter/.gitignore create mode 100644 jsk_arc2017_baxter/robots/baxter.xacro create mode 100644 jsk_arc2017_baxter/robots/baxter.yaml create mode 100644 jsk_arc2017_baxter/robots/right_vacuum_gripper.xacro diff --git a/jsk_arc2017_baxter/.gitignore b/jsk_arc2017_baxter/.gitignore new file mode 100644 index 000000000..323250322 --- /dev/null +++ b/jsk_arc2017_baxter/.gitignore @@ -0,0 +1,3 @@ +robots/baxter.urdf +robots/baxter.dae +robots/baxter.l diff --git a/jsk_arc2017_baxter/robots/baxter.xacro b/jsk_arc2017_baxter/robots/baxter.xacro new file mode 100644 index 000000000..5e1bac392 --- /dev/null +++ b/jsk_arc2017_baxter/robots/baxter.xacro @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/jsk_arc2017_baxter/robots/baxter.yaml b/jsk_arc2017_baxter/robots/baxter.yaml new file mode 100644 index 000000000..da9a6e7f4 --- /dev/null +++ b/jsk_arc2017_baxter/robots/baxter.yaml @@ -0,0 +1,75 @@ +## +## - collada_joint_name : euslisp_joint_name (start with :) +## + +head: + - head_pan : head-neck-y +larm: + - left_s0 : larm-shoulder-y + - left_s1 : larm-shoulder-p + - left_e0 : larm-shoulder-r + - left_e1 : larm-elbow-p + - left_w0 : larm-elbow-r + - left_w1 : larm-wrist-p + - left_w2 : larm-wrist-r + - left_gripper_vacuum_pad_joint : larm-gripper-p +rarm: + - right_s0 : rarm-shoulder-y + - right_s1 : rarm-shoulder-p + - right_e0 : rarm-shoulder-r + - right_e1 : rarm-elbow-p + - right_w0 : rarm-elbow-r + - right_w1 : rarm-wrist-p + - right_w2 : rarm-wrist-r + - right_gripper_prismatic_joint : rarm-gripper-x + - right_gripper_vacuum_pad_joint : rarm-gripper-p + +angle-vector: + reset-pose : [0, 20, -25, -40, 60, -20, 80, 0, 0, -20, -25, 40, 60, 20, 80, 0, 0, 0, 0] + reset-manip-pose : [0, -5, -57, -68, 111, 38, 59, 0, 0, 5, -57, 68, 111, -38, 59, 0, 0, 0] + untuck-pose : [0, -5, -57, -68, 111, 38, 59, 0, 0, 5, -57, 68, 111, -38, 59, 0, 0, 0] # this is reset-manip-pose + tuck-pose : [0, -57, -119, 152, 146, 0, 0, 0, 0, 57, -119, -152, 136, 0, 0, 0, 0, 0] + fold-pose-upper: [0, -35.6955, -0.550063, -174.754, 89.2461, 169.141, 85.136, -4.69734, 90, 35.6955, -0.550063, 174.754, 89.2461, -169.141, 85.136, 4.69734, 0, 90] + fold-pose-middle: [0, 48.8672, -10.437, -80.6616, 148.645, 80.9033, 1.38428, 18.8745, 90, -48.8672, -10.437, 80.6616, 148.645, -80.9033, 1.38428, -18.8745, 0, 90] + fold-pose-lower: [0, -7.27295, 52.6465, -59.9414, 91.582, 123.574, 13.3374, 159.675, 90, 7.27295, 52.6465, 59.9414, 91.582, -123.574, 13.3374, -159.675, 0, 90] + fold-pose-back: [0, 97.4707, -2.39502, -94.5483, 134.67, 91.4062, 8.70117, 0.0, 0, -97.4707, -2.39502, 94.5483, 134.67, -91.4062, 8.70117, 0.0, 0, 0] + fold-to-keep-object: [0, 77.1899, 4.37256, -86.1548, 123.904, 11.8213, 59.8096, 74.5532, 90, -77.1899, 4.37256, 86.1548, 123.904, -11.8213, 59.8096, -74.5532, 0, 90] + place-object-pose: [0, 5.07568, -13.6011, -44.7803, 101.184, -0.725098, 6.63574, 88.6597, 0, -5.07568, -13.6011, 44.7803, 101.184, 0.725098, 6.63574, -88.6597, 0, 0] + avoid-shelf-pose-a: [0, 3.51562, -1.56006, -174.946, 123.86, 113.533, 90.6372, -38.4961, 90, -3.51562, -1.56006, 174.946, 123.86, -113.533, 90.6372, 38.4961, 0, 90] + avoid-shelf-pose-b: [0, 37.4634, -29.3774, -142.449, 117.883, -82.0679, -18.8306, 123.882, 90, -37.4634, -29.3774, 142.449, 117.883, 82.0679, -18.8306, -123.882, 0, 90] + avoid-shelf-pose-c: [0, 3.51562, -1.56006, -174.946, 123.86, 113.533, 90.6372, -38.4961, 90, -3.51562, -1.56006, 174.946, 123.86, -113.533, 90.6372, 38.4961, 0, 90] + avoid-shelf-pose-d: [0, 79.8926, -14.2822, -125.793, 148.271, -46.3623, -4.24072, 100.063, 90, -79.8926, -14.2822, 125.793, 148.271, 46.3623, -4.24072, -100.063, 0, 90] + avoid-shelf-pose-e: [0, 9.4043, -5.27344, -127.31, 131.111, -54.7339, -47.7905, 129.595, 90, -10.8325, -12.4365, 116.257, 132.583, 41.1548, -46.2524, -102.656, 0, 90] + avoid-shelf-pose-f: [0, 79.8926, -14.2822, -125.793, 148.271, -46.3623, -4.24072, 100.063, 90, -79.8926, -14.2822, 125.793, 148.271, 46.3623, -4.24072, -100.063, 0, 90] + avoid-shelf-pose-g: [0, 93.5156, 10.5908, -83.3203, 136.318, 11.1621, 44.6045, 95.4272, 90, -93.5156, 10.5908, 83.3203, 136.318, -11.1621, 44.6045, -95.4272, 0, 90] + avoid-shelf-pose-h: [0, 13.7988, 32.8711, -78.772, 147.48, 2.87842, -45.0, 122.937, 90, -13.7988, 32.8711, 78.772, 147.48, -2.87842, -45.0, -122.937, 0, 90] + avoid-shelf-pose-i: [0, 93.5156, 10.5908, -83.3203, 136.318, 11.1621, 44.6045, 95.4272, 90, -93.5156, 10.5908, 83.3203, 136.318, -11.1621, 44.6045, -95.4272, 0, 90] + avoid-shelf-pose-j: [0, 89.4946, 6.02051, -56.0083, 139.351, 34.1895, 22.6538, 98.2397, 90, -89.4946, 6.02051, 56.0083, 139.351, -34.1895, 22.6538, -98.2397, 0, 90] + avoid-shelf-pose-k: [0, 48.2959, 18.6108, -51.7896, 135.044, 48.186, 6.87744, 89.3408, 90, -39.9243, 16.1938, 70.73, 114.829, -28.5425, 35.7056, -79.8267, 0, 90] + avoid-shelf-pose-l: [0, 89.4946, 6.02051, -56.0083, 139.351, 34.1895, 22.6538, 98.2397, 90, -89.4946, 6.02051, 56.0083, 139.351, -34.1895, 22.6538, -98.2397, 0, 90] + view-hand-pose-a: [0, 1.05469, -3.33984, -174.814, 42.9565, 73.125, 97.6245, -38.6719, 90, 5.18555, -11.7554, 163.608, 80.2441, -93.0542, 78.3545, 60.1831, 0, 90] + view-hand-pose-b: [0, -5.18555, -11.7554, -163.608, 80.2441, 93.0542, 78.3545, -60.1831, 90, 5.18555, -11.7554, 163.608, 80.2441, -93.0542, 78.3545, 60.1831, 0, 90] + view-hand-pose-c: [0, -5.18555, -11.7554, -163.608, 80.2441, 93.0542, 78.3545, -60.1831, 90, -1.05469, -3.33984, 174.814, 42.9565, -73.125, 97.6245, 38.6719, 0, 90] + view-hand-pose-d: [0, 44.8462, -2.8125, -113.313, 85.7593, 10.1733, 51.0205, -4.61426, 90, -17.5122, 23.4668, 143.701, 125.728, 84.0674, -62.7319, -109.358, 0, 90] + view-hand-pose-e: [0, 17.5122, 23.4668, -143.701, 125.728, -84.0674, -62.7319, 109.358, 90, -17.5122, 23.4668, 143.701, 125.728, 84.0674, -62.7319, -109.358, 0, 90] + view-hand-pose-f: [0, 17.5122, 23.4668, -143.701, 125.728, -84.0674, -62.7319, 109.358, 90, -44.8462, -2.8125, 113.313, 85.7593, -10.1733, 51.0205, 4.61426, 0, 90] + view-hand-pose-g: [0, 41.3525, 15.4248, -101.536, 80.8154, 20.083, 65.2368, 5.69092, 90, -30.4761, 9.95361, 93.999, 100.503, -10.2832, 33.3325, 2.10937, 0, 90] + view-hand-pose-h: [0, 30.4761, 9.95361, -93.999, 100.503, 10.2832, 33.3325, -2.10937, 90, -30.4761, 9.95361, 93.999, 100.503, -10.2832, 33.3325, 2.10937, 0, 90] + view-hand-pose-i: [0, 30.4761, 9.95361, -93.999, 100.503, 10.2832, 33.3325, -2.10937, 90, -41.3525, 15.4248, 101.536, 80.8154, -20.083, 65.2368, -5.69092, 0, 90] + view-hand-pose-j: [0, 65.2368, 19.5557, -78.2886, 121.069, 44.4946, 18.3911, 74.0698, 90, -46.3843, 29.0918, 71.4551, 121.794, -48.4058, 13.8647, -79.0356, 0, 90] + view-hand-pose-k: [0, 46.3843, 29.0918, -71.4551, 121.794, 48.4058, 13.8647, 79.0356, 90, -46.3843, 29.0918, 71.4551, 121.794, -48.4058, 13.8647, -79.0356, 0, 90] + view-hand-pose-l: [0, 46.3843, 29.0918, -71.4551, 121.794, 48.4058, 13.8647, 79.0356, 90, -65.2368, 19.5557, 78.2886, 121.069, -44.4946, 18.3911, -74.0698, 0, 90] + approaching-from-downside-pose: [0, 34.8926, 7.64648, -7.82227, 42.2095, -56.7773, 102.986, -23.9722, 90, -34.8926, 7.64648, 7.82227, 42.2095, 56.7773, 102.986, 23.9722, 0, 90] + +larm-end-coords: + parent : left_gripper_vacuum_pad + translate : [-0.0005, 0, 0.085] + rotate : [0, -1, 0, 180] +rarm-end-coords: + parent : right_gripper_pad_with_base + translate : [0, 0, 0.042] + rotate : [0, -1, 0, 180] +head-end-coords: + parent : head_camera + translate : [0, 0, 0] + rotate : [ 0, 0, 1, 0] diff --git a/jsk_arc2017_baxter/robots/right_vacuum_gripper.xacro b/jsk_arc2017_baxter/robots/right_vacuum_gripper.xacro new file mode 100644 index 000000000..2c2e002e2 --- /dev/null +++ b/jsk_arc2017_baxter/robots/right_vacuum_gripper.xacro @@ -0,0 +1,385 @@ + + + + + + true + Gazebo/White + + + true + Gazebo/Grey + + + true + Gazebo/Green + + + false + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 41.66666667 + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1.637369792 + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1.3 + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1.0 + + + + From fdf4a740a7e59bf52a13c65043dbf7c835d66dc0 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Mon, 12 Jun 2017 16:03:10 +0900 Subject: [PATCH 02/24] Add dxl controller for gripper-v6 --- fc.rosinstall | 2 +- .../right_gripper_v6/dxl_controllers.yaml | 56 +++++++++++++++++++ .../include/gripper_v6_dxl_controller.launch | 29 ++++++++++ 3 files changed, 86 insertions(+), 1 deletion(-) create mode 100644 jsk_arc2017_baxter/config/right_gripper_v6/dxl_controllers.yaml create mode 100644 jsk_arc2017_baxter/launch/setup/include/gripper_v6_dxl_controller.launch diff --git a/fc.rosinstall b/fc.rosinstall index b71aef7d0..a19b964e1 100644 --- a/fc.rosinstall +++ b/fc.rosinstall @@ -45,7 +45,7 @@ - git: local-name: FOR_GRIPPER_V5/dynamixel_motor uri: https://github.com/pazeshun/dynamixel_motor.git - version: gripper-v5-devel + version: gripper-v6-devel - git: local-name: FA-I-sensor/force_proximity_ros uri: https://github.com/knorth55/FA-I-sensor.git diff --git a/jsk_arc2017_baxter/config/right_gripper_v6/dxl_controllers.yaml b/jsk_arc2017_baxter/config/right_gripper_v6/dxl_controllers.yaml new file mode 100644 index 000000000..664c65d9d --- /dev/null +++ b/jsk_arc2017_baxter/config/right_gripper_v6/dxl_controllers.yaml @@ -0,0 +1,56 @@ +prismatic_joint_controller: + controller: + package: jsk_2016_01_baxter_apc + module: calib_required_joint_controller + type: CalibRequiredJointController + joint_name: right_gripper_prismatic_joint_motor + joint_speed: 5.5 + joint_torque_limit: 0.3 + calib_speed: 1.0 + calib_torque_limit: 0.3 + detect_limit_load: 0.17 + motor: + id: 1 + init: 0 + min: 0 + max: 3300 +vacuum_pad_tendon_controller: + controller: + package: dynamixel_controllers + module: joint_position_controller + type: JointPositionController + joint_name: right_gripper_vacuum_pad_tendon_winder + joint_speed: 5.5 + joint_torque_limit: 0.4 + motor: + id: 4 + init: 570 + min: 1023 + max: 120 +finger_yaw_joint_controller: + controller: + package: dynamixel_controllers + module: joint_position_controller + type: JointPositionController + joint_name: right_gripper_finger_yaw_joint_motor + joint_speed: 5.5 + joint_torque_limit: 0.15 + motor: + id: 2 + init: 250 + min: 250 + max: 650 +finger_tendon_controller: + controller: + package: dynamixel_controllers + module: joint_position_controller + type: JointPositionController + joint_name: right_gripper_finger_tendon_winder + joint_speed: 5.5 + joint_torque_limit: 0.6 + ignored_errors: ['DXL_OVERLOAD_ERROR'] + motor: + id: 3 + init: 3530 + min: 3530 + max: 1000 diff --git a/jsk_arc2017_baxter/launch/setup/include/gripper_v6_dxl_controller.launch b/jsk_arc2017_baxter/launch/setup/include/gripper_v6_dxl_controller.launch new file mode 100644 index 000000000..cef97396f --- /dev/null +++ b/jsk_arc2017_baxter/launch/setup/include/gripper_v6_dxl_controller.launch @@ -0,0 +1,29 @@ + + + + + + namespace: dxl_manager + serial_ports: + port: + port_name: "/dev/r_dxhub" + baud_rate: 57600 + min_motor_id: 1 + max_motor_id: 25 + update_rate: 20 + + + + + + + + + From 89079093cfd7d7444edda8656f5d78b29cb95a45 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Mon, 12 Jun 2017 16:04:24 +0900 Subject: [PATCH 03/24] Add ros_control layer for gripper-v6 --- jsk_arc2017_baxter/CMakeLists.txt | 53 ++- .../config/right_gripper_v6/ros_control.yaml | 34 ++ .../include/gripper_v6_ros_control.launch | 30 ++ jsk_arc2017_baxter/package.xml | 12 + .../src/gripper_v6_ros_control.cpp | 313 ++++++++++++++++++ 5 files changed, 441 insertions(+), 1 deletion(-) create mode 100644 jsk_arc2017_baxter/config/right_gripper_v6/ros_control.yaml create mode 100644 jsk_arc2017_baxter/launch/setup/include/gripper_v6_ros_control.launch create mode 100644 jsk_arc2017_baxter/src/gripper_v6_ros_control.cpp diff --git a/jsk_arc2017_baxter/CMakeLists.txt b/jsk_arc2017_baxter/CMakeLists.txt index 619c5f29d..8bba12d46 100644 --- a/jsk_arc2017_baxter/CMakeLists.txt +++ b/jsk_arc2017_baxter/CMakeLists.txt @@ -5,6 +5,12 @@ find_package(catkin REQUIRED COMPONENTS force_proximity_ros message_generation roseus + std_msgs + baxter_core_msgs + hardware_interface + transmission_interface + controller_manager + dynamixel_msgs ) ################################################ @@ -42,13 +48,58 @@ catkin_package( CATKIN_DEPENDS message_runtime force_proximity_ros + std_msgs + baxter_core_msgs + hardware_interface + transmission_interface + controller_manager + dynamixel_msgs ) ########### ## Build ## ########### -# XXX +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories(include + ${Boost_INCLUDE_DIRS} +) + +## ------------------- +## baxter.l generation +## ------------------- +add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/robots/baxter.l + COMMAND rosrun euscollada collada2eus baxter.dae baxter.yaml baxter.l + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/robots + DEPENDS ${PROJECT_SOURCE_DIR}/robots/baxter.dae ${PROJECT_SOURCE_DIR}/robots/baxter.yaml) +add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/robots/baxter.dae + COMMAND rosrun collada_urdf urdf_to_collada baxter.urdf baxter.dae + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/robots + DEPENDS ${PROJECT_SOURCE_DIR}/robots/baxter.urdf) +add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/robots/baxter.urdf + COMMAND ROS_PACKAGE_PATH=${PROJECT_SOURCE_DIR}:$ENV{ROS_PACKAGE_PATH} rosrun xacro xacro baxter.xacro > baxter.urdf + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/robots + DEPENDS ${PROJECT_SOURCE_DIR}/robots/baxter.xacro + ${PROJECT_SOURCE_DIR}/robots/right_vacuum_gripper.xacro) +# get collada-dom version +find_package(PkgConfig) +pkg_check_modules(COLLADA collada-dom>=2.4.4) +if(${COLLADA_FOUND}) + add_custom_target(generate_baxter_lisp ALL DEPENDS ${PROJECT_SOURCE_DIR}/robots/baxter.l) +else() + pkg_check_modules(COLLADA collada-dom) + message(WARNING "urdf_to_collada requries collada-dom >= 2.4.4, installed version is ${COLLADA_VERSION}") +endif() + +## Declare a C++ executable +add_executable(gripper_v6_ros_control_node src/gripper_v6_ros_control.cpp) + +## Add cmake target dependencies of the executable +add_dependencies(gripper_v6_ros_control_node ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(gripper_v6_ros_control_node ${catkin_LIBRARIES}) ############# ## Install ## diff --git a/jsk_arc2017_baxter/config/right_gripper_v6/ros_control.yaml b/jsk_arc2017_baxter/config/right_gripper_v6/ros_control.yaml new file mode 100644 index 000000000..891b2f5d1 --- /dev/null +++ b/jsk_arc2017_baxter/config/right_gripper_v6/ros_control.yaml @@ -0,0 +1,34 @@ +# Actuators +actuator_names: + - right_gripper_prismatic_joint_motor + - right_gripper_vacuum_pad_tendon_winder + - right_gripper_finger_yaw_joint_motor + - right_gripper_finger_tendon_winder +joint_names: + - right_gripper_prismatic_joint + - right_gripper_vacuum_pad_joint + - right_gripper_finger_yaw_joint + - right_gripper_finger_roll_joint +controller_names: + - prismatic_joint_controller + - vacuum_pad_tendon_controller + - finger_yaw_joint_controller + - finger_tendon_controller +mechanical_reduction: + - 41.66666667 + - 1.637369792 + - 1.3 + - 1.0 + +# Flex sensors +flex_names: + - right + - left +flex_thresholds: + - 720 + - 700 +wind_offset_flex: + - 0.35 + - 0.35 + +control_rate: 20 diff --git a/jsk_arc2017_baxter/launch/setup/include/gripper_v6_ros_control.launch b/jsk_arc2017_baxter/launch/setup/include/gripper_v6_ros_control.launch new file mode 100644 index 000000000..0c1d00e93 --- /dev/null +++ b/jsk_arc2017_baxter/launch/setup/include/gripper_v6_ros_control.launch @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/jsk_arc2017_baxter/package.xml b/jsk_arc2017_baxter/package.xml index 12fbf9e86..d945444be 100644 --- a/jsk_arc2017_baxter/package.xml +++ b/jsk_arc2017_baxter/package.xml @@ -18,6 +18,12 @@ jsk_apc2016_common message_generation roseus + std_msgs + baxter_core_msgs + hardware_interface + transmission_interface + controller_manager + dynamixel_msgs force_proximity_ros jsk_2015_05_baxter_apc @@ -39,6 +45,12 @@ rospy rosserial_python xacro + std_msgs + baxter_core_msgs + hardware_interface + transmission_interface + controller_manager + dynamixel_msgs jsk_tools diff --git a/jsk_arc2017_baxter/src/gripper_v6_ros_control.cpp b/jsk_arc2017_baxter/src/gripper_v6_ros_control.cpp new file mode 100644 index 000000000..d6179c372 --- /dev/null +++ b/jsk_arc2017_baxter/src/gripper_v6_ros_control.cpp @@ -0,0 +1,313 @@ +#include +#include + +#include +#include +#include +#include +// We probably don't need this because limiters are on dynamixel controllers +//#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +class GripperRosControl : public hardware_interface::RobotHW +{ +private: + ros::NodeHandle nh_; + + hardware_interface::JointStateInterface jnt_state_interface_; + hardware_interface::PositionJointInterface pos_jnt_interface_; + + const std::vector actr_names_; + const std::vector jnt_names_; + const std::vector controller_names_; + const std::vector flex_names_; + const std::vector flex_thre_; + const std::vector wind_offset_flex_; + + // Actuator raw data + std::map actr_curr_pos_; + std::map actr_curr_vel_; + std::map actr_curr_eff_; + std::map actr_cmd_pos_; + + // Joint raw data + std::map jnt_curr_pos_; + std::map jnt_curr_vel_; + std::map jnt_curr_eff_; + std::map jnt_cmd_pos_; + + // For transmission between actuator and joint + transmission_interface::ActuatorToJointStateInterface actr_to_jnt_state_; + transmission_interface::JointToActuatorPositionInterface jnt_to_actr_pos_; + std::vector > trans_; + std::map actr_curr_data_; + std::map actr_cmd_data_; + std::map jnt_curr_data_; + std::map jnt_cmd_data_; + + // ROS publishers + std::map actr_cmd_pub_; + ros::Publisher vacuum_pub_; + + // ROS subscribers + std::map actr_state_sub_; + std::map flex_sub_; + ros::Subscriber robot_state_sub_; + + std::map received_actr_states_; + std::map received_flex_; + bool is_gripper_enabled_; + + // ROS service clients + std::map torque_enable_client_; + + // For multi-threaded spinning + boost::shared_ptr subscriber_spinner_; + ros::CallbackQueue subscriber_queue_; + +public: + GripperRosControl(const std::vector& actr_names, const std::vector& jnt_names, + const std::vector& controller_names, const std::vector& reducers, + const std::vector& flex_names, const std::vector& flex_thre, + const std::vector& wind_offset_flex) + : actr_names_(actr_names) + , jnt_names_(jnt_names) + , controller_names_(controller_names) + , flex_names_(flex_names) + , flex_thre_(flex_thre) + , wind_offset_flex_(wind_offset_flex) + , is_gripper_enabled_(true) + { + for (int i = 0; i < jnt_names_.size(); i++) + { + std::string actr = actr_names_[i]; + std::string jnt = jnt_names_[i]; + + // Get transmission + boost::shared_ptr t_ptr( + new transmission_interface::SimpleTransmission(reducers[i])); + trans_.push_back(t_ptr); + + // Initialize and wrap raw current data + actr_curr_data_[actr].position.push_back(&actr_curr_pos_[actr]); + actr_curr_data_[actr].velocity.push_back(&actr_curr_vel_[actr]); + actr_curr_data_[actr].effort.push_back(&actr_curr_eff_[actr]); + jnt_curr_data_[jnt].position.push_back(&jnt_curr_pos_[jnt]); + jnt_curr_data_[jnt].velocity.push_back(&jnt_curr_vel_[jnt]); + jnt_curr_data_[jnt].effort.push_back(&jnt_curr_eff_[jnt]); + + // Initialize and wrap raw command data + actr_cmd_data_[actr].position.push_back(&actr_cmd_pos_[actr]); + jnt_cmd_data_[jnt].position.push_back(&jnt_cmd_pos_[jnt]); + + // Register transmissions to each interface + actr_to_jnt_state_.registerHandle(transmission_interface::ActuatorToJointStateHandle( + actr + "_trans", trans_[i].get(), actr_curr_data_[actr], jnt_curr_data_[jnt])); + jnt_to_actr_pos_.registerHandle(transmission_interface::JointToActuatorPositionHandle( + jnt + "_trans", trans_[i].get(), actr_cmd_data_[actr], jnt_cmd_data_[jnt])); + + // Connect and register the joint state handle + hardware_interface::JointStateHandle state_handle(jnt, &jnt_curr_pos_[jnt], &jnt_curr_vel_[jnt], + &jnt_curr_eff_[jnt]); + jnt_state_interface_.registerHandle(state_handle); + + // Connect and register the joint position handle + hardware_interface::JointHandle pos_handle(jnt_state_interface_.getHandle(jnt), &jnt_cmd_pos_[jnt]); + pos_jnt_interface_.registerHandle(pos_handle); + + // ROS publishers and subscribers + actr_cmd_pub_[actr] = nh_.advertise("dxl/" + controller_names_[i] + "/command", 5); + actr_state_sub_[actr] = + nh_.subscribe("dxl/" + controller_names_[i] + "/state", 1, &GripperRosControl::actrStateCallback, this); + } + + // Register interfaces + registerInterface(&jnt_state_interface_); + registerInterface(&pos_jnt_interface_); + + // Publisher for vacuum + vacuum_pub_ = nh_.advertise("vacuum", 10); + + // Subscribers for flex + for (std::vector::const_iterator itr = flex_names_.begin(); itr != flex_names_.end(); ++itr) + { + flex_sub_[*itr] = nh_.subscribe("flex/" + *itr + "/state", 1, + boost::bind(&GripperRosControl::flexCallback, this, _1, *itr)); + } + + // Subscriber for robot state + robot_state_sub_ = nh_.subscribe("/robot/state", 1, &GripperRosControl::robotStateCallback, this); + + // Service clients for torque enabling + for (std::vector::const_iterator itr = controller_names_.begin(); itr != controller_names_.end(); + ++itr) + { + torque_enable_client_[*itr] = + nh_.serviceClient("dxl/" + *itr + "/torque_enable"); + } + + // Start spinning + nh_.setCallbackQueue(&subscriber_queue_); + subscriber_spinner_.reset(new ros::AsyncSpinner(1, &subscriber_queue_)); + subscriber_spinner_->start(); + } + + void cleanup() + { + subscriber_spinner_->stop(); + } + + void read() + { + // Update actuator current state + for (int i = 0; i < actr_names_.size(); i++) + { + actr_curr_pos_[actr_names_[i]] = received_actr_states_[actr_names_[i]].current_pos; + actr_curr_vel_[actr_names_[i]] = received_actr_states_[actr_names_[i]].velocity; + + // If fingers flex, add offset angle to finger tendon winder + if (actr_names_[i].find("finger_tendon_winder") != std::string::npos) + { + for (int j = 0; j < flex_names_.size(); j++) + { + if (received_flex_[flex_names_[j]] > flex_thre_[j]) + { + actr_curr_pos_[actr_names_[i]] -= wind_offset_flex_[j]; + } + } + } + } + + // Propagate current actuator state to joints + actr_to_jnt_state_.propagate(); + } + + void write() + { + if (is_gripper_enabled_) + { + // Propagate joint commands to actuators + jnt_to_actr_pos_.propagate(); + + // Publish command to actuator + for (int i = 0; i < actr_names_.size(); i++) + { + // If fingers flex, add offset angle to finger tendon winder + if (actr_names_[i].find("finger_tendon_winder") != std::string::npos) + { + for (int j = 0; j < flex_names_.size(); j++) + { + if (received_flex_[flex_names_[j]] > flex_thre_[j]) + { + actr_cmd_pos_[actr_names_[i]] += wind_offset_flex_[j]; + } + } + } + + std_msgs::Float64 msg; + msg.data = actr_cmd_pos_[actr_names_[i]]; + actr_cmd_pub_[actr_names_[i]].publish(msg); + } + } + else + { + // Switch off vacuum + std_msgs::Bool vacuum; + vacuum.data = false; + vacuum_pub_.publish(vacuum); + + // Gripper servo off + dynamixel_controllers::TorqueEnable srv; + srv.request.torque_enable = false; + for (std::vector::const_iterator itr = controller_names_.begin(); itr != controller_names_.end(); + ++itr) + { + torque_enable_client_[*itr].call(srv); + } + } + } + + bool isGripperEnabled() + { + return is_gripper_enabled_; + } + + void actrStateCallback(const dynamixel_msgs::JointStateConstPtr& dxl_actr_state) + { + received_actr_states_[dxl_actr_state->name] = *dxl_actr_state; + } + + void flexCallback(const std_msgs::UInt16ConstPtr& flex, const std::string& name) + { + received_flex_[name] = flex->data; + } + + void robotStateCallback(const baxter_core_msgs::AssemblyStateConstPtr& state) + { + is_gripper_enabled_ = state->enabled; + } +}; // end class GripperRosControl + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "gripper_v6_ros_control_node"); + + std::vector actr_names; + std::vector jnt_names; + std::vector controller_names; + std::vector reducers; + int rate_hz; + std::vector flex_names; + std::vector flex_thre; + std::vector wind_offset_flex; + + if (!(ros::param::get("~actuator_names", actr_names) && ros::param::get("~joint_names", jnt_names) && + ros::param::get("~controller_names", controller_names) && ros::param::get("~mechanical_reduction", reducers) && + ros::param::get("~control_rate", rate_hz) && ros::param::get("~flex_names", flex_names) && + ros::param::get("~flex_thresholds", flex_thre) && ros::param::get("~wind_offset_flex", wind_offset_flex))) + { + ROS_ERROR("Couldn't get necessary parameters"); + return 0; + } + + GripperRosControl gripper(actr_names, jnt_names, controller_names, reducers, flex_names, flex_thre, wind_offset_flex); + controller_manager::ControllerManager cm(&gripper); + + // For non-realtime spinner thread + ros::AsyncSpinner spinner(1); + spinner.start(); + + // Control loop + ros::Rate rate(rate_hz); + ros::Time prev_time = ros::Time::now(); + bool prev_gripper_enabled = true; + + while (ros::ok()) + { + const ros::Time now = ros::Time::now(); + const ros::Duration elapsed_time = now - prev_time; + const bool gripper_enabled = gripper.isGripperEnabled(); + + gripper.read(); + cm.update(now, elapsed_time, !prev_gripper_enabled && gripper_enabled); + gripper.write(); + prev_time = now; + prev_gripper_enabled = gripper_enabled; + + rate.sleep(); + } + spinner.stop(); + gripper.cleanup(); + + return 0; +} From d19cbaabdc0256dfc45017c0d9d9de6911ae8f8e Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Mon, 12 Jun 2017 16:06:21 +0900 Subject: [PATCH 04/24] Add baxter.launch for right gripper-v6 --- jsk_arc2017_baxter/launch/setup/baxter.launch | 15 +++++++--- .../setup/include/gripper_arduinos.launch | 28 +++++++++++++++++++ 2 files changed, 39 insertions(+), 4 deletions(-) create mode 100644 jsk_arc2017_baxter/launch/setup/include/gripper_arduinos.launch diff --git a/jsk_arc2017_baxter/launch/setup/baxter.launch b/jsk_arc2017_baxter/launch/setup/baxter.launch index a04f48611..5ba4dd934 100644 --- a/jsk_arc2017_baxter/launch/setup/baxter.launch +++ b/jsk_arc2017_baxter/launch/setup/baxter.launch @@ -4,10 +4,11 @@ + + command="$(find xacro)/xacro.py $(find jsk_arc2017_baxter)/robots/baxter.xacro" /> @@ -17,13 +18,19 @@ - + + pkg="jsk_2016_01_baxter_apc" type="gripper_joint_states_publisher"> + $(arg gripper_v4_limb) + + pkg="jsk_2016_01_baxter_apc" type="gripper_trajectory_server" output="screen"> + $(arg gripper_v4_limb) + + + diff --git a/jsk_arc2017_baxter/launch/setup/include/gripper_arduinos.launch b/jsk_arc2017_baxter/launch/setup/include/gripper_arduinos.launch new file mode 100644 index 000000000..eb9f64165 --- /dev/null +++ b/jsk_arc2017_baxter/launch/setup/include/gripper_arduinos.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + From a11ac8cb5d0156636866bd3a4ef8da2e45c1f7a9 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Mon, 12 Jun 2017 16:11:13 +0900 Subject: [PATCH 05/24] Adjust euslisp codes to baxter with right gripper-v6 --- .../euslisp/lib/arc-interface.l | 2 +- .../euslisp/lib/baxter-interface.l | 62 ++++- jsk_arc2017_baxter/euslisp/lib/baxter.l | 236 +++++++++++++++++- .../euslisp/lib/pick-interface.l | 2 +- .../euslisp/lib/stow-interface.l | 2 +- 5 files changed, 290 insertions(+), 14 deletions(-) diff --git a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l index aec3504c7..5e51ae816 100755 --- a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l @@ -481,7 +481,7 @@ (defun jsk_arc2017_baxter::arc-init (&key (ctype :default-controller) (moveit nil)) (let (mvit-env mvit-rb) (when moveit - (setq mvit-env (instance jsk_2016_01_baxter_apc::baxter-moveit-environment)) + (setq mvit-env (instance jsk_arc2017_baxter::baxter-moveit-environment)) (setq mvit-rb (instance jsk_arc2017_baxter::baxter-robot :init))) (unless (boundp '*ri*) (setq *ri* (instance jsk_arc2017_baxter::baxter-interface :init :type ctype diff --git a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l index 71b0b2f00..99f13b280 100644 --- a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l @@ -1,9 +1,10 @@ ;; -*- mode: lisp;-*- +;; FIXME: Please add moveit + (require "package://baxtereus/baxter-interface.l") (require "package://jsk_2015_05_baxter_apc/euslisp/jsk_2015_05_baxter_apc/util.l") -(require "package://jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/baxter-interface.l") -(require "package://jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/baxter.l") +(require "package://jsk_arc2017_baxter/euslisp/lib/baxter.l") (require "package://jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/util.l") @@ -15,17 +16,18 @@ (defclass jsk_arc2017_baxter::baxter-interface :super baxter-interface :slots (rarm-pressure-threshold- - larm-pressure-threshold-)) + larm-pressure-threshold- + right-hand-action-)) (defmethod jsk_arc2017_baxter::baxter-interface (:init (&rest args &key ((:moveit-environment mvit-env) - (instance jsk_2016_01_baxter_apc::baxter-moveit-environment)) - ((:moveit-robot mvit-rb) (instance jsk_2016_01_baxter_apc::baxter-robot :init)) + (instance jsk_arc2017_baxter::baxter-moveit-environment)) + ((:moveit-robot mvit-rb) (instance jsk_arc2017_baxter::baxter-robot :init)) &allow-other-keys) ;; initialize controllers - (send-super* :init :joint-states-queue-size 2 + (send-super* :init :joint-states-queue-size 3 ;; FIXME: :robot jsk_arc2017_baxter::baxter-robot ;; FIXME: :end-coords-interpolation does not work. :robot baxter-robot @@ -51,6 +53,17 @@ (ros::advertise "/vacuum_gripper/limb/right" std_msgs::Bool)) (ros::advertise "/gripper_front/limb/left/servo/torque" std_msgs::Bool) (ros::advertise "/gripper_front/limb/right/servo/torque" std_msgs::Bool) + ;; initialize hand action + (setq right-hand-action- + (instance ros::simple-action-client :init + "/gripper_front/limb/right/hand/follow_joint_trajectory" + control_msgs::FollowJointTrajectoryAction + :groupname groupname)) + ;; check if hand action is respond (based on baxter-interface) + (unless + (and joint-action-enable (send right-hand-action- :wait-for-server 3)) + (ros::ros-warn "~A is not respond" right-hand-action-) + (ros::ros-info "*** if you do not have hand, you can ignore this message ***")) (if mvit-rb (setq moveit-robot mvit-rb)) (if mvit-env (send self :set-moveit-environment (send mvit-env :init :robot moveit-robot)))) ;; Overwrite super class's :rarm-controller @@ -86,10 +99,10 @@ (:rgripper-controller () (list (list - (cons :controller-action "/gripper_front/limb/right/follow_joint_trajectory") - (cons :controller-state "/gripper_front/limb/right/state") + (cons :controller-action "/gripper_front/limb/right/vacuum_gripper/follow_joint_trajectory") + (cons :controller-state "/gripper_front/limb/right/vacuum_gripper/state") (cons :action-type control_msgs::FollowJointTrajectoryAction) - (cons :joint-names (list "right_gripper_vacuum_pad_joint"))))) + (cons :joint-names (list "right_gripper_prismatic_joint" "right_gripper_vacuum_pad_joint"))))) (:lgripper-controller () (list (list @@ -97,6 +110,37 @@ (cons :controller-state "/gripper_front/limb/left/state") (cons :action-type control_msgs::FollowJointTrajectoryAction) (cons :joint-names (list "left_gripper_vacuum_pad_joint"))))) + ;; based on naoqi-interface and fetch-interface + (:move-hand + (arm av tm &key (wait t)) + (if (and (= (length av) 2) (not (send self :simulation-modep))) + (if (eq arm :rarm) + (let (av-list res (goal (instance control_msgs::FollowJointTrajectoryActionGoal :init)) + (traj (instance trajectory_msgs::JointTrajectory :init))) + (dotimes (i (length av)) (pushback (elt av i) av-list)) + (send traj :header :seq 1) + (send traj :header :stamp (ros::time-now)) + (send traj :joint_names + (list "right_gripper_finger_yaw_joint" "right_gripper_finger_roll_joint")) + (send traj :points + (list (instance trajectory_msgs::JointTrajectoryPoint + :init + :positions (mapcar #'deg2rad av-list) + :time_from_start (ros::time (/ tm 1000))))) + (send goal :goal :trajectory traj) + (send right-hand-action- :send-goal goal) + (setq res (send right-hand-action- :get-result)) + (when wait + (send right-hand-action- :wait-for-result) + (setq res (send right-hand-action- :get-result))) + (unless (eq (send res :error_code) 0) + (warning-message 3 ":move-hand error code ~A returns from JTA: ~A~%" + (send res :error_code) (send res :error_string))) + res) + nil) + nil) + ) + (:get-arm-controller (arm) (cond ((eq arm :rarm) :rarm-controller) ((eq arm :larm) :larm-controller) diff --git a/jsk_arc2017_baxter/euslisp/lib/baxter.l b/jsk_arc2017_baxter/euslisp/lib/baxter.l index 78a18a5a9..82012fa63 100644 --- a/jsk_arc2017_baxter/euslisp/lib/baxter.l +++ b/jsk_arc2017_baxter/euslisp/lib/baxter.l @@ -3,16 +3,248 @@ ;; vim: set ft=lisp: (require "package://jsk_2015_05_baxter_apc/euslisp/jsk_2015_05_baxter_apc/util.l") -(require "package://jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/baxter.l") +(require "package://baxtereus/baxter-util.l") +(require "package://jsk_arc2017_baxter/robots/baxter.l") (unless (find-package "JSK_ARC2017_BAXTER") (make-package "JSK_ARC2017_BAXTER")) (defclass jsk_arc2017_baxter::baxter-robot - :super jsk_2016_01_baxter_apc::baxter-robot) + :super baxter-robot + :slots (view-hand-pos- ik-prepared-poses- hand-joints-)) (defmethod jsk_arc2017_baxter::baxter-robot + (:init + (&rest args) + (send-super* :init args) + ;; initialize view-hand-pos- + (setq view-hand-pos- (make-hash-table)) + (sethash :rarm view-hand-pos- (make-hash-table)) + (sethash :larm view-hand-pos- (make-hash-table)) + ;; for larm + (sethash :a (gethash :larm view-hand-pos-) #f(785.344 229.224 762.48)) + (sethash :b (gethash :larm view-hand-pos-) #f(813.742 10.946 828.431)) + (sethash :d (gethash :larm view-hand-pos-) #f(807.059 275.852 633.668)) + (sethash :e (gethash :larm view-hand-pos-) #f(770.657 57.875 550.976)) + (sethash :g (gethash :larm view-hand-pos-) #f(781.985 244.363 368.102)) + (sethash :h (gethash :larm view-hand-pos-) #f(819.589 13.426 415.42)) + (sethash :j (gethash :larm view-hand-pos-) #f(811.875 312.473 156.184)) + (sethash :k (gethash :larm view-hand-pos-) #f(780.791 15.733 137.103)) + ;; for rarm + (sethash :b (gethash :rarm view-hand-pos-) #f(813.742 -10.946 828.431)) + (sethash :c (gethash :rarm view-hand-pos-) #f(785.344 -180 762.48)) + (sethash :e (gethash :rarm view-hand-pos-) #f(770.657 -57.875 550.976)) + (sethash :f (gethash :rarm view-hand-pos-) #f(807.059 -186 633.668)) + (sethash :h (gethash :rarm view-hand-pos-) #f(819.589 -13.426 415.42)) + (sethash :i (gethash :rarm view-hand-pos-) #f(781.985 -184 368.102)) + (sethash :k (gethash :rarm view-hand-pos-) #f(780.791 -15.733 137.103)) + (sethash :l (gethash :rarm view-hand-pos-) #f(811.875 -180 156.184)) + ;; pos of :c, :f, :i, :l is not symmetrical to :a, :d, :g, :j because torso can't see + ;; initialize ik-prepared-poses- + (setq ik-prepared-poses- (list :untuck-pose)) + ;; initialize hand-joints- + (setq hand-joints- (make-hash-table)) + (sethash :rarm hand-joints- + (list :right_gripper_finger_yaw_joint + :right_gripper_finger_roll_joint + ;; We have to move mimic joints, too + :right_gripper_r_finger_yaw_joint + :right_gripper_r_finger_roll_joint)) + ) + (:set-ik-prepared-poses + (poses) + (if (listp poses) + (setq ik-prepared-poses- poses)) + ) + (:ik-prepared-poses () ik-prepared-poses-) + (:inverse-kinematics + (target-coords + &rest args + &key move-target + (link-list + (if (atom move-target) + (send self :link-list (send move-target :parent)) + (mapcar #'(lambda (mt) (send self :link-list (send mt :parent))) move-target))) + (use-gripper nil) (rthre (deg2rad 10)) + &allow-other-keys) + ;; if the last link of link-list is in gripper, remove gripper links + (if (null use-gripper) + (cond ((equal (send (car (last link-list)) :name) "right_gripper_pad_with_base") + (setq link-list (butlast link-list 2))) + ((equal (send (car (last link-list)) :name) "left_gripper_vacuum_pad_base") + (setq link-list (butlast link-list)))) + nil) + (send-super* :inverse-kinematics target-coords + :move-target move-target :link-list link-list :rthre rthre args) + ) + (:rotate-wrist + (arm angle &key (relative t)) + (send self arm :wrist-r :joint-angle angle :relative relative) + (send self :angle-vector) + ) + (:rotate-gripper + (arm angle &key (relative t)) + (send self arm :gripper-p :joint-angle angle :relative relative) + (send self :angle-vector) + ) + (:slide-gripper + (arm pos &key (relative t)) + (send self arm :gripper-x :joint-angle pos :relative relative) + (send self :angle-vector)) + (:hand + (arm &rest args) + (case (car args) + ((:angle-vector) + (let ((av (cadr args)) (joints (gethash arm hand-joints-))) + (when av + (dotimes (i (length av)) + (send self (elt joints i) :joint-angle (elt av i)) + (send self (elt joints (+ i (length av))) :joint-angle (elt av i)))) + (mapcar + #'(lambda (j) (send self j :joint-angle)) + (subseq joints 0 (/ (length joints) 2))) + )) + (t (error ":hand first arg is invalid. args: ~A~%" args)) + )) + (:hand-grasp-pre-pose + (arm style) + (case style + (:opposed (send self :hand arm :angle-vector #f(0 0))) + (:spherical (send self :hand arm :angle-vector #f(30 0))) + (:cylindrical (send self :hand arm :angle-vector #f(90 0))) + (t (error ":hand-grasp-pre-pose no such style ~A~%" style)) + )) + (:hand-grasp-pose + (arm style &key (angle 180)) + (case style + (:opposed (send self :hand arm :angle-vector (float-vector 0 angle))) + (:spherical (send self :hand arm :angle-vector (float-vector 30 angle))) + (:cylindrical (send self :hand arm :angle-vector (float-vector 90 angle))) + (t (error ":hand-grasp-pose no such style ~A~%" style)) + )) + (:avoid-shelf-pose + (arm bin) + (case bin + (:a (send self :avoid-shelf-pose-a arm)) + (:b (send self :avoid-shelf-pose-b arm)) + (:c (send self :avoid-shelf-pose-c arm)) + (:d (send self :avoid-shelf-pose-d arm)) + (:e (send self :avoid-shelf-pose-e arm)) + (:f (send self :avoid-shelf-pose-f arm)) + (:g (send self :avoid-shelf-pose-g arm)) + (:h (send self :avoid-shelf-pose-h arm)) + (:i (send self :avoid-shelf-pose-i arm)) + (:j (send self :avoid-shelf-pose-j arm)) + (:k (send self :avoid-shelf-pose-k arm)) + (:l (send self :avoid-shelf-pose-l arm))) + ) + (:view-hand-pose + (arm bin) + (case bin + (:a (send self :view-hand-pose-a arm)) + (:b (send self :view-hand-pose-b arm)) + (:c (send self :view-hand-pose-c arm)) + (:d (send self :view-hand-pose-d arm)) + (:e (send self :view-hand-pose-e arm)) + (:f (send self :view-hand-pose-f arm)) + (:g (send self :view-hand-pose-g arm)) + (:h (send self :view-hand-pose-h arm)) + (:i (send self :view-hand-pose-i arm)) + (:j (send self :view-hand-pose-j arm)) + (:k (send self :view-hand-pose-k arm)) + (:l (send self :view-hand-pose-l arm))) + (let ((pos (gethash bin (gethash arm view-hand-pos-)))) + (if pos + (send self arm :inverse-kinematics + (make-coords :pos pos) + :rotation-axis nil + :revert-if-fail nil) + ) + ) + ) + (:visualize-path + (avs &key (limb)) + (let (robots coords-list) + (dotimes (i (length avs)) + (let ((robot (instance (class self) :init))) + (send robot :angle-vector (elt avs i)) + (when (not (eq i (1- (length avs)))) + (send-all (send robot :bodies) :set-color nil 0.3) + ) + (pushback robot robots) + (when limb + (pushback (send robot limb :end-coords) coords-list) + ) + ) + ) + (objects (append robots coords-list)) + ) + ) + (:view-opposite-hand-pose + (arm &key (dy 300)) + (let (opposite-arm gripper-coords theta) + (setq opposite-arm (if (eq arm :larm) :rarm :larm)) + (setq gripper-coords + (send (send self opposite-arm :end-coords) + :copy-worldcoords)) + (send gripper-coords :orient 0 :z :world) + (if (eq arm :larm) + (setq theta -1.57) + (progn + (setq dy (- dy)) + (setq theta 1.57))) + (send gripper-coords :translate (float-vector 0 dy -200)) + (send gripper-coords :rotate theta :z :world) + (send self :rotate-gripper arm 70 :relative nil) + (send self arm + :inverse-kinematics gripper-coords + :use-gripper nil + :rotation-axis t + :revert-if-fail nil))) + (:get-support-arm-coords + (arm &key (dx 0) (dy 0) (dz 0)) + (let (support-arm-coords) + (setq support-arm-coords + (send (send self arm :end-coords) + :copy-worldcoords)) + (send support-arm-coords :orient 0 :z :world) + (send support-arm-coords :rotate 3.14 :y :world) + (send support-arm-coords :translate (float-vector dx dy dz) :world) + support-arm-coords)) + (:ik->nearest-pose + (arm target-coords pose-candidates &rest args) + (let ((opposite-arm (if (eq arm :larm) :rarm :larm)) + current-opposite-av current-av chosen-pose) + (unless pose-candidates + (setq pose-candidates (list (send self :angle-vector)))) + (let (current-opposite-av current-av ik-solvable-poses chosen-pose) + (setq current-opposite-av (send self opposite-arm :angle-vector)) + (setq current-av (send self arm :angle-vector)) + ;; exclude poses which IK fail from + (dolist (pose pose-candidates) + (send self :angle-vector pose) + (if (send* self arm :inverse-kinematics target-coords args) + (pushback pose ik-solvable-poses))) + (unless ik-solvable-poses + (ros::ros-error "[:ik->nearest-pose] Cannot solve IK from poses") + (send self opposite-arm :angle-vector current-opposite-av) + (send self arm :angle-vector current-av) + (return-from :ik->nearest-pose nil)) + (setq chosen-pose + (car (sort ik-solvable-poses #'< + #'(lambda (pose) + (norm + (v- + current-av + (progn + (send self :angle-vector pose) + (send* self arm :inverse-kinematics target-coords args) + (send self arm :angle-vector)))))))) + (format t "[:ik->nearest-pose] arm:~a midpose: ~a~%" arm chosen-pose) + (send self :angle-vector chosen-pose) + (send self opposite-arm :angle-vector current-opposite-av) + (send* self arm :inverse-kinematics target-coords args)))) (:spin-off-by-wrist (arm &key (times 10)) (let (avs robot) diff --git a/jsk_arc2017_baxter/euslisp/lib/pick-interface.l b/jsk_arc2017_baxter/euslisp/lib/pick-interface.l index 764b3b715..17efd184a 100644 --- a/jsk_arc2017_baxter/euslisp/lib/pick-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/pick-interface.l @@ -194,7 +194,7 @@ (defun jsk_arc2017_baxter::pick-init (&key (ctype :default-controller) (moveit nil)) (let (mvit-env mvit-rb) (when moveit - (setq mvit-env (instance jsk_2016_01_baxter_apc::baxter-moveit-environment)) + (setq mvit-env (instance jsk_arc2017_baxter::baxter-moveit-environment)) (setq mvit-rb (instance jsk_arc2017_baxter::baxter-robot :init))) (unless (boundp '*ri*) (setq *ri* (instance jsk_arc2017_baxter::baxter-interface :init :type ctype diff --git a/jsk_arc2017_baxter/euslisp/lib/stow-interface.l b/jsk_arc2017_baxter/euslisp/lib/stow-interface.l index 5e2b2b8a6..c9434ba35 100644 --- a/jsk_arc2017_baxter/euslisp/lib/stow-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/stow-interface.l @@ -168,7 +168,7 @@ (defun jsk_arc2017_baxter::stow-init (&key (ctype :default-controller) (moveit nil)) (let (mvit-env mvit-rb) (when moveit - (setq mvit-env (instance jsk_2016_01_baxter_apc::baxter-moveit-environment)) + (setq mvit-env (instance jsk_arc2017_baxter::baxter-moveit-environment)) (setq mvit-rb (instance jsk_arc2017_baxter::baxter-robot :init))) (unless (boundp '*ri*) (setq *ri* (instance jsk_arc2017_baxter::baxter-interface :init :type ctype From 1904f57424212efd4f5e529b70b8015aef04ced2 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Wed, 14 Jun 2017 17:04:53 +0900 Subject: [PATCH 06/24] Fix linter target --- jsk_arc2017_baxter/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/jsk_arc2017_baxter/CMakeLists.txt b/jsk_arc2017_baxter/CMakeLists.txt index 8bba12d46..fc17f2602 100644 --- a/jsk_arc2017_baxter/CMakeLists.txt +++ b/jsk_arc2017_baxter/CMakeLists.txt @@ -123,6 +123,7 @@ if(CATKIN_ENABLE_TESTING) find_package(jsk_tools REQUIRED) find_package(jsk_apc2016_common REQUIRED) if(${jsk_tools_VERSION} VERSION_GREATER 2.0.13) - jsk_tools_add_shell_test(COMMAND "rosrun jsk_apc2016_common euslint ${PROJECT_SOURCE_DIR}") + jsk_tools_add_shell_test(COMMAND "rosrun jsk_apc2016_common euslint ${PROJECT_SOURCE_DIR}/euslisp") + jsk_tools_add_shell_test(COMMAND "rosrun jsk_apc2016_common euslint ${PROJECT_SOURCE_DIR}/test") endif() endif() From 3a3fc0a3fc62dfe2602ada4fa6142c7833d3fd16 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Thu, 15 Jun 2017 19:47:15 +0900 Subject: [PATCH 07/24] Disable rviz in default of stereo_astra_hand --- .../launch/setup/include/stereo_astra_hand.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_arc2017_baxter/launch/setup/include/stereo_astra_hand.launch b/jsk_arc2017_baxter/launch/setup/include/stereo_astra_hand.launch index ea7ba3c08..89aa004ea 100644 --- a/jsk_arc2017_baxter/launch/setup/include/stereo_astra_hand.launch +++ b/jsk_arc2017_baxter/launch/setup/include/stereo_astra_hand.launch @@ -1,6 +1,6 @@ - + From 047cad04c6795b681e30b6af239c3b67e1dd8a8f Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Thu, 15 Jun 2017 19:49:43 +0900 Subject: [PATCH 08/24] Use right_hand_left_camera in setup_for_pick --- jsk_arc2017_baxter/launch/setup/setup_for_pick.launch | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/jsk_arc2017_baxter/launch/setup/setup_for_pick.launch b/jsk_arc2017_baxter/launch/setup/setup_for_pick.launch index 41fad42c6..5b2e970ee 100644 --- a/jsk_arc2017_baxter/launch/setup/setup_for_pick.launch +++ b/jsk_arc2017_baxter/launch/setup/setup_for_pick.launch @@ -46,8 +46,8 @@ + - @@ -59,8 +59,8 @@ - - + + use_multiple_attention: false @@ -69,7 +69,7 @@ - + keep_organized: true @@ -79,7 +79,7 @@ - + From 82628907dafcd2a76ff51c05d2cd31dabac97790 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Thu, 15 Jun 2017 20:00:20 +0900 Subject: [PATCH 09/24] Add finger init motion to pick and stow init --- jsk_arc2017_baxter/euslisp/pick-main.l | 4 ++++ jsk_arc2017_baxter/euslisp/stow-main.l | 4 ++++ 2 files changed, 8 insertions(+) diff --git a/jsk_arc2017_baxter/euslisp/pick-main.l b/jsk_arc2017_baxter/euslisp/pick-main.l index aa5f4a99a..0d18eee2c 100755 --- a/jsk_arc2017_baxter/euslisp/pick-main.l +++ b/jsk_arc2017_baxter/euslisp/pick-main.l @@ -117,6 +117,10 @@ (jsk_arc2017_baxter::pick-init :ctype ctype :moveit moveit) (when moveit (send *ti* :wipe-all-scene)) (send *ri* :gripper-servo-on) + (send *ri* :move-hand :rarm + (send *baxter* :hand-grasp-pre-pose :rarm :cylindrical) 1000) + (send *ri* :move-hand :rarm + (send *baxter* :hand-grasp-pre-pose :rarm :opposed) 1000) (send *ri* :angle-vector (send *baxter* :fold-pose-back)) (send *ri* :wait-interpolation) (when calib-pressure diff --git a/jsk_arc2017_baxter/euslisp/stow-main.l b/jsk_arc2017_baxter/euslisp/stow-main.l index 37fc31076..d5af912e2 100755 --- a/jsk_arc2017_baxter/euslisp/stow-main.l +++ b/jsk_arc2017_baxter/euslisp/stow-main.l @@ -117,6 +117,10 @@ (jsk_arc2017_baxter::stow-init :ctype ctype :moveit moveit) (when moveit (send *ti* :wipe-all-scene)) (send *ri* :gripper-servo-on) + (send *ri* :move-hand :rarm + (send *baxter* :hand-grasp-pre-pose :rarm :cylindrical) 1000) + (send *ri* :move-hand :rarm + (send *baxter* :hand-grasp-pre-pose :rarm :opposed) 1000) (send *ri* :angle-vector (send *baxter* :fold-pose-back)) (send *ri* :wait-interpolation) (when calib-pressure From b0b5397b5c1c52fb49a11bbf1bd6d44ce5170872 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Thu, 15 Jun 2017 20:01:59 +0900 Subject: [PATCH 10/24] Fold fingers in picking to avoid collision --- .../euslisp/lib/arc-interface.l | 85 +++++++++++++++---- 1 file changed, 69 insertions(+), 16 deletions(-) diff --git a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l index 5e51ae816..f370a72f3 100755 --- a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l @@ -222,17 +222,49 @@ (send self :get-object-position arm movable-region :object-index object-index)) (ros::ros-info "[:pick-object-with-movable-region] arm:~a approach to the object" arm) (send *ri* :gripper-servo-on arm) - (send *ri* :angle-vector - (send *baxter* arm :inverse-kinematics - (make-coords :pos (v+ obj-pos #f(0 0 150)) - :rpy #f(0 0 0)) - :use-gripper t - :rotation-axis :z) - :fast (send *ri* :get-arm-controller arm) 0 :scale 5.0) - (send *ri* :wait-interpolation) + (if (eq arm :larm) + (progn + (send *ri* :angle-vector + (send *baxter* arm :inverse-kinematics + (make-coords :pos (v+ obj-pos #f(0 0 150)) + :rpy #f(0 0 0)) + :use-gripper t + :rotation-axis :z) + :fast (send *ri* :get-arm-controller arm) 0 :scale 5.0) + (send *ri* :wait-interpolation)) + (let (prev-av next-av gripper-x) + (setq prev-av (send *baxter* :angle-vector)) + (setq next-av + (send *baxter* arm :inverse-kinematics + (make-coords :pos (v+ obj-pos #f(0 0 150)) + :rpy #f(0 0 0)) + :use-gripper t + :rotation-axis :z + :additional-check #'(lambda () + (< (send *baxter* arm :gripper-x :joint-angle) 110)))) + (setq gripper-x (send *baxter* arm :gripper-x :joint-angle)) + (send *baxter* :angle-vector prev-av) + ;; First, move prismatic joint to target position + (send *ri* :angle-vector + (send *baxter* :slide-gripper arm gripper-x :relative nil) + :fast (send *ri* :get-arm-controller arm) 0 :scale 5.0) + (send *ri* :wait-interpolation) + ;; Fold fingers to avoid collision + (send *ri* :move-hand arm + (send *baxter* :hand-grasp-pre-pose arm :cylindrical) 1000) + (send *ri* :move-hand arm + (send *baxter* :hand-grasp-pose arm :cylindrical :angle 100) 1000) + ;; Move whole arm to target pose + (send *ri* :angle-vector + (send *baxter* :angle-vector next-av) + :fast (send *ri* :get-arm-controller arm) 0 :scale 5.0) + (send *ri* :wait-interpolation))) (dotimes (i n-trial) (dotimes (j n-trial-same-pos) (unless graspingp + ;; Fold fingers again + (send *ri* :move-hand arm + (send *baxter* :hand-grasp-pose arm :cylindrical) 1000) (setq graspingp (send self :try-to-pick-object arm obj-pos :offset (float-vector 0 0 (- (* i -50) 30)))) @@ -241,6 +273,10 @@ (send *ri* :angle-vector-sequence (reverse avs) :fast (send *ri* :get-arm-controller arm) 0 :scale 5.0) (send *ri* :wait-interpolation) + (when (eq arm :rarm) + ;; Move fingers to initial pose + (send *ri* :move-hand arm + (send *baxter* :hand-grasp-pre-pose arm :opposed) 1000)) (send *ri* :gripper-servo-on arm) graspingp)) (:get-object-position @@ -269,14 +305,31 @@ (:try-to-pick-object (arm obj-pos &key (offset #f(0 0 0))) (let (graspingp) - (send *ri* :angle-vector-raw - (send *baxter* arm :inverse-kinematics - (make-coords :pos (v+ obj-pos offset) - :rpy #f(0 0 0)) - :use-gripper t - :rotation-axis :z) - 3000 (send *ri* :get-arm-controller arm) 0) - (send *ri* :wait-interpolation) + (if (eq arm :larm) + (progn + (send *ri* :angle-vector-raw + (send *baxter* arm :inverse-kinematics + (make-coords :pos (v+ obj-pos offset) + :rpy #f(0 0 0)) + :use-gripper t + :rotation-axis :z) + 3000 (send *ri* :get-arm-controller arm) 0) + (send *ri* :wait-interpolation)) + (progn + (send *ri* :angle-vector-raw + (send *baxter* arm :inverse-kinematics + (make-coords :pos (v+ obj-pos offset) + :rpy #f(0 0 0)) + :use-gripper t + :rotation-axis :z + :additional-check #'(lambda () + (< (send *baxter* arm :gripper-x :joint-angle) 110))) + 3000 (send *ri* :get-arm-controller arm) 0) + (send *ri* :wait-interpolation) + ;; Open fingers in bin + (send *ri* :move-hand arm + (send *baxter* :hand-grasp-pre-pose arm :cylindrical) + 1000))) ;; start the vacuum gripper after approaching to the object (ros::ros-info "[:try-to-pick-object] arm:~a start vacuum gripper" arm) (send *ri* :start-grasp arm) From a23bc897b053ad99f35fae98ab9de1c122a48063 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Thu, 15 Jun 2017 21:10:13 +0900 Subject: [PATCH 11/24] Use right_hand_left_camera in setup_for_stow --- jsk_arc2017_baxter/launch/setup/setup_for_stow.launch | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/jsk_arc2017_baxter/launch/setup/setup_for_stow.launch b/jsk_arc2017_baxter/launch/setup/setup_for_stow.launch index d4a4fd079..a577984f3 100644 --- a/jsk_arc2017_baxter/launch/setup/setup_for_stow.launch +++ b/jsk_arc2017_baxter/launch/setup/setup_for_stow.launch @@ -49,12 +49,12 @@ + - - + use_multiple_attention: false initial_pos: [0.6, -0.12, -0.25] @@ -68,7 +68,7 @@ - + keep_organized: true From e22c5140937af8c71c06eb80ba8857e4bf9bcf30 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Fri, 16 Jun 2017 19:41:14 +0900 Subject: [PATCH 12/24] Fix rarm pressure threshold --- jsk_arc2017_baxter/euslisp/lib/baxter-interface.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l index 99f13b280..6b55bd4da 100644 --- a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l @@ -225,7 +225,7 @@ (cond ((eq l/r :larm) (setq larm-pressure-threshold- (- min-pressure 5))) ((eq l/r :rarm) - (setq rarm-pressure-threshold- (- min-pressure 10)))))) + (setq rarm-pressure-threshold- (- min-pressure 15)))))) (send self :stop-grasp arm) (ros::ros-info "[:calib-pressure-threshold] Threshold r: ~a l: ~a" rarm-pressure-threshold- larm-pressure-threshold-))) From fb499b2546061dd01cf4bf125975d544dfe6035e Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Fri, 16 Jun 2017 19:44:47 +0900 Subject: [PATCH 13/24] Fix wait-interpolation-until-grasp for first interpolatingp nil --- jsk_arc2017_baxter/euslisp/lib/baxter-interface.l | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l index 6b55bd4da..ff414bfda 100644 --- a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l @@ -205,13 +205,14 @@ (:wait-interpolation-until-grasp (arm) (when (send self :simulation-modep) - (return-from :wait-interpolation-until-grasp (send self :wait-interpolation)) - ) + (return-from :wait-interpolation-until-grasp (send self :wait-interpolation))) + ;; wait for :interpolatingp + (unix::usleep 1000) (while (send self :interpolatingp) - (if (send self :graspingp arm) - (progn - (ros::ros-info "[:wait-interpolation-until-grasp] Grasping detected. Cancel angle vector: ~a" arm) - (send self :cancel-angle-vector))))) + (if (send self :graspingp arm) + (progn + (ros::ros-info "[:wait-interpolation-until-grasp] Grasping detected. Cancel angle vector: ~a" arm) + (send self :cancel-angle-vector))))) (:calib-pressure-threshold (&optional (arm :arms)) (send self :start-grasp arm) From 2942d50fd3ecaf75fb0371b3fe12e2614a9d1052 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Fri, 16 Jun 2017 19:46:41 +0900 Subject: [PATCH 14/24] Use wait-interpolation-until-grasp to prevent unnecessary push --- .../euslisp/lib/arc-interface.l | 32 +++++++++++-------- 1 file changed, 18 insertions(+), 14 deletions(-) diff --git a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l index f370a72f3..91b9c0a26 100755 --- a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l @@ -264,7 +264,7 @@ (unless graspingp ;; Fold fingers again (send *ri* :move-hand arm - (send *baxter* :hand-grasp-pose arm :cylindrical) 1000) + (send *baxter* :hand-grasp-pose arm :cylindrical :angle 100) 1000) (setq graspingp (send self :try-to-pick-object arm obj-pos :offset (float-vector 0 0 (- (* i -50) 30)))) @@ -314,8 +314,15 @@ :use-gripper t :rotation-axis :z) 3000 (send *ri* :get-arm-controller arm) 0) - (send *ri* :wait-interpolation)) + (send *ri* :wait-interpolation) + ;; start the vacuum gripper after approaching to the object + (ros::ros-info "[:try-to-pick-object] arm:~a start vacuum gripper" arm) + (send *ri* :start-grasp arm) + (unix::sleep 1)) (progn + ;; start the vacuum gripper before approaching to the object + (ros::ros-info "[:try-to-pick-object] arm:~a start vacuum gripper" arm) + (send *ri* :start-grasp arm) (send *ri* :angle-vector-raw (send *baxter* arm :inverse-kinematics (make-coords :pos (v+ obj-pos offset) @@ -325,15 +332,8 @@ :additional-check #'(lambda () (< (send *baxter* arm :gripper-x :joint-angle) 110))) 3000 (send *ri* :get-arm-controller arm) 0) - (send *ri* :wait-interpolation) - ;; Open fingers in bin - (send *ri* :move-hand arm - (send *baxter* :hand-grasp-pre-pose arm :cylindrical) - 1000))) - ;; start the vacuum gripper after approaching to the object - (ros::ros-info "[:try-to-pick-object] arm:~a start vacuum gripper" arm) - (send *ri* :start-grasp arm) - (unix::sleep 1) + ;; Wait until grasp to prevent tooth flying + (send *ri* :wait-interpolation-until-grasp arm))) (send *ri* :angle-vector-raw (send *baxter* arm :inverse-kinematics (make-coords :pos obj-pos @@ -341,7 +341,7 @@ :use-gripper t :rotation-axis :z) 3000 (send *ri* :get-arm-controller arm) 0) - (send *ri* :wait-interpolation) + (send *ri* :wait-interpolation-until-grasp arm) (setq graspingp (send *ri* :graspingp arm)) (ros::ros-info "[:try-to-pick-object] arm:~a graspingp: ~a" arm graspingp) (unless graspingp @@ -351,10 +351,14 @@ (if (send *baxter* arm :move-end-pos #f(0 0 -50) :local) (send *ri* :angle-vector-raw (send *baxter* :angle-vector) 3000 (send *ri* :get-arm-controller arm) 0)) - (send *ri* :wait-interpolation) + (send *ri* :wait-interpolation-until-grasp arm) (send *ri* :angle-vector-raw (send *baxter* :angle-vector temp-av) 3000 (send *ri* :get-arm-controller arm) 0) ;; revert baxter - (send *ri* :wait-interpolation))) + (send *ri* :wait-interpolation-until-grasp arm))) + (when (eq arm :rarm) + ;; Open fingers in bin + (send *ri* :move-hand arm + (send *baxter* :hand-grasp-pre-pose arm :cylindrical) 1000)) ;; lift object (ros::ros-info "[:try-to-pick-object] arm:~a lift the object" arm) (send *ri* :gripper-servo-off arm) From e796081236c0fa1185782d7b7da70a6bf2dba62a Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Fri, 16 Jun 2017 21:00:45 +0900 Subject: [PATCH 15/24] Apply IK additional check to avoid collision to bin wall --- jsk_arc2017_baxter/euslisp/lib/arc-interface.l | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l index 91b9c0a26..8910f27d9 100755 --- a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l @@ -240,8 +240,12 @@ :rpy #f(0 0 0)) :use-gripper t :rotation-axis :z - :additional-check #'(lambda () - (< (send *baxter* arm :gripper-x :joint-angle) 110)))) + :stop 200 + :additional-check + #'(lambda () + (and + (< (send *baxter* arm :gripper-x :joint-angle) 110) + (< -30 (send *baxter* arm :gripper-p :joint-angle) 30))))) (setq gripper-x (send *baxter* arm :gripper-x :joint-angle)) (send *baxter* :angle-vector prev-av) ;; First, move prismatic joint to target position @@ -329,8 +333,12 @@ :rpy #f(0 0 0)) :use-gripper t :rotation-axis :z - :additional-check #'(lambda () - (< (send *baxter* arm :gripper-x :joint-angle) 110))) + :stop 200 + :additional-check + #'(lambda () + (and + (< (send *baxter* arm :gripper-x :joint-angle) 110) + (< -30 (send *baxter* arm :gripper-p :joint-angle) 30)))) 3000 (send *ri* :get-arm-controller arm) 0) ;; Wait until grasp to prevent tooth flying (send *ri* :wait-interpolation-until-grasp arm))) From f1da8090bae1beb97ed68564d4f940deb07fa149 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Sat, 17 Jun 2017 13:27:39 +0900 Subject: [PATCH 16/24] Use only left astra mini --- jsk_arc2017_baxter/launch/setup/baxter.launch | 2 +- .../launch/setup/include/astra_hand.launch | 53 +++++++++++++++++++ .../launch/setup/setup_for_pick.launch | 10 ++-- .../launch/setup/setup_for_stow.launch | 8 +-- 4 files changed, 63 insertions(+), 10 deletions(-) create mode 100644 jsk_arc2017_baxter/launch/setup/include/astra_hand.launch diff --git a/jsk_arc2017_baxter/launch/setup/baxter.launch b/jsk_arc2017_baxter/launch/setup/baxter.launch index 5ba4dd934..4e26b3528 100644 --- a/jsk_arc2017_baxter/launch/setup/baxter.launch +++ b/jsk_arc2017_baxter/launch/setup/baxter.launch @@ -57,7 +57,7 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/jsk_arc2017_baxter/launch/setup/setup_for_pick.launch b/jsk_arc2017_baxter/launch/setup/setup_for_pick.launch index 5b2e970ee..41fad42c6 100644 --- a/jsk_arc2017_baxter/launch/setup/setup_for_pick.launch +++ b/jsk_arc2017_baxter/launch/setup/setup_for_pick.launch @@ -46,8 +46,8 @@ - + @@ -59,8 +59,8 @@ - - + + use_multiple_attention: false @@ -69,7 +69,7 @@ - + keep_organized: true @@ -79,7 +79,7 @@ - + diff --git a/jsk_arc2017_baxter/launch/setup/setup_for_stow.launch b/jsk_arc2017_baxter/launch/setup/setup_for_stow.launch index a577984f3..17e286b84 100644 --- a/jsk_arc2017_baxter/launch/setup/setup_for_stow.launch +++ b/jsk_arc2017_baxter/launch/setup/setup_for_stow.launch @@ -48,13 +48,13 @@ - - + + - + use_multiple_attention: false initial_pos: [0.6, -0.12, -0.25] @@ -68,7 +68,7 @@ - + keep_organized: true From cb16f720217c9eca2f7d5946db7c43317cdd645b Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Sat, 17 Jun 2017 15:05:02 +0900 Subject: [PATCH 17/24] Fix arc-interface to support left hand --- jsk_arc2017_baxter/euslisp/lib/arc-interface.l | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l index 8910f27d9..f49faf2cb 100755 --- a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l @@ -266,9 +266,10 @@ (dotimes (i n-trial) (dotimes (j n-trial-same-pos) (unless graspingp - ;; Fold fingers again - (send *ri* :move-hand arm - (send *baxter* :hand-grasp-pose arm :cylindrical :angle 100) 1000) + (when (eq arm :rarm) + ;; Fold fingers again + (send *ri* :move-hand arm + (send *baxter* :hand-grasp-pose arm :cylindrical :angle 100) 1000)) (setq graspingp (send self :try-to-pick-object arm obj-pos :offset (float-vector 0 0 (- (* i -50) 30)))) From cd4ea26d8828ff7234469815f684c17f8f920986 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Sat, 17 Jun 2017 15:22:01 +0900 Subject: [PATCH 18/24] Adjust rvizconfig to gripper-v6 --- jsk_arc2017_baxter/rvizconfig/pick.rviz | 84 +++++++++++++++++++++++-- jsk_arc2017_baxter/rvizconfig/stow.rviz | 84 +++++++++++++++++++++++-- 2 files changed, 156 insertions(+), 12 deletions(-) diff --git a/jsk_arc2017_baxter/rvizconfig/pick.rviz b/jsk_arc2017_baxter/rvizconfig/pick.rviz index 16ec7a8a3..8c8269db3 100644 --- a/jsk_arc2017_baxter/rvizconfig/pick.rviz +++ b/jsk_arc2017_baxter/rvizconfig/pick.rviz @@ -255,12 +255,36 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - right_gripper_vacuum_pad: + right_gripper_l_finger: Alpha: 1 Show Axes: false Show Trail: false Value: true - right_gripper_vacuum_pad_base: + right_gripper_l_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_pad_with_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_palm_endpoint: + Alpha: 1 + Show Axes: false + Show Trail: false + right_gripper_r_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_r_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_tube: Alpha: 1 Show Axes: false Show Trail: false @@ -768,12 +792,36 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - right_gripper_vacuum_pad: + right_gripper_l_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_l_finger_base: Alpha: 1 Show Axes: false Show Trail: false Value: true - right_gripper_vacuum_pad_base: + right_gripper_pad_with_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_palm_endpoint: + Alpha: 1 + Show Axes: false + Show Trail: false + right_gripper_r_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_r_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_tube: Alpha: 1 Show Axes: false Show Trail: false @@ -1056,12 +1104,36 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - right_gripper_vacuum_pad: + right_gripper_l_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_l_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_pad_with_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_palm_endpoint: + Alpha: 1 + Show Axes: false + Show Trail: false + right_gripper_r_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_r_finger_base: Alpha: 1 Show Axes: false Show Trail: false Value: true - right_gripper_vacuum_pad_base: + right_gripper_tube: Alpha: 1 Show Axes: false Show Trail: false diff --git a/jsk_arc2017_baxter/rvizconfig/stow.rviz b/jsk_arc2017_baxter/rvizconfig/stow.rviz index 85211726f..d8d42732b 100644 --- a/jsk_arc2017_baxter/rvizconfig/stow.rviz +++ b/jsk_arc2017_baxter/rvizconfig/stow.rviz @@ -239,12 +239,36 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - right_gripper_vacuum_pad: + right_gripper_l_finger: Alpha: 1 Show Axes: false Show Trail: false Value: true - right_gripper_vacuum_pad_base: + right_gripper_l_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_pad_with_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_palm_endpoint: + Alpha: 1 + Show Axes: false + Show Trail: false + right_gripper_r_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_r_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_tube: Alpha: 1 Show Axes: false Show Trail: false @@ -768,12 +792,36 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - right_gripper_vacuum_pad: + right_gripper_l_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_l_finger_base: Alpha: 1 Show Axes: false Show Trail: false Value: true - right_gripper_vacuum_pad_base: + right_gripper_pad_with_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_palm_endpoint: + Alpha: 1 + Show Axes: false + Show Trail: false + right_gripper_r_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_r_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_tube: Alpha: 1 Show Axes: false Show Trail: false @@ -1056,12 +1104,36 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - right_gripper_vacuum_pad: + right_gripper_l_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_l_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_pad_with_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_palm_endpoint: + Alpha: 1 + Show Axes: false + Show Trail: false + right_gripper_r_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_r_finger_base: Alpha: 1 Show Axes: false Show Trail: false Value: true - right_gripper_vacuum_pad_base: + right_gripper_tube: Alpha: 1 Show Axes: false Show Trail: false From 92bce86df2dd648aeb5349b5c3c0ccfe5c269a83 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Sat, 17 Jun 2017 16:08:03 +0900 Subject: [PATCH 19/24] Fix parenthesis and add comment to move-hand --- jsk_arc2017_baxter/euslisp/lib/baxter-interface.l | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l index ff414bfda..0bd65223b 100644 --- a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l @@ -110,6 +110,8 @@ (cons :controller-state "/gripper_front/limb/left/state") (cons :action-type control_msgs::FollowJointTrajectoryAction) (cons :joint-names (list "left_gripper_vacuum_pad_joint"))))) + + ;; Hand interface ;; based on naoqi-interface and fetch-interface (:move-hand (arm av tm &key (wait t)) @@ -138,8 +140,7 @@ (send res :error_code) (send res :error_string))) res) nil) - nil) - ) + nil)) (:get-arm-controller (arm) (cond ((eq arm :rarm) :rarm-controller) From 17b8239dd1b23841fd6ac18920f411b8987c9e02 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Sat, 17 Jun 2017 22:35:52 +0900 Subject: [PATCH 20/24] Adjust gravity compensation automatically --- jsk_arc2017_baxter/config/right_gripper_v6/gripper_mass.yaml | 3 +++ jsk_arc2017_baxter/launch/setup/baxter.launch | 5 +++++ 2 files changed, 8 insertions(+) create mode 100644 jsk_arc2017_baxter/config/right_gripper_v6/gripper_mass.yaml diff --git a/jsk_arc2017_baxter/config/right_gripper_v6/gripper_mass.yaml b/jsk_arc2017_baxter/config/right_gripper_v6/gripper_mass.yaml new file mode 100644 index 000000000..0eba0af95 --- /dev/null +++ b/jsk_arc2017_baxter/config/right_gripper_v6/gripper_mass.yaml @@ -0,0 +1,3 @@ +id: 131073 +command: 'configure' +args: '{ "urdf":{ "name": "right_gripper_mass", "link": [ { "name": "right_gripper_mass", "inertial": { "mass": { "value": 1.18 }, "origin": { "xyz": [0.0, 0.0, 0.15] } } } ] }}' diff --git a/jsk_arc2017_baxter/launch/setup/baxter.launch b/jsk_arc2017_baxter/launch/setup/baxter.launch index 4e26b3528..a84501871 100644 --- a/jsk_arc2017_baxter/launch/setup/baxter.launch +++ b/jsk_arc2017_baxter/launch/setup/baxter.launch @@ -16,6 +16,11 @@ + + From 7681814a29759fa9ee9d1b76b3ff2f126ac05394 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Sat, 17 Jun 2017 23:14:11 +0900 Subject: [PATCH 21/24] Add doc about gravity compensation --- doc/jsk_arc2017_baxter/index.rst | 1 + doc/jsk_arc2017_baxter/setup_gripper_v6.rst | 21 +++++++++++++++++++++ 2 files changed, 22 insertions(+) create mode 100644 doc/jsk_arc2017_baxter/setup_gripper_v6.rst diff --git a/doc/jsk_arc2017_baxter/index.rst b/doc/jsk_arc2017_baxter/index.rst index 11668d6d5..a823aad5b 100644 --- a/doc/jsk_arc2017_baxter/index.rst +++ b/doc/jsk_arc2017_baxter/index.rst @@ -10,6 +10,7 @@ jsk_arc2017_baxter arc2017_pick_trial usage_of_baxter + setup_gripper_v6 Testing ------- diff --git a/doc/jsk_arc2017_baxter/setup_gripper_v6.rst b/doc/jsk_arc2017_baxter/setup_gripper_v6.rst new file mode 100644 index 000000000..44bd89e4d --- /dev/null +++ b/doc/jsk_arc2017_baxter/setup_gripper_v6.rst @@ -0,0 +1,21 @@ +Gripper-v6 Setup +================ + +Adjust gravity compensation +--------------------------- + +Gripper-v6 is heavy (1.18kg), so we should adjust gravity compensation of Baxter. + +For now (2017/6/17), ``roslaunch jsk_arc2017_baxter baxter.launch`` does it by: + +.. code-block:: bash + + $ rostopic pub -1 /robot/end_effector/right_gripper/command baxter_core_msgs/EndEffectorCommand '{ id : 131073, command : "configure", args : "{ \"urdf\":{ \"name\": \"right_gripper_mass\", \"link\": [ { \"name\": \"right_gripper_mass\", \"inertial\": { \"mass\": { \"value\": 1.18 }, \"origin\": { \"xyz\": [0.0, 0.0, 0.15] } } } ] }}"}' + +If you want to change gripper, you should restore to the original setting by: + +.. code-block:: bash + + $ rostopic pub -1 /robot/end_effector/right_gripper/command baxter_core_msgs/EndEffectorCommand '{ id : 131073, command : "configure", args : "{ \"urdf\":{ \"name\": \"right_gripper_mass\", \"link\": [ { \"name\": \"right_gripper_mass\", \"inertial\": { \"mass\": { \"value\": 0 }, \"origin\": { \"xyz\": [0.0, 0.0, 0.0] } } } ] }}"}' + +More information about gripper customization of Baxter is on `official page `_ From 190d1f136aa897cef2fafb0c6781de92f4b54437 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Sun, 18 Jun 2017 00:46:32 +0900 Subject: [PATCH 22/24] Add how to move gripper-v6 --- doc/jsk_arc2017_baxter/usage_of_baxter.rst | 29 ++++++++++++++++++++-- 1 file changed, 27 insertions(+), 2 deletions(-) diff --git a/doc/jsk_arc2017_baxter/usage_of_baxter.rst b/doc/jsk_arc2017_baxter/usage_of_baxter.rst index 234895dc1..16b75bdfb 100644 --- a/doc/jsk_arc2017_baxter/usage_of_baxter.rst +++ b/doc/jsk_arc2017_baxter/usage_of_baxter.rst @@ -39,7 +39,23 @@ arc-interface function APIs .. code-block:: lisp - (send *baxter* :rotate-gripper :larm 90) + (send *baxter* :rotate-gripper :larm 90 :relative nil) + + +- slide right gripper + + .. code-block:: lisp + + (send *baxter* :slide-gripper :rarm 50 :relative nil) + + +- move fingers in right gripper + + .. code-block:: lisp + + (send *baxter* :hand :rarm :angle-vector #f(90 90)) + (send *baxter* :hand-grasp-pre-pose :rarm :opposed) + (send *baxter* :hand-grasp-pose :rarm :cylindrical) - send initial pose for arc2017 @@ -53,4 +69,13 @@ arc-interface function APIs .. code-block:: lisp - (send *ri* :send-av) + (send *ti* :send-av) + + +- send current hand joint angles of robot model to real robot + + .. code-block:: lisp + + (send *ri* :move-hand :rarm (send *baxter* :hand :rarm :angle-vector) 1000) + + From 7b271ce50928acf2c65888daf045e37c08f173f7 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Tue, 20 Jun 2017 11:33:57 +0900 Subject: [PATCH 23/24] Revert mvit-env and mvit-rb --- jsk_arc2017_baxter/euslisp/lib/arc-interface.l | 4 ++-- jsk_arc2017_baxter/euslisp/lib/baxter-interface.l | 5 +++-- jsk_arc2017_baxter/euslisp/lib/pick-interface.l | 4 ++-- jsk_arc2017_baxter/euslisp/lib/stow-interface.l | 4 ++-- 4 files changed, 9 insertions(+), 8 deletions(-) diff --git a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l index f49faf2cb..5adc41d61 100755 --- a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l @@ -547,8 +547,8 @@ (defun jsk_arc2017_baxter::arc-init (&key (ctype :default-controller) (moveit nil)) (let (mvit-env mvit-rb) (when moveit - (setq mvit-env (instance jsk_arc2017_baxter::baxter-moveit-environment)) - (setq mvit-rb (instance jsk_arc2017_baxter::baxter-robot :init))) + (setq mvit-env (instance jsk_2016_01_baxter_apc::baxter-moveit-environment)) + (setq mvit-rb (instance jsk_2016_01_baxter_apc::baxter-robot :init))) (unless (boundp '*ri*) (setq *ri* (instance jsk_arc2017_baxter::baxter-interface :init :type ctype :moveit-environment mvit-env diff --git a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l index 0bd65223b..941db229b 100644 --- a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l @@ -4,6 +4,7 @@ (require "package://baxtereus/baxter-interface.l") (require "package://jsk_2015_05_baxter_apc/euslisp/jsk_2015_05_baxter_apc/util.l") +(require "package://jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/baxter-interface.l") (require "package://jsk_arc2017_baxter/euslisp/lib/baxter.l") (require "package://jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/util.l") @@ -23,8 +24,8 @@ (:init (&rest args &key ((:moveit-environment mvit-env) - (instance jsk_arc2017_baxter::baxter-moveit-environment)) - ((:moveit-robot mvit-rb) (instance jsk_arc2017_baxter::baxter-robot :init)) + (instance jsk_2016_01_baxter_apc::baxter-moveit-environment)) + ((:moveit-robot mvit-rb) (instance jsk_2016_01_baxter_apc::baxter-robot :init)) &allow-other-keys) ;; initialize controllers (send-super* :init :joint-states-queue-size 3 diff --git a/jsk_arc2017_baxter/euslisp/lib/pick-interface.l b/jsk_arc2017_baxter/euslisp/lib/pick-interface.l index 17efd184a..22a38b943 100644 --- a/jsk_arc2017_baxter/euslisp/lib/pick-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/pick-interface.l @@ -194,8 +194,8 @@ (defun jsk_arc2017_baxter::pick-init (&key (ctype :default-controller) (moveit nil)) (let (mvit-env mvit-rb) (when moveit - (setq mvit-env (instance jsk_arc2017_baxter::baxter-moveit-environment)) - (setq mvit-rb (instance jsk_arc2017_baxter::baxter-robot :init))) + (setq mvit-env (instance jsk_2016_01_baxter_apc::baxter-moveit-environment)) + (setq mvit-rb (instance jsk_2016_01_baxter_apc::baxter-robot :init))) (unless (boundp '*ri*) (setq *ri* (instance jsk_arc2017_baxter::baxter-interface :init :type ctype :moveit-environment mvit-env diff --git a/jsk_arc2017_baxter/euslisp/lib/stow-interface.l b/jsk_arc2017_baxter/euslisp/lib/stow-interface.l index c9434ba35..204c0ab05 100644 --- a/jsk_arc2017_baxter/euslisp/lib/stow-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/stow-interface.l @@ -168,8 +168,8 @@ (defun jsk_arc2017_baxter::stow-init (&key (ctype :default-controller) (moveit nil)) (let (mvit-env mvit-rb) (when moveit - (setq mvit-env (instance jsk_arc2017_baxter::baxter-moveit-environment)) - (setq mvit-rb (instance jsk_arc2017_baxter::baxter-robot :init))) + (setq mvit-env (instance jsk_2016_01_baxter_apc::baxter-moveit-environment)) + (setq mvit-rb (instance jsk_2016_01_baxter_apc::baxter-robot :init))) (unless (boundp '*ri*) (setq *ri* (instance jsk_arc2017_baxter::baxter-interface :init :type ctype :moveit-environment mvit-env From e0b9585fb01b2a5058b49269be90bd1b9f8b9b5c Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Tue, 20 Jun 2017 12:12:30 +0900 Subject: [PATCH 24/24] Don't load old robot model --- jsk_arc2017_baxter/euslisp/lib/arc-interface.l | 1 - jsk_arc2017_baxter/euslisp/lib/pick-interface.l | 2 -- jsk_arc2017_baxter/euslisp/lib/stow-interface.l | 2 -- 3 files changed, 5 deletions(-) diff --git a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l index 5adc41d61..3b476e02e 100755 --- a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l @@ -2,7 +2,6 @@ (require "package://jsk_2015_05_baxter_apc/euslisp/jsk_2015_05_baxter_apc/util.l") (require "package://jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/util.l") -(require "package://jsk_arc2017_baxter/euslisp/lib/baxter.l") (require "package://jsk_arc2017_baxter/euslisp/lib/baxter-interface.l") (ros::load-ros-manifest "jsk_arc2017_baxter") diff --git a/jsk_arc2017_baxter/euslisp/lib/pick-interface.l b/jsk_arc2017_baxter/euslisp/lib/pick-interface.l index 22a38b943..3babbd5da 100644 --- a/jsk_arc2017_baxter/euslisp/lib/pick-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/pick-interface.l @@ -2,8 +2,6 @@ (require "package://jsk_2015_05_baxter_apc/euslisp/jsk_2015_05_baxter_apc/util.l") (require "package://jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/util.l") -(require "package://jsk_arc2017_baxter/euslisp/lib/baxter.l") -(require "package://jsk_arc2017_baxter/euslisp/lib/baxter-interface.l") (require "package://jsk_arc2017_baxter/euslisp/lib/arc-interface.l") (ros::load-ros-manifest "jsk_arc2017_baxter") diff --git a/jsk_arc2017_baxter/euslisp/lib/stow-interface.l b/jsk_arc2017_baxter/euslisp/lib/stow-interface.l index 204c0ab05..54eb2051a 100644 --- a/jsk_arc2017_baxter/euslisp/lib/stow-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/stow-interface.l @@ -2,8 +2,6 @@ (require "package://jsk_2015_05_baxter_apc/euslisp/jsk_2015_05_baxter_apc/util.l") (require "package://jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/util.l") -(require "package://jsk_arc2017_baxter/euslisp/lib/baxter.l") -(require "package://jsk_arc2017_baxter/euslisp/lib/baxter-interface.l") (require "package://jsk_arc2017_baxter/euslisp/lib/arc-interface.l") (ros::load-ros-manifest "jsk_arc2017_baxter")