Move entire baxterlgv7

jsk_arc2017_baxter/euslisp/sanity-check-gripper-baxterlgv7.l

@@ -0,0 +1,66 @@
+#!/usr/bin/env roseus
+
+(ros::roseus "sanity_check_gripper_baxterlgv7" :anonymous nil)
+
+(require "package://jsk_arc2017_baxter/euslisp/lib/baxterlgv7-interface.l")
+
+(defun sanity-check-gripper-baxterlgv7
+  ()
+  (jsk_arc2017_baxter::baxterlgv7-init)
+
+  ;; sanity check for fingers
+  (let ((l-finger-av-init (send *ri* :get-real-finger-av :larm))
+        (r-finger-av-init (send *ri* :get-real-finger-av :rarm))
+        l-finger-av r-finger-av)
+    (setq l-finger-av (v+ l-finger-av-init (float-vector 45 20 20)))
+    (setq r-finger-av (v+ r-finger-av-init (float-vector 45 45)))
+    (send *ri* :move-hand :larm l-finger-av 2000 :wait nil)
+    (send *ri* :move-hand :rarm r-finger-av 2000 :wait t)
+
+    (setq l-finger-av (v+ l-finger-av-init (float-vector -45 -20 -20)))
+    (setq r-finger-av (v+ r-finger-av-init (float-vector -45 -45)))
+    (send *ri* :move-hand :larm l-finger-av 2000 :wait nil)
+    (send *ri* :move-hand :rarm r-finger-av 2000 :wait t)
+
+    (send *ri* :move-hand :larm l-finger-av-init 2000 :wait nil)
+    (send *ri* :move-hand :rarm r-finger-av-init 2000 :wait t)
+    )
+
+  ;; sanity check for prismatic joint
+  (let ((l-gripper-x-init (send *baxter* :larm :gripper-x :joint-angle))
+        (r-gripper-x-init (send *baxter* :rarm :gripper-x :joint-angle))
+        (l-gripper-p-init (send *baxter* :larm :gripper-p :joint-angle))
+        (r-gripper-p-init (send *baxter* :rarm :gripper-p :joint-angle)))
+    (send *baxter* :slide-gripper :larm 30 :relative t)
+    (send *baxter* :slide-gripper :rarm 30 :relative t)
+    (send *baxter* :rotate-gripper :larm 30 :relative t)
+    (send *baxter* :rotate-gripper :rarm 30 :relative t)
+    (send *ri* :angle-vector-raw (send *baxter* :angle-vector))
+    (send *ri* :wait-interpolation)
+
+    (send *baxter* :slide-gripper :larm -60 :relative t)
+    (send *baxter* :slide-gripper :rarm -60 :relative t)
+    (send *baxter* :rotate-gripper :larm -60 :relative t)
+    (send *baxter* :rotate-gripper :rarm -60 :relative t)
+    (send *ri* :angle-vector-raw (send *baxter* :angle-vector))
+    (send *ri* :wait-interpolation)
+
+    (send *baxter* :slide-gripper :larm l-gripper-x-init :relative nil)
+    (send *baxter* :slide-gripper :rarm r-gripper-x-init :relative nil)
+    (send *baxter* :rotate-gripper :larm l-gripper-p-init :relative nil)
+    (send *baxter* :rotate-gripper :rarm r-gripper-p-init :relative nil)
+    (send *ri* :angle-vector-raw (send *baxter* :angle-vector))
+    (send *ri* :wait-interpolation)
+    )
+
+  ;; sanity check for suction
+  (dolist (arm (list :larm :rarm))
+    (send *ri* :start-grasp arm)
+    (unix::sleep 1)
+    (send *ri* :stop-grasp arm)
+    (unix::sleep 1)
+    )
+  )
+
+(sanity-check-gripper-baxterlgv7)
+(exit)

jsk_arc2017_baxter/launch/setup/baxter.launch

@@ -58,7 +58,7 @@
         pkg="jsk_baxter_startup" type="xdisplay_image_topic.py"
         args="/desktop_bg_publisher/output" />
 
-  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/baxter_moveit.launch" if="$(arg moveit)">
+  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/moveit/baxter/baxter_moveit.launch" if="$(arg moveit)">
     <arg name="load_robot_description" value="false" />
     <arg name="debug" value="$(arg debug)" />
   </include>

