diff --git a/jsk_panda_robot/README.md b/jsk_panda_robot/README.md index d035983bce..13abc21ca4 100644 --- a/jsk_panda_robot/README.md +++ b/jsk_panda_robot/README.md @@ -49,17 +49,17 @@ ``` -## 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:= + roslaunch jsk_panda_startup panda.launch robot_ip:= # FR3: roslaunch jsk_panda_startup fr3.launch robot_ip:= ``` -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 @@ -67,8 +67,8 @@ ``` 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)) diff --git a/jsk_panda_robot/jsk_panda_startup/launch/fr3.launch b/jsk_panda_robot/jsk_panda_startup/launch/fr3.launch new file mode 100644 index 0000000000..d72b0c25ae --- /dev/null +++ b/jsk_panda_robot/jsk_panda_startup/launch/fr3.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/jsk_panda_robot/panda_eus/.gitignore b/jsk_panda_robot/panda_eus/.gitignore index 97a6d36fcf..d38fdb8d62 100644 --- a/jsk_panda_robot/panda_eus/.gitignore +++ b/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 diff --git a/jsk_panda_robot/panda_eus/CMakeLists.txt b/jsk_panda_robot/panda_eus/CMakeLists.txt index 6727ab33cd..7db0dc8e9d 100644 --- a/jsk_panda_robot/panda_eus/CMakeLists.txt +++ b/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() diff --git a/jsk_panda_robot/panda_eus/euslisp/fr3-interface.l b/jsk_panda_robot/panda_eus/euslisp/fr3-interface.l new file mode 100644 index 0000000000..b2f0eccd63 --- /dev/null +++ b/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) diff --git a/jsk_panda_robot/panda_eus/euslisp/fr3-utils.l b/jsk_panda_robot/panda_eus/euslisp/fr3-utils.l new file mode 100644 index 0000000000..6b75402de1 --- /dev/null +++ b/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) diff --git a/jsk_panda_robot/panda_eus/euslisp/panda-interface.l b/jsk_panda_robot/panda_eus/euslisp/panda-interface.l index 78cb96a842..ecbfa8fe46 100644 --- a/jsk_panda_robot/panda_eus/euslisp/panda-interface.l +++ b/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) diff --git a/jsk_panda_robot/panda_eus/models/fr3.yaml b/jsk_panda_robot/panda_eus/models/fr3.yaml new file mode 100644 index 0000000000..7783bacb5f --- /dev/null +++ b/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 ]