Refactor to re-use code

jsk_panda_robot/README.md

@@ -49,6 +49,35 @@
    ```
 
 
+## Running single Panda
+### Boot robot
+1. Please turn on the controller box and unlock joints by accessing desk.
+### Via roseus
+1. Start controller on controller PC:
+   ```bash
+   ssh leus@dual-panda1.jsk.imi.i.u-tokyo.ac.jp  # Or ssh leus@dual-panda2.jsk.imi.i.u-tokyo.ac.jp
+   roslaunch jsk_panda_startup panda.launch robot_ip:=<IP OF TARGET ROBOT>
+   ```
+
+2. Controlling single Panda via roseus:
+   1. Setting up network:
+      ```bash
+      rossetmaster dual-panda1.jsk.imi.i.u-tokyo.ac.jp  # Or rossetmaster dual-panda2.jsk.imi.i.u-tokyo.ac.jp
+      rossetip
+      ```
+   2. Execute following script in roseus:
+      ```lisp
+      (load "package://panda_eus/euslisp/panda-interface.l")
+      (panda-init)
+      (send *robot* :angle-vector (send *robot* :reset-pose))
+      (when (send *ri* :check-error)
+        (send *ri* :recover-error))
+      (send *ri* :angle-vector (send *robot* :angle-vector) 3000)
+      ```
+      - Notice
+        - `(send *ri* :recover-error)` is required every time when you press and release the black switch (`activated` -> `monitored stop` -> `activated`).
+
+
 ## Running Dual-Panda
 ### Boot robot
 1. Please turn on the controller box and unlock joints by accessing desk.
@@ -76,7 +105,9 @@
         (send *ri* :recover-error))
       (send *ri* :angle-vector (send *robot* :angle-vector) 3000)
       ```
-      `(send *ri* :recover-error)` is required every time when you press and release the black switch (`activated` -> `monitored stop` -> `activated`).
+      - Notice
+        - `(send *ri* :recover-error)` is required every time when you press and release the black switch (`activated` -> `monitored stop` -> `activated`).
+        - `dual_panda`'s `end-coords`, `reset-pose`, and `reset-manip-pose` are different from single `panda`'s ones for historical reasons.
 #### Record/play rosbag
 ```bash
 roslaunch jsk_panda_startup dual_panda1_record.launch  # Or roslaunch jsk_panda_startup dual_panda2_record.launch

jsk_panda_robot/jsk_panda_startup/launch/franka.launch

@@ -0,0 +1,19 @@
+<?xml version="1.0" ?>
+
+<launch>
+  <arg name="robot_ip"     default="192.168.0.2"/>
+  <arg name="robot"        default="panda" />
+  <arg name="arm_id"       default="$(arg robot)" />
+  <arg name="load_gripper" default="true" />
+  <arg name="xacro_args"   default="" />
+
+  <include file="$(find franka_control)/launch/franka_control.launch">
+    <arg name="robot_ip"     value="$(arg robot_ip)" />
+    <arg name="robot"        value="$(arg robot)" />
+    <arg name="arm_id"       value="$(arg arm_id)" />
+    <arg name="load_gripper" value="$(arg load_gripper)" />
+    <arg name="xacro_args"   value="$(arg xacro_args)" />
+  </include>
+
+  <node name="position_joint_trajectory_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="position_joint_trajectory_controller" />
+</launch>

jsk_panda_robot/jsk_panda_startup/launch/panda.launch

@@ -7,14 +7,11 @@
   <arg name="load_gripper" default="true" />
   <arg name="xacro_args"   default="" />
 
-  <include file="$(find franka_control)/launch/franka_control.launch">
+  <include file="$(find jsk_panda_startup)/launch/franka.launch">
     <arg name="robot_ip"     value="$(arg robot_ip)" />
     <arg name="robot"        value="$(arg robot)" />
     <arg name="arm_id"       value="$(arg arm_id)" />
     <arg name="load_gripper" value="$(arg load_gripper)" />
     <arg name="xacro_args"   value="$(arg xacro_args)" />
   </include>
-
-  <param name="robot_description" command="$(find xacro)/xacro $(find panda_eus)/models/panda.urdf.xacro hand:=$(arg load_gripper) arm_id:=$(arg arm_id) $(arg xacro_args)"/>
-  <node name="position_joint_trajectory_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="position_joint_trajectory_controller" />
 </launch>

jsk_panda_robot/panda_eus/.gitignore

@@ -1,2 +1,4 @@
 models/dual_panda.l
 models/dual_panda.urdf
+models/panda.l
+models/panda.urdf

jsk_panda_robot/panda_eus/CMakeLists.txt

@@ -16,7 +16,7 @@ if(franka_description_FOUND
    AND (NOT ("${franka_description_VERSION}" VERSION_LESS "${_franka_description_min_ver}"))
    AND (NOT ("${xacro_VERSION}" VERSION_LESS "${_xacro_min_ver}")))
   # Why franka_description >= 0.10.0:
-  #   panda.urdf.xacro and dual_panda.urdf.xacro assumes file structure of franka_description >= 0.10.0.
+  #   dual_panda.urdf.xacro assumes file structure of franka_description >= 0.10.0.
   #   See https://github.com/frankaemika/franka_ros/compare/0.9.1...0.10.0 for details.
   # Why xacro >= 1.13.14:
   #   xacro.load_yaml cannot be recognized when xacro < 1.13.14, while it is recommended when xacro >= 1.13.14.
@@ -40,14 +40,17 @@ if(franka_description_FOUND
   ###
   ### panda.l generation
   ###
+  set(_panda_xacro ${franka_description_SOURCE_PREFIX}/robots/panda/panda.urdf.xacro)  # franka_description is installed from source
+  if(NOT EXISTS ${_panda_xacro})
+    set(_panda_xacro ${franka_description_PREFIX}/share/franka_description/robots/panda/panda.urdf.xacro)  # franka_description is installed from apt
+  endif()
   add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/models/panda.l
     COMMAND rosrun euscollada collada2eus -I panda.urdf -C panda.yaml -O panda.l
     WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/models
     DEPENDS ${PROJECT_SOURCE_DIR}/models/panda.urdf ${PROJECT_SOURCE_DIR}/models/panda.yaml)
   add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/models/panda.urdf
-    COMMAND rosrun xacro xacro --inorder panda.urdf.xacro > panda.urdf
-    WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/models
-    DEPENDS ${PROJECT_SOURCE_DIR}/models/panda.urdf.xacro)
+    COMMAND rosrun xacro xacro --inorder ${_panda_xacro} hand:=true > ${PROJECT_SOURCE_DIR}/models/panda.urdf
+    DEPENDS ${_panda_xacro})
 
   add_custom_target(generate_panda_lisp ALL DEPENDS ${PROJECT_SOURCE_DIR}/models/panda.l)