jsk_arc2017_baxter/launch/setup/baxterlgv7.launch

@@ -0,0 +1,69 @@
+<launch>
+
+  <arg name="debug" default="false" />
+  <arg name="moveit" default="true" />
+  <arg name="pick" default="true" doc="pick_task: true, stow_task: false" />
+
+  <!-- get machine tag -->
+  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/baxter.machine" />
+
+  <!-- custom baxter params -->
+  <param name="/robot_description"
+         command="$(find xacro)/xacro.py $(find jsk_arc2017_baxter)/robots/baxterlgv7.xacro" />
+  <node name="robot_state_publisher"
+        pkg="robot_state_publisher" type="robot_state_publisher">
+    <remap from="joint_states" to="/robot/joint_states"/>
+  </node>
+  <rosparam param="/baxter_joint_trajectory"
+            file="$(find jsk_2015_05_baxter_apc)/config/baxter_joint_trajectory.yaml"/>
+
+  <!-- right gripper-v6 and vacuum cleaners -->
+  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/setup_gripper_baxterlgv7.launch" />
+
+  <!-- voice -->
+  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/sound_play.launch" />
+
+  <!-- baxter startup -->
+  <include file="$(find jsk_baxter_startup)/baxter.launch">
+    <arg name="launch_servo" value="true"/>
+    <arg name="launch_joint_trajectory" value="true"/>
+    <arg name="sanity_check_joint_trajectory" value="true" />
+    <arg name="launch_gripper_action" value="false"/>
+    <arg name="launch_openni" value="false"/>
+    <arg name="launch_kinect2" value="false"/>
+    <arg name="launch_voice_echo" value="true"/>
+    <arg name="launch_moveit" value="false"/>
+    <arg name="launch_teleop" value="false"/>
+    <arg name="launch_tweet" value="false"/>
+    <arg name="launch_mongodb" value="false"/>
+    <arg name="launch_wrench" value="false"/>
+    <arg name="launch_time_signal" value="false"/>
+    <arg name="start_openni" value="false"/>
+    <arg name="USER_NAME" value="false"/>
+  </include>
+
+
+  <!-- picking source location: shelf/tote -->
+  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/setup_source_location.xml">
+    <arg name="pick" value="$(arg pick)" />
+  </include>
+
+  <node name="desktop_bg_publisher"
+        pkg="jsk_perception" type="image_publisher.py">
+    <rosparam subst_value="true">
+      file_name: $(find jsk_apc2016_common)/resource/wallpapers/wallpaper_black.png
+    </rosparam>
+  </node>
+  <node name="xdisplay_image_topic"
+        pkg="jsk_baxter_startup" type="xdisplay_image_topic.py"
+        args="/desktop_bg_publisher/output" />
+
+  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/moveit/baxterlgv7/baxterlgv7_moveit.launch" if="$(arg moveit)">
+    <arg name="load_robot_description" value="false" />
+    <arg name="debug" value="$(arg debug)" />
+  </include>
+
+  <!-- axis camera -->
+  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/axis_camera.launch" />
+
+</launch>

jsk_arc2017_baxter/launch/setup/include/arduinos_baxterlgv7.launch

@@ -0,0 +1,31 @@
+<launch>
+
+  <!-- Arduino -->
+  <node name="rosserial_node_0"
+        pkg="rosserial_python" type="serial_node.py"
+        respawn="true"
+        machine="c2">
+    <param name="port" type="string" value="/dev/arduino0" />
+    <param name="baud" value="115200" />
+  </node>
+
+  <node name="rosserial_node_1"
+        pkg="rosserial_python" type="serial_node.py"
+        respawn="true"
+        machine="c2">
+    <param name="port" type="string" value="/dev/arduino1" />
+    <param name="baud" value="115200" />
+  </node>
+
+  <!-- Republish gripper sensor states -->
+  <node name="rgripper_sensors_republisher"
+        pkg="jsk_arc2017_baxter" type="republish_gripper_sensor_states.py"
+        machine="c2">
+    <remap from="~input" to="/rgripper_sensors" />
+    <remap from="~proximity_array" to="/gripper_front/limb/right/proximity_array" />
+    <remap from="~pressure/state" to="/gripper_front/limb/right/pressure/state" />
+    <remap from="~flex/right/state" to="/gripper_front/limb/right/flex/right/state" />
+    <remap from="~flex/left/state" to="/gripper_front/limb/right/flex/left/state" />
+  </node>
+
+</launch>

