Add gripper-v6 to baxter right hand

doc/jsk_arc2017_baxter/index.rst

@@ -10,6 +10,7 @@ jsk_arc2017_baxter
 
    arc2017_pick_trial
    usage_of_baxter
+   setup_gripper_v6
 
 Testing
 -------

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 <http://sdk.rethinkrobotics.com/wiki/Gripper_Customization>`_

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)
+
+

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

jsk_arc2017_baxter/.gitignore

@@ -0,0 +1,3 @@
+robots/baxter.urdf
+robots/baxter.dae
+robots/baxter.l

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 ##
@@ -72,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()

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

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] } } } ] }}'

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