add single panda (#1799)

* add single panda

* update from latest franka_control.launch

* add comments

* remove unused config

* remove gazebo descriptions

* move code for panda.l and modify messages

* change name

* change to include franka_control.launch

* change larm to rarm

* panda.urdf.xacro is unnecessary

* [panda_eus] Add panda.urdf and panda.l to .gitignore

* [panda_eus] Extract common part of panda-interface.l and dual_panda-interface.l as panda-common-interface.l

* [jsk_panda_robot] Add single panda to README

* [jsk_panda_robot] Add notice about end-coords, reset-pose, and reset-manip-pose

* [panda_eus] Improve error message from :set-joint-pd-gain

* enable access to gripper-grasp-action

* add missing bracket

* Enable to get results of :stop-gripper/:homing-gripper/:start-grasp/:stop-grasp/:move-gripper from their return values

* Enable to get results of :stop-gripper/:homing-gripper/:start-grasp/:stop-grasp/:move-gripper from separated methods

* Consider compatibility with FR3 (Franka Research 3)

* Remove unnecessary method

* extract action from hash

* Revert "extract action from hash"

This reverts commit 0066eb7.

* Make errors from :get-*-result readable when * is not called

* Force to call all :wait-for-result and postprocess even if some of them fail

* [panda_eus] Change dual_panda's end-coords to the same as single panda

* [panda_eus] Move end-coords of single panda and dual_panda to the center of the fingers

* [panda_eus] Make :gripper method work

* [panda_eus] Make URDF follow euslisp model

* [panda_eus] Enable to get gripper method result without waiting

* [panda_eus] Enable to get status of gripper actions

---------

Co-authored-by: Shun Hasegawa <[email protected]>

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 [email protected]  # Or ssh [email protected]
+   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 `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

@@ -0,0 +1,17 @@
+<?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 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>
+</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

@@ -10,9 +10,6 @@ find_package(franka_description)  # Just in case when description is not release
 
 catkin_package()
 
-###
-### dual_panda.l generation
-###
 set(_franka_description_min_ver "0.10.0")
 set(_xacro_min_ver "1.13.14")
 if(franka_description_FOUND
@@ -25,6 +22,10 @@ if(franka_description_FOUND
   #   xacro.load_yaml cannot be recognized when xacro < 1.13.14, while it is recommended when xacro >= 1.13.14.
   #   PR introducing xacro.load_yaml: https://github.com/ros/xacro/pull/283
   #   Related issue: https://github.com/ros/xacro/issues/298
+
+  ###
+  ### dual_panda.l generation
+  ###
   add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/models/dual_panda.l
     COMMAND rosrun euscollada collada2eus -I dual_panda.urdf -C dual_panda.yaml -O dual_panda.l
     WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/models
@@ -34,9 +35,27 @@ if(franka_description_FOUND
     WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/models
     DEPENDS ${PROJECT_SOURCE_DIR}/models/dual_panda.urdf.xacro)
 
-  add_custom_target(generate_panda_lisp ALL DEPENDS ${PROJECT_SOURCE_DIR}/models/dual_panda.l)
+  add_custom_target(generate_dual_panda_lisp ALL DEPENDS ${PROJECT_SOURCE_DIR}/models/dual_panda.l)
+
+  ###
+  ### 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_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)
+
 else()
-  message(WARNING "Dependency is not met, so skip generating dual_panda.l")
+  message(WARNING "Dependency is not met, so skip generating panda.l and dual_panda.l")
   message(WARNING "franka_description version: ${franka_description_VERSION}, must be >= ${_franka_description_min_ver}")
   message(WARNING "xacro version: ${xacro_VERSION}, must be >= ${_xacro_min_ver}")
 endif()