jsk_arc2017_baxter/launch/setup/include/baxter_moveit.launch → jsk_arc2017_baxter/launch/setup/include/moveit/baxter/baxter_moveit.launch

@@ -4,12 +4,14 @@
   <arg name="debug" default="false" />
 
   <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
-  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/planning_context.launch">
+  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/moveit/common/planning_context.launch">
+    <arg name="robot" value="baxter"/>
     <arg name="load_robot_description" value="$(arg load_robot_description)"/>
   </include>
 
   <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
-  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/move_group.launch">
+  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/moveit/common/move_group.launch">
+    <arg name="robot" value="baxter"/>
     <arg name="allow_trajectory_execution" value="true"/>
     <arg name="info" value="$(arg debug)"/>
     <arg name="debug" value="$(arg debug)"/>

jsk_arc2017_baxter/launch/setup/include/trajectory_execution.launch.xml → jsk_arc2017_baxter/launch/setup/include/moveit/baxter/baxter_trajectory_execution.launch.xml

@@ -13,6 +13,7 @@
 
   <!-- Load the robot specific controller manager; this sets the moveit_controller_manager ROS parameter -->
   <arg name="moveit_controller_manager" default="baxter" />
-  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" />
-
+  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/moveit/common/moveit_controller_manager.launch.xml">
+    <arg name="robot" value="$(arg moveit_controller_manager)"/>
+  </include>
 </launch>

jsk_arc2017_baxter/launch/setup/include/moveit/baxterlgv7/baxterlgv7_moveit.launch

@@ -0,0 +1,20 @@
+<launch>
+
+  <arg name="load_robot_description" default="false" />
+  <arg name="debug" default="false" />
+
+  <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
+  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/moveit/common/planning_context.launch">
+    <arg name="robot" value="baxterlgv7"/>
+    <arg name="load_robot_description" value="$(arg load_robot_description)"/>
+  </include>
+
+  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
+  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/moveit/common/move_group.launch">
+    <arg name="robot" value="baxterlgv7"/>
+    <arg name="allow_trajectory_execution" value="true"/>
+    <arg name="info" value="$(arg debug)"/>
+    <arg name="debug" value="$(arg debug)"/>
+  </include>
+
+</launch>

jsk_arc2017_baxter/launch/setup/include/moveit/baxterlgv7/baxterlgv7_trajectory_execution.launch.xml

@@ -0,0 +1,19 @@
+<launch>
+
+  <!-- This file makes it easy to include the settings for trajectory execution  -->
+
+  <!-- Flag indicating whether MoveIt! is allowed to load/unload  or switch controllers -->
+  <arg name="moveit_manage_controllers" default="true"/>
+  <param name="moveit_manage_controllers" value="$(arg moveit_manage_controllers)"/>
+
+  <!-- When determining the expected duration of a trajectory, this multiplicative factor is applied to get the allowed duration of execution -->
+  <param name="trajectory_execution/allowed_execution_duration_scaling" value="1.2"/> <!-- default 1.2 -->
+  <!-- Allow more than the expected execution time before triggering a trajectory cancel (applied after scaling) -->
+  <param name="trajectory_execution/allowed_goal_duration_margin" value="0.5"/> <!-- default 0.5 -->
+
+  <!-- Load the robot specific controller manager; this sets the moveit_controller_manager ROS parameter -->
+  <arg name="moveit_controller_manager" default="baxterlgv7" />
+  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/moveit/common/moveit_controller_manager.launch.xml">
+    <arg name="robot" value="$(arg moveit_controller_manager)"/>
+  </include>
+</launch>

jsk_arc2017_baxter/launch/setup/include/move_group.launch → jsk_arc2017_baxter/launch/setup/include/moveit/common/move_group.launch

@@ -1,5 +1,7 @@
 <launch>
 
+  <arg name="robot" default="baxter"/>
+
   <!-- GDB Debug Option -->
   <arg name="debug" default="false" />
   <arg unless="$(arg debug)" name="launch_prefix" value="" />
