add single fr3

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/jsk_panda_startup/launch/fr3.launch

@@ -0,0 +1,17 @@
+<?xml version="1.0" ?>
+
+<launch>
+  <arg name="robot_ip"     default="192.168.0.2"/>
+  <arg name="robot"        default="fr3" />
+  <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

@@ -2,3 +2,5 @@ models/dual_panda.l
 models/dual_panda.urdf
 models/panda.l
 models/panda.urdf
+models/fr3.l
+models/fr3.urdf

jsk_panda_robot/panda_eus/CMakeLists.txt

@@ -54,8 +54,25 @@ if(franka_description_FOUND
 
   add_custom_target(generate_panda_lisp ALL DEPENDS ${PROJECT_SOURCE_DIR}/models/panda.l)
 
+  ###
+  ### fr3.l generation
+  ###
+  set(_fr3_xacro ${franka_description_SOURCE_PREFIX}/robots/fr3/fr3.urdf.xacro)  # franka_description is installed from source
+  if(NOT EXISTS ${_fr3_xacro})
+    set(_fr3_xacro ${franka_description_PREFIX}/share/franka_description/robots/fr3/fr3.urdf.xacro)  # franka_description is installed from apt
+  endif()
+  add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/models/fr3.l
+    COMMAND rosrun euscollada collada2eus -I fr3.urdf -C fr3.yaml -O fr3.l
+    WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/models
+    DEPENDS ${PROJECT_SOURCE_DIR}/models/fr3.urdf ${PROJECT_SOURCE_DIR}/models/fr3.yaml)
+  add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/models/fr3.urdf
+    COMMAND rosrun xacro xacro --inorder ${_fr3_xacro} hand:=true > ${PROJECT_SOURCE_DIR}/models/fr3.urdf
+    DEPENDS ${_fr3_xacro})
+
+  add_custom_target(generate_fr3_lisp ALL DEPENDS ${PROJECT_SOURCE_DIR}/models/fr3.l)
+
 else()
-  message(WARNING "Dependency is not met, so skip generating panda.l and dual_panda.l")
+  message(WARNING "Dependency is not met, so skip generating panda.l, dual_panda.l and fr3.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()

jsk_panda_robot/panda_eus/euslisp/fr3-interface.l

@@ -0,0 +1,19 @@
+(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 panda-robot-interface
+  :slots ())
+
+(defmethod fr3-robot-interface
+  (:init
+    (&rest args)
+    (send-super* :init :robot fr3-robot
+                 args))
+  )
+
+(defun fr3-init ()
+  (setq *ri* (instance fr3-robot-interface :init))
+  (setq *robot* (fr3)))
+
+(provide :fr3-interface)

jsk_panda_robot/panda_eus/euslisp/fr3-utils.l

@@ -0,0 +1,26 @@
+(require :fr3 "package://panda_eus/models/fr3.l")
+
+(defmethod fr3-robot
+  (:arm (&rest args) (send* self :rarm args))  ;; Enable to call (send *fr3* :arm :angle-vector)
+  (:start-grasp
+    (arm &rest args &key (width 0.0) &allow-other-keys)
+    (send* self :move-gripper arm width args))
+  (:stop-grasp
+    (arm &rest args &key (width 0.08) &allow-other-keys)
+    (send* self :move-gripper arm width args))
+  (:move-gripper
+    (arm width &rest args)
+    "Move the gripper to the target `width`.
+Arguments:
+- arm : :arm, :rarm, or :arms (only for compatibility with fr3-robot-interface)
+- width : target distance between the fingers [m]
+"
+    (send-all
+      (remove nil (mapcar
+                    #'(lambda (jt)
+                        (if (= (send jt :min-angle) (send jt :max-angle)) nil jt))
+                    (send self :rarm :gripper :joint-list)))
+      ;; Get joint list of gripper excluding fixed joints
+      :joint-angle (* (/ width 2.0) 1000))))
+
+(provide :fr3-utils)

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)

jsk_panda_robot/panda_eus/models/fr3.yaml

@@ -0,0 +1,19 @@
+rarm:
+  - fr3_joint1 : rarm-collar-y
+  - fr3_joint2 : rarm-shoulder-p
+  - fr3_joint3 : rarm-shoulder-y
+  - fr3_joint4 : rarm-elbow-p
+  - fr3_joint5 : rarm-wrist-r
+  - fr3_joint6 : rarm-wrist-p
+  - fr3_joint7 : rarm-wrist-y
+
+rarm-end-coords:
+  parent: fr3_hand  # If fr3_hand_tcp is used to delete the following translation, (send *fr3* :rarm :gripper :joint-list) does not include finger joints
+  translate: [0, 0, 0.1034]  # https://github.com/frankaemika/franka_ros/blob/0.10.1/franka_description/robots/common/franka_robot.xacro#L8
+  rotate : [0, -1, 0, 90]
+
+angle-vector:
+  reset-pose: [ 0.0, -45.0, 0.0, -135.0, 0.0, 90.0, 45.0,
+                0.0, -75.0 ]
+  reset-manip-pose: [ 0.0, -45.0, 0.0, -135.0, 0.0, 90.0, 45.0,
+                      0.0, -75.0 ]