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 ]