@@ -18,12 +20,14 @@
   <arg name="publish_monitored_planning_scene" default="true"/>
 
   <!-- Planning Functionality -->
-  <include ns="move_group" file="$(find jsk_arc2017_baxter)/launch/setup/include/ompl_planning_pipeline.launch" />
+  <include ns="move_group" file="$(find jsk_arc2017_baxter)/launch/setup/include/moveit/common/ompl_planning_pipeline.launch">
+    <arg name="robot" value="$(arg robot)"/>
+  </include>
 
   <!-- Trajectory Execution Functionality -->
-  <include ns="move_group" file="$(find jsk_arc2017_baxter)/launch/setup/include/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
+  <include ns="move_group" file="$(find jsk_arc2017_baxter)/launch/setup/include/moveit/$(arg robot)/$(arg robot)_trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
     <arg name="moveit_manage_controllers" value="true" />
-    <arg name="moveit_controller_manager" value="baxter"/>
+    <arg name="moveit_controller_manager" value="$(arg robot)"/>
   </include>
 
   <!-- Start the actual move_group node/action server -->

jsk_arc2017_baxter/launch/setup/include/baxter_moveit_controller_manager.launch.xml → jsk_arc2017_baxter/launch/setup/include/moveit/common/moveit_controller_manager.launch.xml

@@ -1,5 +1,7 @@
 <launch>
 
+  <arg name="robot" default="baxter"/>
+
   <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
   <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
   <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
@@ -16,6 +18,6 @@
   <param name="use_controller_manager" value="$(arg use_controller_manager)" />
 
   <!-- load controller_list and controller_manager_ns -->
-  <rosparam file="$(find jsk_arc2017_baxter)/robots/moveit_config/baxter_controllers.yaml"/>
+  <rosparam file="$(find jsk_arc2017_baxter)/robots/moveit_config/$(arg robot)/controllers.yaml"/>
 
 </launch>

jsk_arc2017_baxter/launch/setup/include/ompl_planning_pipeline.launch → jsk_arc2017_baxter/launch/setup/include/moveit/common/ompl_planning_pipeline.launch

@@ -1,5 +1,7 @@
 <launch>
 
+  <arg name="robot" default="baxter"/>
+
   <!-- OMPL Plugin for MoveIt! -->
   <arg name="planning_plugin" value="ompl_interface/OMPLPlanner" />
 
@@ -18,6 +20,6 @@
   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
 
   <rosparam command="load" file="$(find baxter_moveit_config)/config/kinematics.yaml"/>
-  <rosparam command="load" file="$(find jsk_arc2017_baxter)/robots/moveit_config/ompl_planning.yaml"/>
+  <rosparam command="load" file="$(find jsk_arc2017_baxter)/robots/moveit_config/$(arg robot)/ompl_planning.yaml"/>
 
 </launch>

jsk_arc2017_baxter/launch/setup/include/planning_context.launch → jsk_arc2017_baxter/launch/setup/include/moveit/common/planning_context.launch

@@ -1,20 +1,23 @@
 <launch>
+
+  <arg name="robot" default="baxter"/>
+
   <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
   <arg name="load_robot_description" default="false"/>
 
   <!-- The name of the parameter under which the URDF is loaded -->
   <arg name="robot_description" default="robot_description"/>
 
   <!-- Load universal robot description format (URDF) -->
-  <param if="$(arg load_robot_description)" name="$(arg robot_description)" command="rosrun xacro xacro --inorder $(find jsk_arc2017_baxter)/robots/baxter.xacro"/>
+  <param if="$(arg load_robot_description)" name="$(arg robot_description)" command="rosrun xacro xacro --inorder $(find jsk_arc2017_baxter)/robots/$(arg robot).xacro"/>
 
   <!-- The semantic description that corresponds to the URDF -->
   <param name="$(arg robot_description)_semantic"
-      command="rosrun xacro xacro --inorder $(find jsk_arc2017_baxter)/robots/moveit_config/baxter.srdf.xacro"/>
+      command="rosrun xacro xacro --inorder $(find jsk_arc2017_baxter)/robots/moveit_config/$(arg robot)/$(arg robot).srdf.xacro"/>
 
   <!-- Load updated joint limits (override information from URDF) -->
   <group ns="$(arg robot_description)_planning">
