add fr3 model

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,34 @@
+(require :franka-common-interface "package://panda_eus/euslisp/franka-common-interface.l")
+(require :fr3-utils "package://panda_eus/euslisp/fr3-utils.l")
+
+(defclass fr3-robot-interface
+  :super franka-common-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 ()
+  (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/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 ]