From ca18d815dfeb427603f334bf21016e283df904b5 Mon Sep 17 00:00:00 2001 From: Yuki Asano Date: Tue, 31 Oct 2023 12:35:28 +0900 Subject: [PATCH 1/3] add fr3 model --- .../jsk_panda_startup/launch/fr3.launch | 17 ++++++++++ jsk_panda_robot/panda_eus/.gitignore | 2 ++ jsk_panda_robot/panda_eus/CMakeLists.txt | 19 ++++++++++- .../panda_eus/euslisp/fr3-interface.l | 34 +++++++++++++++++++ jsk_panda_robot/panda_eus/euslisp/fr3-utils.l | 26 ++++++++++++++ jsk_panda_robot/panda_eus/models/fr3.yaml | 19 +++++++++++ 6 files changed, 116 insertions(+), 1 deletion(-) create mode 100644 jsk_panda_robot/jsk_panda_startup/launch/fr3.launch create mode 100644 jsk_panda_robot/panda_eus/euslisp/fr3-interface.l create mode 100644 jsk_panda_robot/panda_eus/euslisp/fr3-utils.l create mode 100644 jsk_panda_robot/panda_eus/models/fr3.yaml 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..7a98979271 --- /dev/null +++ b/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) 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/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 ] From 04940de072c1b9cada3a86c0fa05b9d7e6fa872c Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Thu, 2 Nov 2023 21:47:55 +0900 Subject: [PATCH 2/3] [panda_eus] Re-use panda-robot-interface in fr3-robot-interface --- .../panda_eus/euslisp/fr3-interface.l | 19 ++----------------- .../panda_eus/euslisp/panda-interface.l | 4 ++-- 2 files changed, 4 insertions(+), 19 deletions(-) diff --git a/jsk_panda_robot/panda_eus/euslisp/fr3-interface.l b/jsk_panda_robot/panda_eus/euslisp/fr3-interface.l index 7a98979271..b2f0eccd63 100644 --- a/jsk_panda_robot/panda_eus/euslisp/fr3-interface.l +++ b/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 () 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) From c8273af33058b18c31f4b9f1390125ffac100575 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Thu, 2 Nov 2023 21:56:28 +0900 Subject: [PATCH 3/3] [jsk_panda_robot] Add fr3 to README --- jsk_panda_robot/README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) 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))