-    <rosparam command="load" file="$(find jsk_arc2017_baxter)/robots/moveit_config/joint_limits.yaml"/>
+    <rosparam command="load" file="$(find jsk_arc2017_baxter)/robots/moveit_config/$(arg robot)/joint_limits.yaml"/>
   </group>
 
   <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->

jsk_arc2017_baxter/launch/setup/include/setup_gripper_baxterlgv7.launch

@@ -0,0 +1,27 @@
+<launch>
+
+  <arg name="define_machine" default="false" />
+  <group if="$(arg define_machine)">
+    <include file="$(find jsk_arc2017_baxter)/launch/setup/include/baxter.machine" />
+  </group>
+
+  <!-- adjust gravity compensation for gripper-v6 -->
+  <node pkg="rostopic" type="rostopic" name="adjust_compensation_right"
+        args="pub -1 /robot/end_effector/right_gripper/command baxter_core_msgs/EndEffectorCommand
+              -f $(find jsk_arc2017_baxter)/robots/right_gripper_v6/gripper_mass.yaml"
+        output="screen"/>
+
+  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/arduinos_baxterlgv7.launch" />
+  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/gripper_v6_dxl_controller.launch">
+    <arg name="launch_right" value="true" />
+    <arg name="launch_left" value="false" />
+  </include>
+  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/gripper_v6_ros_control.launch">
+    <arg name="launch_right" value="true" />
+    <arg name="launch_left" value="false" />
+  </include>
+
+  <node name="sanity_check_gripper_baxterlgv7"
+        pkg="jsk_arc2017_baxter" type="sanity-check-gripper-baxterlgv7.l" />
+
+</launch>

jsk_arc2017_baxter/robots/left_gripper_v7.xacro

@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="left_vacuum_gripper">
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="left_gripper_v7">
   <xacro:include filename="$(find jsk_2016_01_baxter_apc)/robots/common_constants.xacro"/>
   <!-- Gazebo Tags -->
   <gazebo reference="left_gripper_base">

jsk_arc2017_baxter/robots/moveit_config/baxter.srdf.xacro → jsk_arc2017_baxter/robots/moveit_config/baxter/baxter.srdf.xacro

@@ -2,12 +2,12 @@
 <robot name="baxter" xmlns:xacro="http://www.ros.org/wiki/xacro">
   <xacro:arg name="left_tip_name" default="left_gripper_pad_with_base"/>
   <xacro:arg name="right_tip_name" default="right_gripper_pad_with_base"/>
-  <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/baxter_base.srdf.xacro" />
+  <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/baxter/baxter_base.srdf.xacro" />
   <xacro:baxter_base left_tip_name="$(arg left_tip_name)" right_tip_name="$(arg right_tip_name)"/>
   <!--Left End Effector Collisions-->
-  <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/gripper_v6.srdf.xacro" />
+  <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/grippers/gripper_v6.srdf.xacro" />
   <xacro:vacuum_gripper side="left"/>
   <!--Right End Effector Collisions-->
-  <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/gripper_v6.srdf.xacro" />
+  <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/grippers/gripper_v6.srdf.xacro" />
   <xacro:vacuum_gripper side="right"/>
 </robot>

jsk_arc2017_baxter/robots/moveit_config/baxterlgv7/baxterlgv7.srdf.xacro

@@ -0,0 +1,13 @@
+<?xml version="1.0" ?>
+<robot name="baxter" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:arg name="left_tip_name" default="left_gripper_pad_with_base"/>
+  <xacro:arg name="right_tip_name" default="right_gripper_pad_with_base"/>
+  <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/baxterlgv7/baxterlgv7_base.srdf.xacro" />
+  <xacro:baxterlgv7_base left_tip_name="$(arg left_tip_name)" right_tip_name="$(arg right_tip_name)"/>
+  <!--Left End Effector Collisions-->
+  <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/grippers/gripper_v7.srdf.xacro" />
+  <xacro:vacuum_gripper side="left"/>
+  <!--Right End Effector Collisions-->
+  <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/grippers/gripper_v6.srdf.xacro" />
+  <xacro:vacuum_gripper side="right"/>
+</robot>