Re-use panda-robot-interface in fr3-robot-interface & Add fr3 to README

jsk_panda_robot/README.md

@@ -49,26 +49,26 @@
    ```
 
 
-## Running single Panda
+## Running single Panda/FR3 (Franka Research 3)
 ### 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>
+   roslaunch jsk_panda_startup panda.launch robot_ip:=<IP OF TARGET ROBOT>  # FR3: roslaunch jsk_panda_startup fr3.launch robot_ip:=<IP OF TARGET ROBOT>
    ```
 
-2. Controlling single Panda via roseus:
+2. Controlling single Panda/FR3 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)
+      (load "package://panda_eus/euslisp/panda-interface.l")  ;; FR3: (load "package://panda_eus/euslisp/fr3-interface.l")
+      (panda-init)  ;; FR3: (fr3-init)
       (send *robot* :angle-vector (send *robot* :reset-pose))
       (when (send *ri* :check-error)
         (send *ri* :recover-error))

jsk_panda_robot/panda_eus/euslisp/fr3-interface.l

@@ -1,30 +1,15 @@
-(require :franka-common-interface "package://panda_eus/euslisp/franka-common-interface.l")
+(require :panda-interface "package://panda_eus/euslisp/panda-interface.l")
 (require :fr3-utils "package://panda_eus/euslisp/fr3-utils.l")
 
 (defclass fr3-robot-interface
-  :super franka-common-interface
+  :super panda-robot-interface
   :slots ())
 
 (defmethod fr3-robot-interface
   (:init
     (&rest args)
     (send-super* :init :robot fr3-robot
-                 :joint-states-topic "joint_states"
-                 :all-arms (list :rarm)
-                 :all-arm-aliases (list :arm)
-                 :error-topics (list "/franka_state_controller/franka_states")
-                 :error-topic-types (list franka_msgs::FrankaState)
-                 :error-recovery-action "/franka_control/error_recovery"
-                 :gripper-action-prefixes (list "")
                  args))
-  (:default-controller
-    ()
-    (list
-      (list
-       (cons :controller-action "/position_joint_trajectory_controller/follow_joint_trajectory")
-       (cons :controller-state  "/position_joint_trajectory_controller/state")
-       (cons :action-type control_msgs::FollowJointTrajectoryAction)
-       (cons :joint-names (send-all (send robot :joint-list) :name)))))
   )
 
 (defun fr3-init ()

jsk_panda_robot/panda_eus/euslisp/panda-interface.l

@@ -7,8 +7,8 @@
 
 (defmethod panda-robot-interface
   (:init
-    (&rest args)
-    (send-super* :init :robot panda-robot
+    (&rest args &key (robot panda-robot))
+    (send-super* :init :robot robot
                  :joint-states-topic "joint_states"
                  :all-arms (list :rarm)
                  :all-arm-aliases (list :arm)