From 017a34dcd93a60719fc17c7097aade3ec048e7c1 Mon Sep 17 00:00:00 2001 From: Yuki Asano Date: Fri, 28 Apr 2023 13:44:36 +0900 Subject: [PATCH 01/31] add single panda --- .../config/panda_control_node.yaml | 34 +++ .../config/panda_controllers.yaml | 82 ++++++ .../launch/panda_control.launch | 36 +++ jsk_panda_robot/panda_eus/CMakeLists.txt | 18 +- .../panda_eus/euslisp/panda-interface.l | 237 ++++++++++++++++++ .../panda_eus/models/panda.urdf.xacro | 5 + jsk_panda_robot/panda_eus/models/panda.yaml | 19 ++ 7 files changed, 430 insertions(+), 1 deletion(-) create mode 100644 jsk_panda_robot/jsk_panda_startup/config/panda_control_node.yaml create mode 100644 jsk_panda_robot/jsk_panda_startup/config/panda_controllers.yaml create mode 100644 jsk_panda_robot/jsk_panda_startup/launch/panda_control.launch create mode 100644 jsk_panda_robot/panda_eus/euslisp/panda-interface.l create mode 100644 jsk_panda_robot/panda_eus/models/panda.urdf.xacro create mode 100644 jsk_panda_robot/panda_eus/models/panda.yaml diff --git a/jsk_panda_robot/jsk_panda_startup/config/panda_control_node.yaml b/jsk_panda_robot/jsk_panda_startup/config/panda_control_node.yaml new file mode 100644 index 0000000000..836846cb05 --- /dev/null +++ b/jsk_panda_robot/jsk_panda_startup/config/panda_control_node.yaml @@ -0,0 +1,34 @@ +robot_hardware: + - panda_arm + +panda_arm: + type: franka_hw/FrankaCombinableHW + arm_id: panda_arm + joint_names: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 + # Configure the threshold angle for printing joint limit warnings. + joint_limit_warning_threshold: 0.1 # [rad] + # Activate rate limiter? [true|false] + rate_limiting: true + # Cutoff frequency of the low-pass filter. Set to >= 1000 to deactivate. + cutoff_frequency: 1000 + # Internal controller for motion generators [joint_impedance|cartesian_impedance] + internal_controller: joint_impedance + # Used to decide whether to enforce realtime mode [enforce|ignore] + realtime_config: enforce + # Configure the initial defaults for the collision behavior reflexes. + collision_config: + lower_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm] + upper_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm] + lower_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm] + upper_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm] + lower_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm] + upper_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm] + lower_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm] + upper_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm] diff --git a/jsk_panda_robot/jsk_panda_startup/config/panda_controllers.yaml b/jsk_panda_robot/jsk_panda_startup/config/panda_controllers.yaml new file mode 100644 index 0000000000..eb4f5abb65 --- /dev/null +++ b/jsk_panda_robot/jsk_panda_startup/config/panda_controllers.yaml @@ -0,0 +1,82 @@ +panda_effort_joint_trajectory_controller: + type: effort_controllers/JointTrajectoryController + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 + constraints: + goal_time: 0.5 + panda_joint1: + goal: 0.05 + panda_joint2: + goal: 0.05 + panda_joint3: + goal: 0.05 + panda_joint4: + goal: 0.05 + panda_joint5: + goal: 0.05 + panda_joint6: + goal: 0.05 + panda_joint7: + goal: 0.05 + gains: + panda_joint1: {p: 600, d: 30, i: 0, i_clamp: 1} + panda_joint2: {p: 600, d: 30, i: 0, i_clamp: 1} + panda_joint3: {p: 600, d: 30, i: 0, i_clamp: 1} + panda_joint4: {p: 600, d: 30, i: 0, i_clamp: 1} + panda_joint5: {p: 250, d: 10, i: 0, i_clamp: 1} + panda_joint6: {p: 150, d: 10, i: 0, i_clamp: 1} + panda_joint7: {p: 50, d: 5, i: 0, i_clamp: 1} + +panda_pose_cartesian_controller: + type: franka_example_controllers/CartesianPoseExampleController + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 + constraints: + goal_time: 0.5 + panda_joint1: + goal: 0.05 + panda_joint2: + goal: 0.05 + panda_joint3: + goal: 0.05 + panda_joint4: + goal: 0.05 + panda_joint5: + goal: 0.05 + panda_joint6: + goal: 0.05 + panda_joint7: + goal: 0.05 + gains: + panda_joint1: {p: 600, d: 30, i: 0, i_clamp: 1} + panda_joint2: {p: 600, d: 30, i: 0, i_clamp: 1} + panda_joint3: {p: 600, d: 30, i: 0, i_clamp: 1} + panda_joint4: {p: 600, d: 30, i: 0, i_clamp: 1} + panda_joint5: {p: 250, d: 10, i: 0, i_clamp: 1} + panda_joint6: {p: 150, d: 10, i: 0, i_clamp: 1} + panda_joint7: {p: 50, d: 5, i: 0, i_clamp: 1} + +state_controller: + type: franka_control/FrankaStateController + arm_id: panda_arm + joint_names: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 + publish_rate: 30 # [Hz] diff --git a/jsk_panda_robot/jsk_panda_startup/launch/panda_control.launch b/jsk_panda_robot/jsk_panda_startup/launch/panda_control.launch new file mode 100644 index 0000000000..dd0a558b5d --- /dev/null +++ b/jsk_panda_robot/jsk_panda_startup/launch/panda_control.launch @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + [franka_state_controller/joint_states, franka_gripper/joint_states] + [franka_state_controller/joint_states] + + + + [franka_state_controller/joint_states_desired, franka_gripper/joint_states] + [franka_state_controller/joint_states_desired] + + + + + diff --git a/jsk_panda_robot/panda_eus/CMakeLists.txt b/jsk_panda_robot/panda_eus/CMakeLists.txt index 4fea19581f..93d8e52241 100644 --- a/jsk_panda_robot/panda_eus/CMakeLists.txt +++ b/jsk_panda_robot/panda_eus/CMakeLists.txt @@ -34,7 +34,7 @@ if(franka_description_FOUND WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/models DEPENDS ${PROJECT_SOURCE_DIR}/models/dual_panda.urdf.xacro) - add_custom_target(generate_panda_lisp ALL DEPENDS ${PROJECT_SOURCE_DIR}/models/dual_panda.l) + add_custom_target(generate_dual_panda_lisp ALL DEPENDS ${PROJECT_SOURCE_DIR}/models/dual_panda.l) else() message(WARNING "Dependency is not met, so skip generating dual_panda.l") message(WARNING "franka_description version: ${franka_description_VERSION}, must be >= ${_franka_description_min_ver}") @@ -42,6 +42,22 @@ else() endif() +### +### panda.l generation +### +add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/models/panda.l + COMMAND rosrun euscollada collada2eus -I panda.urdf -C panda.yaml -O panda.l + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/models + DEPENDS ${PROJECT_SOURCE_DIR}/models/panda.urdf ${PROJECT_SOURCE_DIR}/models/panda.yaml) + +add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/models/panda.urdf + COMMAND rosrun xacro xacro --inorder panda.urdf.xacro > panda.urdf + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/models + DEPENDS ${PROJECT_SOURCE_DIR}/models/panda.urdf.xacro) + +add_custom_target(generate_panda_lisp ALL DEPENDS ${PROJECT_SOURCE_DIR}/models/panda.l) + + ############# ## Install ## ############# diff --git a/jsk_panda_robot/panda_eus/euslisp/panda-interface.l b/jsk_panda_robot/panda_eus/euslisp/panda-interface.l new file mode 100644 index 0000000000..cc85420e79 --- /dev/null +++ b/jsk_panda_robot/panda_eus/euslisp/panda-interface.l @@ -0,0 +1,237 @@ +(require :robot-interface "package://pr2eus/robot-interface.l") +(require :franka "package://panda_eus/models/panda.l") + +(ros::roseus-add-msgs "franka_msgs") +(ros::roseus-add-msgs "franka_gripper") + +(defclass panda-robot-interface + :super robot-interface + :slots (gripper-action gazebop + error-recovery-act error + gripper-grasp-action gripper-move-action gripper-homing-action gripper-stop-action + ) + ) +(defmethod panda-robot-interface + (:init + (&rest args &key ((:controller-timeout ct) nil)) + (prog1 + (send-super* :init :robot panda-robot + :joint-states-topic "joint_states" + :controller-timeout ct args) + ;; check whather the environement is Gazebo + (setq gazebop (numberp (ros::get-param "/gazebo/time_step"))) + (if gazebop (ros::ros-warn "Using Gazebo environment")) + + ;; Controller setting for Gazebo environment + (when gazebop + (ros::subscribe "/hand_controller/state" control_msgs::JointTrajectoryControllerState + #'send self :gripper-state-callback :arm :groupname groupname) + (setq gripper-action (instance ros::simple-action-client :init + "/hand_controller/follow_joint_trajectory" + control_msgs::FollowJointTrajectoryAction)) + (dolist (action (list gripper-action)) + (unless (and joint-action-enable (send action :wait-for-server 3)) + (setq joint-action-enable nil) + (ros::ros-warn "~A is not respond" action) + (return))) + ) ;; when gazebo + (unless gazebop + (ros::ros-warn "real franka environemt interface is not impelemented")) ;; TODO + + ;; for error recovery + (ros::create-nodehandle "error_group") + (ros::subscribe "/panda/arm/has_error" std_msgs::Bool + #'send self :callback-arm-error 1 :groupname "error_group") + (setq error-recovery-act (instance ros::simple-action-client :init + "/franka_control/error_recovery" + franka_msgs::ErrorRecoveryAction + :groupname "error_group" + )) + ;; actions for gripper + (setq gripper-grasp-action + (instance ros::simple-action-client :init + "/franka_gripper/grasp" + franka_gripper::GraspAction)) + (setq gripper-homing-action + (instance ros::simple-action-client :init + "/franka_gripper/homing" + franka_gripper::HomingAction)) + (setq gripper-move-action + (instance ros::simple-action-client :init + "/franka_gripper/move" + franka_gripper::MoveAction)) + (setq gripper-stop-action + (instance ros::simple-action-client :init + "/franka_gripper/stop" + franka_gripper::StopAction)) + )) + (: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)) + ))) + (:larm-controller + () + (list + (list + (cons :controller-action "/larm_controller/follow_joint_trajectory") + (cons :controller-state "/position_joint_trajectory_controller/follow_joint_trajectory") + (cons :action-type control_msgs::FollowJointTrajectoryAction) + (cons :joint-names + (remove-if #'(lambda (jn) (substringp "finger" jn)) + (mapcar #'(lambda (n) (if (symbolp n) (symbol-name n) n)) + (send-all (send robot :larm :joint-list) :name))))))) + (:hand-controller + () + (list + (list + (cons :controller-action "/hand_controller/follow_joint_trajectory") + (cons :controller-state "/hand_controller/state") + (cons :action-type control_msgs::FollowJointTrajectoryAction) + (cons :joint-names (list "left_finger_joint1" "left_finger_joint2") )))) + (:set-joint-pd-gain + (joint-name pgain dgain) + (let ((req (instance dynamic_reconfigure::ReconfigureRequest :init))) + (send req :config :doubles + (list (instance dynamic_reconfigure::DoubleParameter :init + :name "p" :value pgain) + (instance dynamic_reconfigure::DoubleParameter :init + :name "d" :value dgain))) + (ros::service-call + (format nil "/position_joint_trajectory_controller/gains/~A/set_parameters" joint-name) + req) + )) + (:set-all-joint-pd-gain + (pgain dgain) + (dolist (j (send robot :joint-list)) + (send self :set-joint-pd-gain (send j :name) pgain dgain)) + ) + (:check-error () + (ros::spin-once "error_group") + (or error) + ) + (:callback-arm-error (msg) + (setq error (send msg :data)) + ) + (:wait-recover-error () (send error-recovery-act :wait-for-result)) + (:recover-error (&key (wait t)) + (let ((goal (instance franka_msgs::ErrorRecoveryActionGoal :init))) + (send goal :header :stamp (ros::time-now)) + (send error-recovery-act :send-goal goal) + (if wait (send self :wait-recover-error)) + )) + ;; gripper action for real-controller + (:send-gripper-grasp-action + (act width speed force &key (wait t) (inner 0.005) (outer 0.005)) + (let ((goal (instance franka_gripper::GraspActionGoal :init))) + (send goal :header :stamp (ros::time-now)) + (send goal :goal :width width) ;; [m] + (send goal :goal :speed speed) ;; [m/s] + (send goal :goal :force force) ;; [N] + (send goal :goal :epsilon :inner inner) ;; [m] + (send goal :goal :epsilon :outer outer) ;; [m] + ;; + (send act :send-goal goal) + (if wait (send act :wait-for-result)) + )) + (:send-gripper-homing-action + (act &key (wait t)) + (let ((goal (instance franka_gripper::HomingActionGoal :init))) + (send goal :header :stamp (ros::time-now)) + ;; + (send act :send-goal goal) + (if wait (send act :wait-for-result)) + )) + (:send-gripper-move-action + (act width speed &key (wait t)) + (let ((goal (instance franka_gripper::MoveActionGoal :init))) + (send goal :header :stamp (ros::time-now)) + (send goal :goal :width width) ;; [m] + (send goal :goal :speed speed) ;; [m/s] + ;; + (send act :send-goal goal) + (if wait (send act :wait-for-result)) + )) + (:send-gripper-stop-action + (act &key (wait t)) + (let ((goal (instance franka_gripper::StopActionGoal :init))) + (send goal :header :stamp (ros::time-now)) + ;; + (send act :send-goal goal) + (if wait (send act :wait-for-result)) + )) + (:stop-gripper + (arm &key (wait nil)) + (let (acts) + (case + arm + (:arm (setq acts (list gripper-stop-action))) + ) + (when acts + (dolist (act acts) + (send self :send-gripper-stop-action act + pos (/ (* 1000 0.08) tm) :wait nil)) + (if wait (mapcar #'(lambda (act) (send act :wait-for-result)) acts)) + ))) + (:homing-gripper + (arm &key (wait nil)) + (let (acts) + (case + arm + (:arm (setq acts (list gripper-homing-action))) + ) + (when acts + (dolist (act acts) + (send self :send-gripper-homing-action act :wait nil)) + (if wait (mapcar #'(lambda (act) (send act :wait-for-result)) acts)) + ))) + (:start-grasp + (arm &optional (pos 0.0) &key (effort 10.0) (tm 500) (wait nil)) ;; TODO :arms is not implemented + (let (acts) + (case + arm + (:arm (setq acts (list gripper-grasp-action))) + ) + (when acts + (dolist (act acts) + (send self :send-gripper-grasp-action act + pos (/ (* 1000 0.08) tm) effort :wait nil)) + (if wait (mapcar #'(lambda (act) (send act :wait-for-result)) acts)) + ))) + (:stop-grasp + (arm &key (wait nil)) ;; TODO :arms is not implemented + (unless (memq arm '(:arm :arms)) + (error "you must specify arm ~A from ~A" (car args) '(:arm :arms)) + (return-from :stop-grasp nil)) + (send self :move-gripper arm 0.08 :tm 500 :wait wait) + ) + (:move-gripper + (arm pos &key (tm 500) (timeout 5000) (wait nil)) ;; TODO :arms is not implemented + (let (acts) + (case + arm + (:arm (setq acts (list gripper-move-action))) + ) + (when acts + (dolist (act acts) + (send self :send-gripper-move-action act + pos (/ (* 1000 0.08) tm) :wait nil)) + (if wait (mapcar #'(lambda (act) (send act :wait-for-result)) acts)) + ))) + ) + +;; grasp controller ... +(defun panda-init () + (setq *ri* (instance panda-robot-interface :init)) + (setq *robot* (panda)) + ) + +#| +(send *ri* :set-all-joint-pd-gain 1000.0 5.0) ;; default +(send *ri* :set-all-joint-pd-gain 300.0 5.0) ;; hard +(send *ri* :set-all-joint-pd-gain 30.0 0.5) ;; soft +|# diff --git a/jsk_panda_robot/panda_eus/models/panda.urdf.xacro b/jsk_panda_robot/panda_eus/models/panda.urdf.xacro new file mode 100644 index 0000000000..0f0058ab53 --- /dev/null +++ b/jsk_panda_robot/panda_eus/models/panda.urdf.xacro @@ -0,0 +1,5 @@ + + + + + diff --git a/jsk_panda_robot/panda_eus/models/panda.yaml b/jsk_panda_robot/panda_eus/models/panda.yaml new file mode 100644 index 0000000000..1a25f11681 --- /dev/null +++ b/jsk_panda_robot/panda_eus/models/panda.yaml @@ -0,0 +1,19 @@ +larm: + - panda_joint1 : larm-collar-y + - panda_joint2 : larm-shoulder-p + - panda_joint3 : larm-shoulder-y + - panda_joint4 : larm-elbow-p + - panda_joint5 : larm-wrist-r + - panda_joint6 : larm-wrist-p + - panda_joint7 : larm-wrist-y + +larm-end-coords: + parent: panda_link8 + translate: [0, 0, 0.1] + rotate : [-0.35740972270209, -0.86285515644477, -0.35740630816298, 98.42112728719206] + +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 24798178926b4e6d184c08c82806a2898b870f7b Mon Sep 17 00:00:00 2001 From: Yuki Asano Date: Tue, 9 May 2023 14:11:56 +0900 Subject: [PATCH 02/31] update from latest franka_control.launch --- .../launch/panda_control.launch | 26 +++++++++---------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/jsk_panda_robot/jsk_panda_startup/launch/panda_control.launch b/jsk_panda_robot/jsk_panda_startup/launch/panda_control.launch index dd0a558b5d..ecfe4bc784 100644 --- a/jsk_panda_robot/jsk_panda_startup/launch/panda_control.launch +++ b/jsk_panda_robot/jsk_panda_startup/launch/panda_control.launch @@ -1,36 +1,34 @@ - + + + + + + + - - + + - + - - - + + [franka_state_controller/joint_states, franka_gripper/joint_states] [franka_state_controller/joint_states] - - [franka_state_controller/joint_states_desired, franka_gripper/joint_states] - [franka_state_controller/joint_states_desired] - - - - From aa2f556effc957c6e9002eb5692b83ef53b9dc9c Mon Sep 17 00:00:00 2001 From: Yuki Asano Date: Tue, 9 May 2023 15:24:56 +0900 Subject: [PATCH 03/31] add comments --- .../jsk_panda_startup/launch/panda_control.launch | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/jsk_panda_robot/jsk_panda_startup/launch/panda_control.launch b/jsk_panda_robot/jsk_panda_startup/launch/panda_control.launch index ecfe4bc784..7212aa0ad4 100644 --- a/jsk_panda_robot/jsk_panda_startup/launch/panda_control.launch +++ b/jsk_panda_robot/jsk_panda_startup/launch/panda_control.launch @@ -4,15 +4,13 @@ - + - - - + @@ -24,7 +22,7 @@ - + [franka_state_controller/joint_states, franka_gripper/joint_states] From c943a29e24f7d8175393321b7f215e91d0b69829 Mon Sep 17 00:00:00 2001 From: Yuki Asano Date: Tue, 9 May 2023 16:03:44 +0900 Subject: [PATCH 04/31] remove unused config --- .../config/panda_control_node.yaml | 34 -------- .../config/panda_controllers.yaml | 82 ------------------- 2 files changed, 116 deletions(-) delete mode 100644 jsk_panda_robot/jsk_panda_startup/config/panda_control_node.yaml delete mode 100644 jsk_panda_robot/jsk_panda_startup/config/panda_controllers.yaml diff --git a/jsk_panda_robot/jsk_panda_startup/config/panda_control_node.yaml b/jsk_panda_robot/jsk_panda_startup/config/panda_control_node.yaml deleted file mode 100644 index 836846cb05..0000000000 --- a/jsk_panda_robot/jsk_panda_startup/config/panda_control_node.yaml +++ /dev/null @@ -1,34 +0,0 @@ -robot_hardware: - - panda_arm - -panda_arm: - type: franka_hw/FrankaCombinableHW - arm_id: panda_arm - joint_names: - - panda_joint1 - - panda_joint2 - - panda_joint3 - - panda_joint4 - - panda_joint5 - - panda_joint6 - - panda_joint7 - # Configure the threshold angle for printing joint limit warnings. - joint_limit_warning_threshold: 0.1 # [rad] - # Activate rate limiter? [true|false] - rate_limiting: true - # Cutoff frequency of the low-pass filter. Set to >= 1000 to deactivate. - cutoff_frequency: 1000 - # Internal controller for motion generators [joint_impedance|cartesian_impedance] - internal_controller: joint_impedance - # Used to decide whether to enforce realtime mode [enforce|ignore] - realtime_config: enforce - # Configure the initial defaults for the collision behavior reflexes. - collision_config: - lower_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm] - upper_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm] - lower_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm] - upper_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm] - lower_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm] - upper_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm] - lower_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm] - upper_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] # [N, N, N, Nm, Nm, Nm] diff --git a/jsk_panda_robot/jsk_panda_startup/config/panda_controllers.yaml b/jsk_panda_robot/jsk_panda_startup/config/panda_controllers.yaml deleted file mode 100644 index eb4f5abb65..0000000000 --- a/jsk_panda_robot/jsk_panda_startup/config/panda_controllers.yaml +++ /dev/null @@ -1,82 +0,0 @@ -panda_effort_joint_trajectory_controller: - type: effort_controllers/JointTrajectoryController - joints: - - panda_joint1 - - panda_joint2 - - panda_joint3 - - panda_joint4 - - panda_joint5 - - panda_joint6 - - panda_joint7 - constraints: - goal_time: 0.5 - panda_joint1: - goal: 0.05 - panda_joint2: - goal: 0.05 - panda_joint3: - goal: 0.05 - panda_joint4: - goal: 0.05 - panda_joint5: - goal: 0.05 - panda_joint6: - goal: 0.05 - panda_joint7: - goal: 0.05 - gains: - panda_joint1: {p: 600, d: 30, i: 0, i_clamp: 1} - panda_joint2: {p: 600, d: 30, i: 0, i_clamp: 1} - panda_joint3: {p: 600, d: 30, i: 0, i_clamp: 1} - panda_joint4: {p: 600, d: 30, i: 0, i_clamp: 1} - panda_joint5: {p: 250, d: 10, i: 0, i_clamp: 1} - panda_joint6: {p: 150, d: 10, i: 0, i_clamp: 1} - panda_joint7: {p: 50, d: 5, i: 0, i_clamp: 1} - -panda_pose_cartesian_controller: - type: franka_example_controllers/CartesianPoseExampleController - joints: - - panda_joint1 - - panda_joint2 - - panda_joint3 - - panda_joint4 - - panda_joint5 - - panda_joint6 - - panda_joint7 - constraints: - goal_time: 0.5 - panda_joint1: - goal: 0.05 - panda_joint2: - goal: 0.05 - panda_joint3: - goal: 0.05 - panda_joint4: - goal: 0.05 - panda_joint5: - goal: 0.05 - panda_joint6: - goal: 0.05 - panda_joint7: - goal: 0.05 - gains: - panda_joint1: {p: 600, d: 30, i: 0, i_clamp: 1} - panda_joint2: {p: 600, d: 30, i: 0, i_clamp: 1} - panda_joint3: {p: 600, d: 30, i: 0, i_clamp: 1} - panda_joint4: {p: 600, d: 30, i: 0, i_clamp: 1} - panda_joint5: {p: 250, d: 10, i: 0, i_clamp: 1} - panda_joint6: {p: 150, d: 10, i: 0, i_clamp: 1} - panda_joint7: {p: 50, d: 5, i: 0, i_clamp: 1} - -state_controller: - type: franka_control/FrankaStateController - arm_id: panda_arm - joint_names: - - panda_joint1 - - panda_joint2 - - panda_joint3 - - panda_joint4 - - panda_joint5 - - panda_joint6 - - panda_joint7 - publish_rate: 30 # [Hz] From d19204ba8670ad0b08cdff432a81638453e71bbe Mon Sep 17 00:00:00 2001 From: Yuki Asano Date: Fri, 12 May 2023 16:36:00 +0900 Subject: [PATCH 05/31] remove gazebo descriptions --- .../panda_eus/euslisp/panda-interface.l | 22 +------------------ 1 file changed, 1 insertion(+), 21 deletions(-) diff --git a/jsk_panda_robot/panda_eus/euslisp/panda-interface.l b/jsk_panda_robot/panda_eus/euslisp/panda-interface.l index cc85420e79..f410d98f93 100644 --- a/jsk_panda_robot/panda_eus/euslisp/panda-interface.l +++ b/jsk_panda_robot/panda_eus/euslisp/panda-interface.l @@ -6,7 +6,7 @@ (defclass panda-robot-interface :super robot-interface - :slots (gripper-action gazebop + :slots (gripper-action error-recovery-act error gripper-grasp-action gripper-move-action gripper-homing-action gripper-stop-action ) @@ -18,26 +18,6 @@ (send-super* :init :robot panda-robot :joint-states-topic "joint_states" :controller-timeout ct args) - ;; check whather the environement is Gazebo - (setq gazebop (numberp (ros::get-param "/gazebo/time_step"))) - (if gazebop (ros::ros-warn "Using Gazebo environment")) - - ;; Controller setting for Gazebo environment - (when gazebop - (ros::subscribe "/hand_controller/state" control_msgs::JointTrajectoryControllerState - #'send self :gripper-state-callback :arm :groupname groupname) - (setq gripper-action (instance ros::simple-action-client :init - "/hand_controller/follow_joint_trajectory" - control_msgs::FollowJointTrajectoryAction)) - (dolist (action (list gripper-action)) - (unless (and joint-action-enable (send action :wait-for-server 3)) - (setq joint-action-enable nil) - (ros::ros-warn "~A is not respond" action) - (return))) - ) ;; when gazebo - (unless gazebop - (ros::ros-warn "real franka environemt interface is not impelemented")) ;; TODO - ;; for error recovery (ros::create-nodehandle "error_group") (ros::subscribe "/panda/arm/has_error" std_msgs::Bool From 7a093afe919bc2038fde6d3611f7cf2fba24eed9 Mon Sep 17 00:00:00 2001 From: Yuki Asano Date: Fri, 12 May 2023 17:01:21 +0900 Subject: [PATCH 06/31] move code for panda.l and modify messages --- jsk_panda_robot/panda_eus/CMakeLists.txt | 42 ++++++++++++------------ 1 file changed, 21 insertions(+), 21 deletions(-) diff --git a/jsk_panda_robot/panda_eus/CMakeLists.txt b/jsk_panda_robot/panda_eus/CMakeLists.txt index 93d8e52241..e29a0217aa 100644 --- a/jsk_panda_robot/panda_eus/CMakeLists.txt +++ b/jsk_panda_robot/panda_eus/CMakeLists.txt @@ -10,21 +10,22 @@ find_package(franka_description) # Just in case when description is not release catkin_package() -### -### dual_panda.l generation -### set(_franka_description_min_ver "0.10.0") set(_xacro_min_ver "1.13.14") if(franka_description_FOUND AND (NOT ("${franka_description_VERSION}" VERSION_LESS "${_franka_description_min_ver}")) AND (NOT ("${xacro_VERSION}" VERSION_LESS "${_xacro_min_ver}"))) # Why franka_description >= 0.10.0: - # dual_panda.urdf.xacro assumes file structure of franka_description >= 0.10.0. + # panda.urdf.xacro and dual_panda.urdf.xacro assumes file structure of franka_description >= 0.10.0. # See https://github.com/frankaemika/franka_ros/compare/0.9.1...0.10.0 for details. # Why xacro >= 1.13.14: # xacro.load_yaml cannot be recognized when xacro < 1.13.14, while it is recommended when xacro >= 1.13.14. # PR introducing xacro.load_yaml: https://github.com/ros/xacro/pull/283 # Related issue: https://github.com/ros/xacro/issues/298 + + ### + ### dual_panda.l generation + ### add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/models/dual_panda.l COMMAND rosrun euscollada collada2eus -I dual_panda.urdf -C dual_panda.yaml -O dual_panda.l WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/models @@ -35,29 +36,28 @@ if(franka_description_FOUND DEPENDS ${PROJECT_SOURCE_DIR}/models/dual_panda.urdf.xacro) add_custom_target(generate_dual_panda_lisp ALL DEPENDS ${PROJECT_SOURCE_DIR}/models/dual_panda.l) + + ### + ### panda.l generation + ### + add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/models/panda.l + COMMAND rosrun euscollada collada2eus -I panda.urdf -C panda.yaml -O panda.l + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/models + DEPENDS ${PROJECT_SOURCE_DIR}/models/panda.urdf ${PROJECT_SOURCE_DIR}/models/panda.yaml) + add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/models/panda.urdf + COMMAND rosrun xacro xacro --inorder panda.urdf.xacro > panda.urdf + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/models + DEPENDS ${PROJECT_SOURCE_DIR}/models/panda.urdf.xacro) + + add_custom_target(generate_panda_lisp ALL DEPENDS ${PROJECT_SOURCE_DIR}/models/panda.l) + else() - message(WARNING "Dependency is not met, so skip generating dual_panda.l") + message(WARNING "Dependency is not met, so skip generating panda.l and dual_panda.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() -### -### panda.l generation -### -add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/models/panda.l - COMMAND rosrun euscollada collada2eus -I panda.urdf -C panda.yaml -O panda.l - WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/models - DEPENDS ${PROJECT_SOURCE_DIR}/models/panda.urdf ${PROJECT_SOURCE_DIR}/models/panda.yaml) - -add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/models/panda.urdf - COMMAND rosrun xacro xacro --inorder panda.urdf.xacro > panda.urdf - WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/models - DEPENDS ${PROJECT_SOURCE_DIR}/models/panda.urdf.xacro) - -add_custom_target(generate_panda_lisp ALL DEPENDS ${PROJECT_SOURCE_DIR}/models/panda.l) - - ############# ## Install ## ############# From dfa63af836818f3ea5bd52a5cee6b9459e6b0506 Mon Sep 17 00:00:00 2001 From: Yuki Asano Date: Fri, 12 May 2023 17:44:21 +0900 Subject: [PATCH 07/31] change name --- .../launch/{panda_control.launch => panda.launch} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename jsk_panda_robot/jsk_panda_startup/launch/{panda_control.launch => panda.launch} (100%) diff --git a/jsk_panda_robot/jsk_panda_startup/launch/panda_control.launch b/jsk_panda_robot/jsk_panda_startup/launch/panda.launch similarity index 100% rename from jsk_panda_robot/jsk_panda_startup/launch/panda_control.launch rename to jsk_panda_robot/jsk_panda_startup/launch/panda.launch From 3a79e150c8f174dcc34241ba5c556359c52a27be Mon Sep 17 00:00:00 2001 From: Yuki Asano Date: Mon, 15 May 2023 09:42:17 +0900 Subject: [PATCH 08/31] change to include franka_control.launch --- .../jsk_panda_startup/launch/panda.launch | 36 +++++++------------ 1 file changed, 12 insertions(+), 24 deletions(-) diff --git a/jsk_panda_robot/jsk_panda_startup/launch/panda.launch b/jsk_panda_robot/jsk_panda_startup/launch/panda.launch index 7212aa0ad4..d306dfc5d3 100644 --- a/jsk_panda_robot/jsk_panda_startup/launch/panda.launch +++ b/jsk_panda_robot/jsk_panda_startup/launch/panda.launch @@ -1,32 +1,20 @@ - - - - - - + + + - - - + - - - + + + + + + - - - - - - - - - [franka_state_controller/joint_states, franka_gripper/joint_states] - [franka_state_controller/joint_states] - - + + From 74fc6e4a871e547dc3855c0f20ab4c0f2ec66c6a Mon Sep 17 00:00:00 2001 From: Yuki Asano Date: Tue, 23 May 2023 13:25:53 +0900 Subject: [PATCH 09/31] change larm to rarm --- .../panda_eus/euslisp/panda-interface.l | 6 +++--- jsk_panda_robot/panda_eus/models/panda.yaml | 18 +++++++++--------- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/jsk_panda_robot/panda_eus/euslisp/panda-interface.l b/jsk_panda_robot/panda_eus/euslisp/panda-interface.l index f410d98f93..d7f5c5dbb3 100644 --- a/jsk_panda_robot/panda_eus/euslisp/panda-interface.l +++ b/jsk_panda_robot/panda_eus/euslisp/panda-interface.l @@ -54,17 +54,17 @@ (cons :action-type control_msgs::FollowJointTrajectoryAction) (cons :joint-names (send-all (send robot :joint-list) :name)) ))) - (:larm-controller + (:rarm-controller () (list (list - (cons :controller-action "/larm_controller/follow_joint_trajectory") + (cons :controller-action "/rarm_controller/follow_joint_trajectory") (cons :controller-state "/position_joint_trajectory_controller/follow_joint_trajectory") (cons :action-type control_msgs::FollowJointTrajectoryAction) (cons :joint-names (remove-if #'(lambda (jn) (substringp "finger" jn)) (mapcar #'(lambda (n) (if (symbolp n) (symbol-name n) n)) - (send-all (send robot :larm :joint-list) :name))))))) + (send-all (send robot :rarm :joint-list) :name))))))) (:hand-controller () (list diff --git a/jsk_panda_robot/panda_eus/models/panda.yaml b/jsk_panda_robot/panda_eus/models/panda.yaml index 1a25f11681..03d3457795 100644 --- a/jsk_panda_robot/panda_eus/models/panda.yaml +++ b/jsk_panda_robot/panda_eus/models/panda.yaml @@ -1,13 +1,13 @@ -larm: - - panda_joint1 : larm-collar-y - - panda_joint2 : larm-shoulder-p - - panda_joint3 : larm-shoulder-y - - panda_joint4 : larm-elbow-p - - panda_joint5 : larm-wrist-r - - panda_joint6 : larm-wrist-p - - panda_joint7 : larm-wrist-y +rarm: + - panda_joint1 : rarm-collar-y + - panda_joint2 : rarm-shoulder-p + - panda_joint3 : rarm-shoulder-y + - panda_joint4 : rarm-elbow-p + - panda_joint5 : rarm-wrist-r + - panda_joint6 : rarm-wrist-p + - panda_joint7 : rarm-wrist-y -larm-end-coords: +rarm-end-coords: parent: panda_link8 translate: [0, 0, 0.1] rotate : [-0.35740972270209, -0.86285515644477, -0.35740630816298, 98.42112728719206] From 0c7347bd99807b678e46e1b2a8161df9796856a7 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Fri, 19 May 2023 11:29:53 +0900 Subject: [PATCH 10/31] panda.urdf.xacro is unnecessary --- jsk_panda_robot/jsk_panda_startup/launch/panda.launch | 1 - jsk_panda_robot/panda_eus/CMakeLists.txt | 11 +++++++---- jsk_panda_robot/panda_eus/models/panda.urdf.xacro | 5 ----- 3 files changed, 7 insertions(+), 10 deletions(-) delete mode 100644 jsk_panda_robot/panda_eus/models/panda.urdf.xacro diff --git a/jsk_panda_robot/jsk_panda_startup/launch/panda.launch b/jsk_panda_robot/jsk_panda_startup/launch/panda.launch index d306dfc5d3..689d57318f 100644 --- a/jsk_panda_robot/jsk_panda_startup/launch/panda.launch +++ b/jsk_panda_robot/jsk_panda_startup/launch/panda.launch @@ -15,6 +15,5 @@ - diff --git a/jsk_panda_robot/panda_eus/CMakeLists.txt b/jsk_panda_robot/panda_eus/CMakeLists.txt index e29a0217aa..6727ab33cd 100644 --- a/jsk_panda_robot/panda_eus/CMakeLists.txt +++ b/jsk_panda_robot/panda_eus/CMakeLists.txt @@ -16,7 +16,7 @@ if(franka_description_FOUND AND (NOT ("${franka_description_VERSION}" VERSION_LESS "${_franka_description_min_ver}")) AND (NOT ("${xacro_VERSION}" VERSION_LESS "${_xacro_min_ver}"))) # Why franka_description >= 0.10.0: - # panda.urdf.xacro and dual_panda.urdf.xacro assumes file structure of franka_description >= 0.10.0. + # dual_panda.urdf.xacro assumes file structure of franka_description >= 0.10.0. # See https://github.com/frankaemika/franka_ros/compare/0.9.1...0.10.0 for details. # Why xacro >= 1.13.14: # xacro.load_yaml cannot be recognized when xacro < 1.13.14, while it is recommended when xacro >= 1.13.14. @@ -40,14 +40,17 @@ if(franka_description_FOUND ### ### panda.l generation ### + set(_panda_xacro ${franka_description_SOURCE_PREFIX}/robots/panda/panda.urdf.xacro) # franka_description is installed from source + if(NOT EXISTS ${_panda_xacro}) + set(_panda_xacro ${franka_description_PREFIX}/share/franka_description/robots/panda/panda.urdf.xacro) # franka_description is installed from apt + endif() add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/models/panda.l COMMAND rosrun euscollada collada2eus -I panda.urdf -C panda.yaml -O panda.l WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/models DEPENDS ${PROJECT_SOURCE_DIR}/models/panda.urdf ${PROJECT_SOURCE_DIR}/models/panda.yaml) add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/models/panda.urdf - COMMAND rosrun xacro xacro --inorder panda.urdf.xacro > panda.urdf - WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/models - DEPENDS ${PROJECT_SOURCE_DIR}/models/panda.urdf.xacro) + COMMAND rosrun xacro xacro --inorder ${_panda_xacro} hand:=true > ${PROJECT_SOURCE_DIR}/models/panda.urdf + DEPENDS ${_panda_xacro}) add_custom_target(generate_panda_lisp ALL DEPENDS ${PROJECT_SOURCE_DIR}/models/panda.l) diff --git a/jsk_panda_robot/panda_eus/models/panda.urdf.xacro b/jsk_panda_robot/panda_eus/models/panda.urdf.xacro deleted file mode 100644 index 0f0058ab53..0000000000 --- a/jsk_panda_robot/panda_eus/models/panda.urdf.xacro +++ /dev/null @@ -1,5 +0,0 @@ - - - - - From 21dcf545a8f9dd1677706d1272be0ff3e2106269 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Fri, 19 May 2023 15:36:34 +0900 Subject: [PATCH 11/31] [panda_eus] Add panda.urdf and panda.l to .gitignore --- jsk_panda_robot/panda_eus/.gitignore | 2 ++ 1 file changed, 2 insertions(+) diff --git a/jsk_panda_robot/panda_eus/.gitignore b/jsk_panda_robot/panda_eus/.gitignore index da9165908a..97a6d36fcf 100644 --- a/jsk_panda_robot/panda_eus/.gitignore +++ b/jsk_panda_robot/panda_eus/.gitignore @@ -1,2 +1,4 @@ models/dual_panda.l models/dual_panda.urdf +models/panda.l +models/panda.urdf From 0a7fad91b34a3fb2e3abe7246bbc15a387440a00 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Wed, 24 May 2023 20:31:57 +0900 Subject: [PATCH 12/31] [panda_eus] Extract common part of panda-interface.l and dual_panda-interface.l as panda-common-interface.l --- .../panda_eus/euslisp/dual_panda-interface.l | 286 ++------------ .../panda_eus/euslisp/dual_panda-utils.l | 29 ++ .../euslisp/panda-common-interface.l | 371 ++++++++++++++++++ .../panda_eus/euslisp/panda-interface.l | 230 ++--------- .../panda_eus/euslisp/panda-utils.l | 26 ++ 5 files changed, 481 insertions(+), 461 deletions(-) create mode 100644 jsk_panda_robot/panda_eus/euslisp/dual_panda-utils.l create mode 100644 jsk_panda_robot/panda_eus/euslisp/panda-common-interface.l create mode 100644 jsk_panda_robot/panda_eus/euslisp/panda-utils.l diff --git a/jsk_panda_robot/panda_eus/euslisp/dual_panda-interface.l b/jsk_panda_robot/panda_eus/euslisp/dual_panda-interface.l index a8620e0b85..4f90c9b0e9 100644 --- a/jsk_panda_robot/panda_eus/euslisp/dual_panda-interface.l +++ b/jsk_panda_robot/panda_eus/euslisp/dual_panda-interface.l @@ -1,264 +1,42 @@ -(require :robot-interface "package://pr2eus/robot-interface.l") -(require :dual_panda "package://panda_eus/models/dual_panda.l") - -(ros::roseus-add-msgs "franka_msgs") -(ros::roseus-add-msgs "franka_gripper") +(require :panda-common-interface "package://panda_eus/euslisp/panda-common-interface.l") +(require :dual_panda-utils "package://panda_eus/euslisp/dual_panda-utils.l") (defclass dual_panda-robot-interface - :super robot-interface - :slots (error-recovery-act - r-error l-error - gripper-grasp-actions gripper-move-actions gripper-homing-actions gripper-stop-actions - ) - ) + :super panda-common-interface + :slots ()) + (defmethod dual_panda-robot-interface (:init - (&rest args) - (prog1 - (send-super* :init :robot dual_panda-robot - :joint-states-topic "dual_panda/joint_states" - args) - ;; for error recovery - (ros::create-nodehandle "error_group") - (ros::subscribe "/dual_panda/rarm/has_error" std_msgs::Bool - #'send self :callback-rarm-error 1 :groupname "error_group") - (ros::subscribe "/dual_panda/larm/has_error" std_msgs::Bool - #'send self :callback-larm-error 1 :groupname "error_group") - (setq error-recovery-act (instance ros::simple-action-client :init - "/dual_panda/error_recovery" - franka_msgs::ErrorRecoveryAction - :groupname "error_group" - )) - ;; actions for gripper - (setq gripper-grasp-actions (make-hash-table)) - (setq gripper-homing-actions (make-hash-table)) - (setq gripper-move-actions (make-hash-table)) - (setq gripper-stop-actions (make-hash-table)) - (dolist (arm (list :rarm :larm)) - (sethash arm gripper-grasp-actions - (instance ros::simple-action-client :init - (format nil "/dual_panda/~a/franka_gripper/grasp" - (string-downcase (string arm))) - franka_gripper::GraspAction)) - (sethash arm gripper-homing-actions - (instance ros::simple-action-client :init - (format nil "/dual_panda/~a/franka_gripper/homing" - (string-downcase (string arm))) - franka_gripper::HomingAction)) - (sethash arm gripper-move-actions - (instance ros::simple-action-client :init - (format nil "/dual_panda/~a/franka_gripper/move" - (string-downcase (string arm))) - franka_gripper::MoveAction)) - (sethash arm gripper-stop-actions - (instance ros::simple-action-client :init - (format nil "/dual_panda/~a/franka_gripper/stop" - (string-downcase (string arm))) - franka_gripper::StopAction))) - )) + (&rest args) + (let ((all-arms (list :rarm :larm))) + (send-super* :init :robot dual_panda-robot + :joint-states-topic "dual_panda/joint_states" + :all-arms all-arms + :all-arm-aliases (mapcar #'(lambda (arm) nil) all-arms) + :error-topics (mapcar + #'(lambda (arm) + (format nil "/dual_panda/~a/has_error" + (string-downcase (string arm)))) + all-arms) + :error-topic-types (mapcar #'(lambda (arm) std_msgs::Bool) all-arms) + :error-recovery-action "/dual_panda/error_recovery" + :gripper-action-prefixes (mapcar + #'(lambda (arm) + (format nil "/dual_panda/~a" + (string-downcase (string arm)))) + all-arms) + args))) (:default-controller - () - (list + () (list - (cons :controller-action "/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory") - (cons :controller-state "/dual_panda/dual_panda_effort_joint_trajectory_controller/state") - (cons :action-type control_msgs::FollowJointTrajectoryAction) - (cons :joint-names (send-all (send robot :joint-list) :name)) - ))) - (:set-joint-pd-gain - (joint-name pgain dgain) - "Set P gain and D gain of each joint" - (let ((req (instance dynamic_reconfigure::ReconfigureRequest :init))) - (send req :config :doubles - (list (instance dynamic_reconfigure::DoubleParameter :init - :name "p" :value pgain) - (instance dynamic_reconfigure::DoubleParameter :init - :name "d" :value dgain))) - (ros::service-call - (format nil "/dual_panda/dual_panda_effort_joint_trajectory_controller/gains/~A/set_parameters" joint-name) - req) - )) - (:set-all-joint-pd-gain - (pgain dgain) - "Set P gain and D gain of all joints" - (dolist (j (send robot :joint-list)) - (send self :set-joint-pd-gain (send j :name) pgain dgain)) - ) - (:check-error - () - "Check if the robot has error. -If this method returns T, you must call :recover-error to move the robot. -" - (ros::spin-once "error_group") - (or r-error l-error) - ) - (:callback-rarm-error - (msg) - (setq r-error (send msg :data)) - ) - (:callback-larm-error - (msg) - (setq l-error (send msg :data)) - ) - (:wait-recover-error () (send error-recovery-act :wait-for-result)) - (:recover-error - (&key (wait t)) - "Recover from errors and reflexes. -Details: `ErrorRecoveryAction` part of https://frankaemika.github.io/docs/franka_ros.html#franka-control -" - (let ((goal (instance franka_msgs::ErrorRecoveryActionGoal :init))) - (send goal :header :stamp (ros::time-now)) - (send error-recovery-act :send-goal goal) - (if wait (send self :wait-recover-error)) - )) - ;; gripper action for real-controller - (:send-gripper-grasp-action - (arm width speed force &key (wait t) (inner 0.005) (outer 0.07)) - (let ((goal (instance franka_gripper::GraspActionGoal :init))) - (send goal :header :stamp (ros::time-now)) - (send goal :goal :width width) ;; [m] - (send goal :goal :speed speed) ;; [m/s] - (send goal :goal :force force) ;; [N] - (send goal :goal :epsilon :inner inner) ;; [m] - (send goal :goal :epsilon :outer outer) ;; [m] - ;; - (send (gethash arm gripper-grasp-actions) :send-goal goal) - (if wait (send (gethash arm gripper-grasp-actions) :wait-for-result)) - )) - (:send-gripper-homing-action - (arm &key (wait t)) - (let ((goal (instance franka_gripper::HomingActionGoal :init))) - (send goal :header :stamp (ros::time-now)) - ;; - (send (gethash arm gripper-homing-actions) :send-goal goal) - (if wait (send (gethash arm gripper-homing-actions) :wait-for-result)) - )) - (:send-gripper-move-action - (arm width speed &key (wait t)) - (let ((goal (instance franka_gripper::MoveActionGoal :init))) - (send goal :header :stamp (ros::time-now)) - (send goal :goal :width width) ;; [m] - (send goal :goal :speed speed) ;; [m/s] - ;; - (send (gethash arm gripper-move-actions) :send-goal goal) - (if wait (send (gethash arm gripper-move-actions) :wait-for-result)) - )) - (:send-gripper-stop-action - (arm &key (wait t)) - (let ((goal (instance franka_gripper::StopActionGoal :init))) - (send goal :header :stamp (ros::time-now)) - ;; - (send (gethash arm gripper-stop-actions) :send-goal goal) - (if wait (send (gethash arm gripper-stop-actions) :wait-for-result)) - )) - (:arm2arms - (arm) - (case arm - ((:rarm :larm) (list arm)) - (:arms (list :rarm :larm)) - )) - (:gripper-method-helper - (action-sender actions arm wait) - (let ((arms (send self :arm2arms arm))) - (when arms - (dolist (a arms) - (send self action-sender a :wait nil)) - (if wait - (mapcar #'(lambda (a) (send (gethash a actions) :wait-for-result)) - arms)) - ))) - (:stop-gripper - (arm &key (wait nil)) - "Abort a running gripper action. This can be used to stop applying forces after grasping. -Details: `StopAction` part of https://frankaemika.github.io/docs/franka_ros.html#franka-gripper -" - (send self :gripper-method-helper - :send-gripper-stop-action gripper-stop-actions arm wait) - ) - (:homing-gripper - (arm &key (wait nil)) - "Home the gripper and update the maximum width given the mounted fingers (i.e., calibrate & initialize the gripper). -Details: `HomingAction` part of https://frankaemika.github.io/docs/franka_ros.html#franka-gripper -" - (send self :gripper-method-helper - :send-gripper-homing-action gripper-homing-actions arm wait) - ) - (:gripper - (&rest args) - "Get information of gripper -Arguments: -- arm : :rarm, :larm, or :arms -- type : :position ([m]) -Example: (send self :gripper :rarm :position) => 0.00 -" - (when (eq (car args) :arms) - (return-from :gripper - (mapcar #'(lambda (x) - (send self :gripper x (cadr args))) - '(:larm :rarm)))) - (unless (memq (car args) '(:larm :rarm)) - (error "you must specify arm ~A from ~A" (car args) '(:larm :rarm)) - (return-from :gripper nil)) - (send self :update-robot-state) - (case (cadr args) - (:position (* 0.001 (v. (send robot (car args) :gripper :angle-vector) - #f(0 0 0 0 1 0 0 1)))) - )) - (:gripper-method-with-width-helper - (action-sender actions arm wait width tm &rest args) - (let ((arms (send self :arm2arms arm))) - (when arms - (dolist (a arms) - (send* self action-sender a width - (/ (abs (- (send self :gripper a :position) width)) - (/ tm 1000.0)) - (append args (list :wait nil)))) - (if wait - (mapcar #'(lambda (a) (send (gethash a actions) :wait-for-result)) - arms)) - ))) - (:start-grasp - (arm &key (width 0.0) (effort 80.0) (tm 500) (wait nil) (inner 0.005) (outer 0.06)) - "Try to grasp at the desired `width` with the desired `effort` while closing with the desired speed calculated from `tm`. -Arguments: -- arm : :rarm, :larm, or :arms -- width : target distance between the fingers [m] -- effort : target effort [N] -- tm : time to target [ms]. This will be converted to the movement speed -- wait : if this argument is T, this method waits until the movement finishes -- inner : lower admissible error of width. If this is violated, the gripper stops applying forces -- outer : upper admissible error of width. If this is violated, the gripper stops applying forces - Details: https://github.com/ykawamura96/jsk_robot/pull/1#issuecomment-860324988 -Details: `GraspAction` part of https://frankaemika.github.io/docs/franka_ros.html#franka-gripper -" - (send self :gripper-method-with-width-helper - :send-gripper-grasp-action gripper-grasp-actions arm wait width tm - effort :inner inner :outer outer) - ) - (:stop-grasp - (arm &key (wait nil) (width 0.08)) - "Open the gripper to the target `width` [m]" - (unless (memq arm '(:larm :rarm :arms)) - (error "you must specify arm ~A from ~A" (car args) '(:larm :rarm :arms)) - (return-from :stop-grasp nil)) - (send self :move-gripper arm width :tm 500 :wait wait) - ) - (:move-gripper - (arm width &key (tm 500) (wait nil)) - "Move the gripper to the target `width` [m] while closing with the desired speed calculated from `tm` [ms]. -Details: `MoveAction` part of https://frankaemika.github.io/docs/franka_ros.html#franka-gripper -" - (send self :gripper-method-with-width-helper - :send-gripper-move-action gripper-move-actions arm wait width tm) - ) - ) + (list + (cons :controller-action "/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory") + (cons :controller-state "/dual_panda/dual_panda_effort_joint_trajectory_controller/state") + (cons :action-type control_msgs::FollowJointTrajectoryAction) + (cons :joint-names (send-all (send robot :joint-list) :name)))))) (defun dual_panda-init () (setq *ri* (instance dual_panda-robot-interface :init)) - (setq *robot* (dual_panda)) - ) + (setq *robot* (dual_panda))) -#| -You can check current gains with rqt_reconfigure. -Be careful changing them with :set-joint-pd-gain or :set-all-joint-pd-gain -|# +(provide :dual_panda-interface) diff --git a/jsk_panda_robot/panda_eus/euslisp/dual_panda-utils.l b/jsk_panda_robot/panda_eus/euslisp/dual_panda-utils.l new file mode 100644 index 0000000000..4f8b9a1ab4 --- /dev/null +++ b/jsk_panda_robot/panda_eus/euslisp/dual_panda-utils.l @@ -0,0 +1,29 @@ +(require :dual_panda "package://panda_eus/models/dual_panda.l") + +(defmethod dual_panda-robot + (: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 : :rarm, :larm, or :arms +- width : target distance between the fingers [m] +" + (let ((arms (case arm + ((:rarm :larm) (list arm)) + (:arms (list :rarm :larm))))) + (dolist (am arms) + (send-all + (remove nil (mapcar + #'(lambda (jt) + (if (= (send jt :min-angle) (send jt :max-angle)) nil jt)) + (send self am :gripper :joint-list))) + ;; Get joint list of gripper excluding fixed joints + :joint-angle (* (/ width 2.0) 1000)))))) + +(provide :dual_panda-utils) diff --git a/jsk_panda_robot/panda_eus/euslisp/panda-common-interface.l b/jsk_panda_robot/panda_eus/euslisp/panda-common-interface.l new file mode 100644 index 0000000000..091769ef4f --- /dev/null +++ b/jsk_panda_robot/panda_eus/euslisp/panda-common-interface.l @@ -0,0 +1,371 @@ +(require :robot-interface "package://pr2eus/robot-interface.l") + +(ros::roseus-add-msgs "franka_msgs") +(ros::roseus-add-msgs "franka_gripper") + +(defclass panda-common-interface + :super robot-interface + :slots (all-arms all-arm-aliases + arm-error-p error-recovery-act + gripper-grasp-actions gripper-move-actions gripper-homing-actions gripper-stop-actions + ) + ) +(defmethod panda-common-interface + (:init + (&rest args &key (robot nil) + (joint-states-topic nil) + ((:all-arms aa) nil) + ((:all-arm-aliases aaa) nil) + (error-topics nil) + (error-topic-types nil) + (error-recovery-action nil) + (gripper-action-prefixes nil) + &allow-other-keys) + "Create robot interface for panda +Arguments specific to panda: +- all-arms : names of all arms (e.g., (list :rarm)) +- all-arm-aliases : aliases of all arms (e.g., (list :arm)) +- error-topics : topics used for getting error states of arms (e.g., (list \"/franka_state_controller/franka_states\")) +- error-topic-types : message types of error-topics (e.g., (list franka_msgs::FrankaState)) +- error-recovery-action : action for error recovery (e.g., \"/franka_control/error_recovery\") +- gripper-action-prefixes : prefixes added to gripper action names (e.g., (list \"\")) +" + (setq all-arms aa) + (setq all-arm-aliases aaa) + (unless (= (length all-arms) (length all-arm-aliases) + (length error-topics) (length error-topic-types) + (length gripper-action-prefixes)) + (error + "all-arms, all-arm-aliases, error-topics, error-topic-types, and gripper-action-prefixes must have the same length") + (return-from :init nil)) + (prog1 + (send-super* :init :robot robot + :joint-states-topic joint-states-topic + args) + ;; for error recovery + (ros::create-nodehandle "error_group") + (setq arm-error-p (make-hash-table)) + (dotimes (i (length all-arms)) + (let ((arm (elt all-arms i)) (etype (elt error-topic-types i))) + (sethash arm arm-error-p nil) + (cond + ((eq etype std_msgs::Bool) + ;; franka_combined_control.launch (used in dual_panda) publishes has_error topic (800Hz) + (ros::subscribe + (elt error-topics i) std_msgs::Bool + #'send self :callback-error arm + 1 :groupname "error_group")) + ((eq etype franka_msgs::FrankaState) + ;; franka_control.launch (used in single panda) does not publish has_error topic. + ;; We use franka_states topic instead, but its frequency is lower (30Hz). + ;; We do not use franka_combined_control.launch in single panda for backward compatibility. + ;; position_joint_trajectory_controller, which we used in single panda so far, cannot be used on franka_combined_control.launch. + ;; This is because + ;; https://github.com/frankaemika/franka_ros/blob/0.10.1/franka_hw/src/franka_hw.cpp#L528-L529 + ;; does not exist in + ;; https://github.com/frankaemika/franka_ros/blob/0.10.1/franka_hw/src/franka_combinable_hw.cpp#L26-L39. + (ros::subscribe + (elt error-topics i) franka_msgs::FrankaState + #'send self :callback-franka-state arm + 1 :groupname "error_group")) + (t + (ros::ros-error "error-topic-types includes unsupported type") + (ros::ros-error ":check-error will return meaningless value"))))) + (setq error-recovery-act (instance ros::simple-action-client :init + error-recovery-action + franka_msgs::ErrorRecoveryAction + :groupname "error_group" + )) + ;; actions for gripper + (setq gripper-grasp-actions (make-hash-table)) + (setq gripper-homing-actions (make-hash-table)) + (setq gripper-move-actions (make-hash-table)) + (setq gripper-stop-actions (make-hash-table)) + (dotimes (i (length all-arms)) + (let ((arm (elt all-arms i)) (prefix (elt gripper-action-prefixes i))) + (sethash arm gripper-grasp-actions + (instance ros::simple-action-client :init + (format nil "~a/franka_gripper/grasp" prefix) + franka_gripper::GraspAction)) + (sethash arm gripper-homing-actions + (instance ros::simple-action-client :init + (format nil "~a/franka_gripper/homing" prefix) + franka_gripper::HomingAction)) + (sethash arm gripper-move-actions + (instance ros::simple-action-client :init + (format nil "~a/franka_gripper/move" prefix) + franka_gripper::MoveAction)) + (sethash arm gripper-stop-actions + (instance ros::simple-action-client :init + (format nil "~a/franka_gripper/stop"prefix) + franka_gripper::StopAction)))) + )) + (:set-joint-pd-gain + (joint-name pgain dgain &key (type :default-controller)) + "Set P gain and D gain of each joint. +This method works only when `type` comes from effort_controllers/JointTrajectoryController and includes `joint-name`. +" + (if (send self :simulation-modep) + (progn + (ros::ros-warn ":set-joint-pd-gain is not implemented on simulation mode, always returns t") + (return-from :set-joint-pd-gain t))) + (let ((req (instance dynamic_reconfigure::ReconfigureRequest :init))) + (send req :config :doubles + (list (instance dynamic_reconfigure::DoubleParameter :init + :name "p" :value pgain) + (instance dynamic_reconfigure::DoubleParameter :init + :name "d" :value dgain))) + (dolist (fjt-name (mapcar #'(lambda (joint-param) + (cdr (assoc :controller-action joint-param))) + (send self type))) + (let ((cname (subseq fjt-name 0 (- (length fjt-name) + (position #\/ (reverse fjt-name)) + 1)))) + ;; If fjt-name is "/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory", + ;; cname will be "/dual_panda/dual_panda_effort_joint_trajectory_controller" + (ros::service-call + (format nil "~a/gains/~a/set_parameters" cname joint-name) + req))) + )) + (:set-all-joint-pd-gain + (pgain dgain &key (type :default-controller)) + "Set P gain and D gain of all joints. +This method fully works only when `type` comes from effort_controllers/JointTrajectoryController and includes all joints. +" + (dolist (j (send robot :joint-list)) + (send self :set-joint-pd-gain (send j :name) pgain dgain :type type)) + ) + (:check-error + () + "Check if the robot has error. +If this method returns T, you must call :recover-error to move the robot. +" + (ros::spin-once "error_group") + (reduce #'(lambda (x y) (or x y)) + (mapcar #'(lambda (arm) (gethash arm arm-error-p)) all-arms)) + ) + (:callback-error + (arm msg) + (sethash arm arm-error-p (send msg :data)) + ) + (:callback-franka-state + (arm msg) + (sethash arm arm-error-p + (not (= (send msg :robot_mode) + franka_msgs::FrankaState::*ROBOT_MODE_MOVE*))) + ) + (:wait-recover-error + () + (if (send self :simulation-modep) + (progn + (ros::ros-warn ":wait-recover-error is not implemented on simulation mode, always returns t") + (return-from :wait-recover-error t))) + (send error-recovery-act :wait-for-result) + ) + (:recover-error + (&key (wait t)) + "Recover from errors and reflexes. +Details: `ErrorRecoveryAction` part of https://frankaemika.github.io/docs/franka_ros.html#franka-control +" + (if (send self :simulation-modep) + (progn + (ros::ros-warn ":recover-error is not implemented on simulation mode, always returns t") + (return-from :recover-error t))) + (let ((goal (instance franka_msgs::ErrorRecoveryActionGoal :init))) + (send goal :header :stamp (ros::time-now)) + (send error-recovery-act :send-goal goal) + (if wait (send self :wait-recover-error)) + )) + (:expand-arm-alias + (arm-alias) + (let ((expanded + (if arm-alias + (let ((i (position arm-alias all-arm-aliases))) + (if i + (elt all-arms i) + arm-alias)) + nil))) + (if (memq expanded all-arms) + expanded + (progn + (error "arm ~a does not exist" arm-alias) + nil)) + )) + ;; gripper action for real-controller + (:send-gripper-grasp-action + (arm width speed force &key (wait t) (inner 0.005) (outer 0.07)) + (let ((goal (instance franka_gripper::GraspActionGoal :init)) + (action (gethash (send self :expand-arm-alias arm) gripper-grasp-actions))) + (send goal :header :stamp (ros::time-now)) + (send goal :goal :width width) ;; [m] + (send goal :goal :speed speed) ;; [m/s] + (send goal :goal :force force) ;; [N] + (send goal :goal :epsilon :inner inner) ;; [m] + (send goal :goal :epsilon :outer outer) ;; [m] + ;; + (if action + (progn + (send action :send-goal goal) + (if wait (send action :wait-for-result)))) + )) + (:send-gripper-homing-action + (arm &key (wait t)) + (let ((goal (instance franka_gripper::HomingActionGoal :init)) + (action (gethash (send self :expand-arm-alias arm) gripper-homing-actions))) + (send goal :header :stamp (ros::time-now)) + ;; + (if action + (progn + (send action :send-goal goal) + (if wait (send action :wait-for-result)))) + )) + (:send-gripper-move-action + (arm width speed &key (wait t)) + (let ((goal (instance franka_gripper::MoveActionGoal :init)) + (action (gethash (send self :expand-arm-alias arm) gripper-move-actions))) + (send goal :header :stamp (ros::time-now)) + (send goal :goal :width width) ;; [m] + (send goal :goal :speed speed) ;; [m/s] + ;; + (if action + (progn + (send action :send-goal goal) + (if wait (send action :wait-for-result)))) + )) + (:send-gripper-stop-action + (arm &key (wait t)) + (let ((goal (instance franka_gripper::StopActionGoal :init)) + (action (gethash (send self :expand-arm-alias arm) gripper-stop-actions))) + (send goal :header :stamp (ros::time-now)) + ;; + (if action + (progn + (send action :send-goal goal) + (if wait (send action :wait-for-result)))) + )) + (:arm2arms + (arm) + (case arm + (:arms all-arms) + (t (list arm)) + )) + (:gripper-method-helper + (action-sender actions arm wait) + (let ((arms (send self :arm2arms arm))) + (dolist (a arms) + (send self action-sender a :wait nil)) + (if wait + (mapcar #'(lambda (a) + (let ((action (gethash (send self :expand-arm-alias a) actions))) + (if action (send action :wait-for-result) nil))) + arms)) + )) + (:stop-gripper + (arm &key (wait nil)) + "Abort a running gripper action. This can be used to stop applying forces after grasping. +Details: `StopAction` part of https://frankaemika.github.io/docs/franka_ros.html#franka-gripper +" + (if (send self :simulation-modep) + (progn + (ros::ros-warn ":stop-gripper is not implemented on simulation mode, always returns t") + (return-from :stop-gripper t))) + (send self :gripper-method-helper + :send-gripper-stop-action gripper-stop-actions arm wait) + ) + (:homing-gripper + (arm &key (wait nil)) + "Home the gripper and update the maximum width given the mounted fingers (i.e., calibrate & initialize the gripper). +Details: `HomingAction` part of https://frankaemika.github.io/docs/franka_ros.html#franka-gripper +" + (if (send self :simulation-modep) + (send self :stop-grasp arm :wait wait) + (send self :gripper-method-helper + :send-gripper-homing-action gripper-homing-actions arm wait) + )) + (:gripper + (&rest args) + "Get information of gripper +Arguments: +- arm : :arms or any element of all-arms and all-arm-aliases (e.g., :rarm) +- type : :position ([m]) +Example: (send self :gripper :rarm :position) => 0.00 +" + (if (eq (car args) :arms) + (return-from :gripper + (mapcar #'(lambda (x) + (send self :gripper x (cadr args))) + all-arms))) + (let ((arm (send self :expand-arm-alias (car args)))) + (if arm + (progn + (send self :update-robot-state) + (case (cadr args) + (:position + (* 0.001 + (apply #'+ + (send-all + (remove nil (mapcar #'(lambda (jt) + (if (= (send jt :min-angle) + (send jt :max-angle)) + nil jt)) + (send robot arm :gripper :joint-list))) + ;; Get joint list of gripper excluding fixed joints + :joint-angle)))))))) + ) + (:gripper-method-with-width-helper + (action-sender actions arm wait width tm &rest args) + (let ((arms (send self :arm2arms arm))) + (dolist (a arms) + (send* self action-sender a width + (/ (abs (- (send self :gripper a :position) width)) + (/ tm 1000.0)) + (append args (list :wait nil)))) + (if wait + (mapcar #'(lambda (a) + (let ((action (gethash (send self :expand-arm-alias a) actions))) + (if action (send action :wait-for-result) nil))) + arms)) + )) + (:start-grasp + (arm &key (width 0.0) (effort 80.0) (tm 500) (wait nil) (inner 0.005) (outer 0.06)) + "Try to grasp at the desired `width` with the desired `effort` while closing with the desired speed calculated from `tm`. +Arguments: +- arm : :arms or any element of all-arms and all-arm-aliases (e.g., :rarm) +- width : target distance between the fingers [m] +- effort : target effort [N] +- tm : time to target [ms]. This will be converted to the movement speed +- wait : if this argument is T, this method waits until the movement finishes +- inner : lower admissible error of width. If this is violated, the gripper stops applying forces +- outer : upper admissible error of width. If this is violated, the gripper stops applying forces + Details: https://github.com/ykawamura96/jsk_robot/pull/1#issuecomment-860324988 +Details: `GraspAction` part of https://frankaemika.github.io/docs/franka_ros.html#franka-gripper +" + (if (send self :simulation-modep) + (send robot :start-grasp arm :width width) + (send self :gripper-method-with-width-helper + :send-gripper-grasp-action gripper-grasp-actions arm wait width tm + effort :inner inner :outer outer) + )) + (:stop-grasp + (arm &key (wait nil) (width 0.08)) + "Open the gripper to the target `width` [m]" + (send self :move-gripper arm width :tm 500 :wait wait) + ) + (:move-gripper + (arm width &key (tm 500) (wait nil)) + "Move the gripper to the target `width` [m] while closing with the desired speed calculated from `tm` [ms]. +Details: `MoveAction` part of https://frankaemika.github.io/docs/franka_ros.html#franka-gripper +" + (if (send self :simulation-modep) + (send robot :move-gripper arm width) + (send self :gripper-method-with-width-helper + :send-gripper-move-action gripper-move-actions arm wait width tm) + )) + ) + +(provide :panda-common-interface) + +#| +You can check current joint gains with rqt_reconfigure when you use effort_controllers/JointTrajectoryController. +Be careful changing them with :set-joint-pd-gain or :set-all-joint-pd-gain +|# diff --git a/jsk_panda_robot/panda_eus/euslisp/panda-interface.l b/jsk_panda_robot/panda_eus/euslisp/panda-interface.l index d7f5c5dbb3..daf8ba1fd9 100644 --- a/jsk_panda_robot/panda_eus/euslisp/panda-interface.l +++ b/jsk_panda_robot/panda_eus/euslisp/panda-interface.l @@ -1,217 +1,33 @@ -(require :robot-interface "package://pr2eus/robot-interface.l") -(require :franka "package://panda_eus/models/panda.l") - -(ros::roseus-add-msgs "franka_msgs") -(ros::roseus-add-msgs "franka_gripper") +(require :panda-common-interface "package://panda_eus/euslisp/panda-common-interface.l") +(require :panda-utils "package://panda_eus/euslisp/panda-utils.l") (defclass panda-robot-interface - :super robot-interface - :slots (gripper-action - error-recovery-act error - gripper-grasp-action gripper-move-action gripper-homing-action gripper-stop-action - ) - ) + :super panda-common-interface + :slots ()) + (defmethod panda-robot-interface (:init - (&rest args &key ((:controller-timeout ct) nil)) - (prog1 - (send-super* :init :robot panda-robot - :joint-states-topic "joint_states" - :controller-timeout ct args) - ;; for error recovery - (ros::create-nodehandle "error_group") - (ros::subscribe "/panda/arm/has_error" std_msgs::Bool - #'send self :callback-arm-error 1 :groupname "error_group") - (setq error-recovery-act (instance ros::simple-action-client :init - "/franka_control/error_recovery" - franka_msgs::ErrorRecoveryAction - :groupname "error_group" - )) - ;; actions for gripper - (setq gripper-grasp-action - (instance ros::simple-action-client :init - "/franka_gripper/grasp" - franka_gripper::GraspAction)) - (setq gripper-homing-action - (instance ros::simple-action-client :init - "/franka_gripper/homing" - franka_gripper::HomingAction)) - (setq gripper-move-action - (instance ros::simple-action-client :init - "/franka_gripper/move" - franka_gripper::MoveAction)) - (setq gripper-stop-action - (instance ros::simple-action-client :init - "/franka_gripper/stop" - franka_gripper::StopAction)) - )) + (&rest args) + (send-super* :init :robot panda-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)) - ))) - (:rarm-controller - () - (list - (list - (cons :controller-action "/rarm_controller/follow_joint_trajectory") - (cons :controller-state "/position_joint_trajectory_controller/follow_joint_trajectory") - (cons :action-type control_msgs::FollowJointTrajectoryAction) - (cons :joint-names - (remove-if #'(lambda (jn) (substringp "finger" jn)) - (mapcar #'(lambda (n) (if (symbolp n) (symbol-name n) n)) - (send-all (send robot :rarm :joint-list) :name))))))) - (:hand-controller - () - (list + () (list - (cons :controller-action "/hand_controller/follow_joint_trajectory") - (cons :controller-state "/hand_controller/state") - (cons :action-type control_msgs::FollowJointTrajectoryAction) - (cons :joint-names (list "left_finger_joint1" "left_finger_joint2") )))) - (:set-joint-pd-gain - (joint-name pgain dgain) - (let ((req (instance dynamic_reconfigure::ReconfigureRequest :init))) - (send req :config :doubles - (list (instance dynamic_reconfigure::DoubleParameter :init - :name "p" :value pgain) - (instance dynamic_reconfigure::DoubleParameter :init - :name "d" :value dgain))) - (ros::service-call - (format nil "/position_joint_trajectory_controller/gains/~A/set_parameters" joint-name) - req) - )) - (:set-all-joint-pd-gain - (pgain dgain) - (dolist (j (send robot :joint-list)) - (send self :set-joint-pd-gain (send j :name) pgain dgain)) - ) - (:check-error () - (ros::spin-once "error_group") - (or error) - ) - (:callback-arm-error (msg) - (setq error (send msg :data)) - ) - (:wait-recover-error () (send error-recovery-act :wait-for-result)) - (:recover-error (&key (wait t)) - (let ((goal (instance franka_msgs::ErrorRecoveryActionGoal :init))) - (send goal :header :stamp (ros::time-now)) - (send error-recovery-act :send-goal goal) - (if wait (send self :wait-recover-error)) - )) - ;; gripper action for real-controller - (:send-gripper-grasp-action - (act width speed force &key (wait t) (inner 0.005) (outer 0.005)) - (let ((goal (instance franka_gripper::GraspActionGoal :init))) - (send goal :header :stamp (ros::time-now)) - (send goal :goal :width width) ;; [m] - (send goal :goal :speed speed) ;; [m/s] - (send goal :goal :force force) ;; [N] - (send goal :goal :epsilon :inner inner) ;; [m] - (send goal :goal :epsilon :outer outer) ;; [m] - ;; - (send act :send-goal goal) - (if wait (send act :wait-for-result)) - )) - (:send-gripper-homing-action - (act &key (wait t)) - (let ((goal (instance franka_gripper::HomingActionGoal :init))) - (send goal :header :stamp (ros::time-now)) - ;; - (send act :send-goal goal) - (if wait (send act :wait-for-result)) - )) - (:send-gripper-move-action - (act width speed &key (wait t)) - (let ((goal (instance franka_gripper::MoveActionGoal :init))) - (send goal :header :stamp (ros::time-now)) - (send goal :goal :width width) ;; [m] - (send goal :goal :speed speed) ;; [m/s] - ;; - (send act :send-goal goal) - (if wait (send act :wait-for-result)) - )) - (:send-gripper-stop-action - (act &key (wait t)) - (let ((goal (instance franka_gripper::StopActionGoal :init))) - (send goal :header :stamp (ros::time-now)) - ;; - (send act :send-goal goal) - (if wait (send act :wait-for-result)) - )) - (:stop-gripper - (arm &key (wait nil)) - (let (acts) - (case - arm - (:arm (setq acts (list gripper-stop-action))) - ) - (when acts - (dolist (act acts) - (send self :send-gripper-stop-action act - pos (/ (* 1000 0.08) tm) :wait nil)) - (if wait (mapcar #'(lambda (act) (send act :wait-for-result)) acts)) - ))) - (:homing-gripper - (arm &key (wait nil)) - (let (acts) - (case - arm - (:arm (setq acts (list gripper-homing-action))) - ) - (when acts - (dolist (act acts) - (send self :send-gripper-homing-action act :wait nil)) - (if wait (mapcar #'(lambda (act) (send act :wait-for-result)) acts)) - ))) - (:start-grasp - (arm &optional (pos 0.0) &key (effort 10.0) (tm 500) (wait nil)) ;; TODO :arms is not implemented - (let (acts) - (case - arm - (:arm (setq acts (list gripper-grasp-action))) - ) - (when acts - (dolist (act acts) - (send self :send-gripper-grasp-action act - pos (/ (* 1000 0.08) tm) effort :wait nil)) - (if wait (mapcar #'(lambda (act) (send act :wait-for-result)) acts)) - ))) - (:stop-grasp - (arm &key (wait nil)) ;; TODO :arms is not implemented - (unless (memq arm '(:arm :arms)) - (error "you must specify arm ~A from ~A" (car args) '(:arm :arms)) - (return-from :stop-grasp nil)) - (send self :move-gripper arm 0.08 :tm 500 :wait wait) - ) - (:move-gripper - (arm pos &key (tm 500) (timeout 5000) (wait nil)) ;; TODO :arms is not implemented - (let (acts) - (case - arm - (:arm (setq acts (list gripper-move-action))) - ) - (when acts - (dolist (act acts) - (send self :send-gripper-move-action act - pos (/ (* 1000 0.08) tm) :wait nil)) - (if wait (mapcar #'(lambda (act) (send act :wait-for-result)) acts)) - ))) - ) + (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)))))) -;; grasp controller ... (defun panda-init () (setq *ri* (instance panda-robot-interface :init)) - (setq *robot* (panda)) - ) + (setq *robot* (panda))) -#| -(send *ri* :set-all-joint-pd-gain 1000.0 5.0) ;; default -(send *ri* :set-all-joint-pd-gain 300.0 5.0) ;; hard -(send *ri* :set-all-joint-pd-gain 30.0 0.5) ;; soft -|# +(provide :panda-interface) diff --git a/jsk_panda_robot/panda_eus/euslisp/panda-utils.l b/jsk_panda_robot/panda_eus/euslisp/panda-utils.l new file mode 100644 index 0000000000..1657664f38 --- /dev/null +++ b/jsk_panda_robot/panda_eus/euslisp/panda-utils.l @@ -0,0 +1,26 @@ +(require :panda "package://panda_eus/models/panda.l") + +(defmethod panda-robot + (:arm (&rest args) (send* self :rarm args)) ;; Enable to call (send *panda* :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 panda-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 :panda-utils) From 9dfe6fbb1d32e18355360c68cbd3e04e3b15490e Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Thu, 25 May 2023 21:52:48 +0900 Subject: [PATCH 13/31] [jsk_panda_robot] Add single panda to README --- jsk_panda_robot/README.md | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/jsk_panda_robot/README.md b/jsk_panda_robot/README.md index 7e41b7e428..23e035979f 100644 --- a/jsk_panda_robot/README.md +++ b/jsk_panda_robot/README.md @@ -49,6 +49,34 @@ ``` +## Running single Panda +### 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:= + ``` + +2. Controlling single Panda 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) + (send *robot* :angle-vector (send *robot* :reset-pose)) + (when (send *ri* :check-error) + (send *ri* :recover-error)) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + ``` + `(send *ri* :recover-error)` is required every time when you press and release the black switch (`activated` -> `monitored stop` -> `activated`). + + ## Running Dual-Panda ### Boot robot 1. Please turn on the controller box and unlock joints by accessing desk. From c0bba7974001ad4acb9aea4b10011c1c84d07107 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Fri, 26 May 2023 10:04:52 +0900 Subject: [PATCH 14/31] [jsk_panda_robot] Add notice about end-coords, reset-pose, and reset-manip-pose --- jsk_panda_robot/README.md | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/jsk_panda_robot/README.md b/jsk_panda_robot/README.md index 23e035979f..eb353bde2b 100644 --- a/jsk_panda_robot/README.md +++ b/jsk_panda_robot/README.md @@ -74,7 +74,8 @@ (send *ri* :recover-error)) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) ``` - `(send *ri* :recover-error)` is required every time when you press and release the black switch (`activated` -> `monitored stop` -> `activated`). + - Notice + - `(send *ri* :recover-error)` is required every time when you press and release the black switch (`activated` -> `monitored stop` -> `activated`). ## Running Dual-Panda @@ -104,7 +105,9 @@ (send *ri* :recover-error)) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) ``` - `(send *ri* :recover-error)` is required every time when you press and release the black switch (`activated` -> `monitored stop` -> `activated`). + - Notice + - `(send *ri* :recover-error)` is required every time when you press and release the black switch (`activated` -> `monitored stop` -> `activated`). + - `dual_panda`'s `end-coords`, `reset-pose`, and `reset-manip-pose` are different from single `panda`'s ones for historical reasons. #### Record/play rosbag ```bash roslaunch jsk_panda_startup dual_panda1_record.launch # Or roslaunch jsk_panda_startup dual_panda2_record.launch From 65cc004e9be53228ab2a88544456d2fb6708c817 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Fri, 26 May 2023 13:10:51 +0900 Subject: [PATCH 15/31] [panda_eus] Improve error message from :set-joint-pd-gain --- .../euslisp/panda-common-interface.l | 26 ++++++++++++------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/jsk_panda_robot/panda_eus/euslisp/panda-common-interface.l b/jsk_panda_robot/panda_eus/euslisp/panda-common-interface.l index 091769ef4f..44c3436138 100644 --- a/jsk_panda_robot/panda_eus/euslisp/panda-common-interface.l +++ b/jsk_panda_robot/panda_eus/euslisp/panda-common-interface.l @@ -103,13 +103,13 @@ Arguments specific to panda: (:set-joint-pd-gain (joint-name pgain dgain &key (type :default-controller)) "Set P gain and D gain of each joint. -This method works only when `type` comes from effort_controllers/JointTrajectoryController and includes `joint-name`. +This method works only when `type` includes a controller coming from effort_controllers/JointTrajectoryController and including `joint-name`. " (if (send self :simulation-modep) (progn (ros::ros-warn ":set-joint-pd-gain is not implemented on simulation mode, always returns t") (return-from :set-joint-pd-gain t))) - (let ((req (instance dynamic_reconfigure::ReconfigureRequest :init))) + (let ((req (instance dynamic_reconfigure::ReconfigureRequest :init)) (set-p nil)) (send req :config :doubles (list (instance dynamic_reconfigure::DoubleParameter :init :name "p" :value pgain) @@ -118,19 +118,27 @@ This method works only when `type` comes from effort_controllers/JointTrajectory (dolist (fjt-name (mapcar #'(lambda (joint-param) (cdr (assoc :controller-action joint-param))) (send self type))) - (let ((cname (subseq fjt-name 0 (- (length fjt-name) - (position #\/ (reverse fjt-name)) - 1)))) + (let* ((cname (subseq fjt-name 0 (- (length fjt-name) + (position #\/ (reverse fjt-name)) + 1))) + (srv-name (format nil "~a/gains/~a/set_parameters" cname joint-name))) ;; If fjt-name is "/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory", ;; cname will be "/dual_panda/dual_panda_effort_joint_trajectory_controller" - (ros::service-call - (format nil "~a/gains/~a/set_parameters" cname joint-name) - req))) + (if (ros::wait-for-service srv-name 0) + (progn + (ros::service-call srv-name req) + (setq set-p t))))) + (if (not set-p) + (progn + (ros::ros-error "Setting PD gains of ~a via ~a failed" joint-name type) + (ros::ros-error + "~a may not include a controller coming from effort_controllers/JointTrajectoryController and including ~a" + type joint-name))) )) (:set-all-joint-pd-gain (pgain dgain &key (type :default-controller)) "Set P gain and D gain of all joints. -This method fully works only when `type` comes from effort_controllers/JointTrajectoryController and includes all joints. +This method fully works only when each joint is included by a controller in `type` coming from effort_controllers/JointTrajectoryController. " (dolist (j (send robot :joint-list)) (send self :set-joint-pd-gain (send j :name) pgain dgain :type type)) From c6164d9da71e6f9b4a515440934bcc77461da34a Mon Sep 17 00:00:00 2001 From: Yuki Asano Date: Wed, 31 May 2023 09:53:49 +0900 Subject: [PATCH 16/31] enable access to gripper-grasp-action --- jsk_panda_robot/panda_eus/euslisp/panda-interface.l | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/jsk_panda_robot/panda_eus/euslisp/panda-interface.l b/jsk_panda_robot/panda_eus/euslisp/panda-interface.l index daf8ba1fd9..50c479a9e0 100644 --- a/jsk_panda_robot/panda_eus/euslisp/panda-interface.l +++ b/jsk_panda_robot/panda_eus/euslisp/panda-interface.l @@ -24,7 +24,11 @@ (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)))))) + (cons :joint-names (send-all (send robot :joint-list) :name))))) + (:gripper-grasp-action + () + (gethash :rarm gripper-grasp-actions) + ) (defun panda-init () (setq *ri* (instance panda-robot-interface :init)) From 6abfe116c622dc0773133cd4b5acfa6b0c840468 Mon Sep 17 00:00:00 2001 From: Yuki Asano Date: Wed, 31 May 2023 12:43:49 +0900 Subject: [PATCH 17/31] add missing bracket --- jsk_panda_robot/panda_eus/euslisp/panda-interface.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_panda_robot/panda_eus/euslisp/panda-interface.l b/jsk_panda_robot/panda_eus/euslisp/panda-interface.l index 50c479a9e0..d2597f32a9 100644 --- a/jsk_panda_robot/panda_eus/euslisp/panda-interface.l +++ b/jsk_panda_robot/panda_eus/euslisp/panda-interface.l @@ -27,7 +27,7 @@ (cons :joint-names (send-all (send robot :joint-list) :name))))) (:gripper-grasp-action () - (gethash :rarm gripper-grasp-actions) + (gethash :rarm gripper-grasp-actions)) ) (defun panda-init () From 3ff3fd75400afcbd01aa2063207c25bdf5dd4d2f Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Wed, 31 May 2023 19:46:53 +0900 Subject: [PATCH 18/31] Enable to get results of :stop-gripper/:homing-gripper/:start-grasp/:stop-grasp/:move-gripper from their return values --- .../euslisp/panda-common-interface.l | 30 ++++++++++++------- 1 file changed, 20 insertions(+), 10 deletions(-) diff --git a/jsk_panda_robot/panda_eus/euslisp/panda-common-interface.l b/jsk_panda_robot/panda_eus/euslisp/panda-common-interface.l index 44c3436138..eef643ffab 100644 --- a/jsk_panda_robot/panda_eus/euslisp/panda-common-interface.l +++ b/jsk_panda_robot/panda_eus/euslisp/panda-common-interface.l @@ -257,16 +257,30 @@ Details: `ErrorRecoveryAction` part of https://frankaemika.github.io/docs/franka (:arms all-arms) (t (list arm)) )) + (:send-gripper-action-method + (arm actions method &rest args) + (let ((action (gethash (send self :expand-arm-alias arm) actions))) + (if action (send* action method args) nil)) + ) + (:gripper-action-postprocess + (actions arm arms wait) + (if wait + (dolist (a arms) + (send self :send-gripper-action-method a actions :wait-for-result))) + (case arm + (:arms + (mapcar #'(lambda (a) + (send self :send-gripper-action-method a actions :get-result)) + arms)) + (t + (send self :send-gripper-action-method arm actions :get-result))) + ) (:gripper-method-helper (action-sender actions arm wait) (let ((arms (send self :arm2arms arm))) (dolist (a arms) (send self action-sender a :wait nil)) - (if wait - (mapcar #'(lambda (a) - (let ((action (gethash (send self :expand-arm-alias a) actions))) - (if action (send action :wait-for-result) nil))) - arms)) + (send self :gripper-action-postprocess actions arm arms wait) )) (:stop-gripper (arm &key (wait nil)) @@ -328,11 +342,7 @@ Example: (send self :gripper :rarm :position) => 0.00 (/ (abs (- (send self :gripper a :position) width)) (/ tm 1000.0)) (append args (list :wait nil)))) - (if wait - (mapcar #'(lambda (a) - (let ((action (gethash (send self :expand-arm-alias a) actions))) - (if action (send action :wait-for-result) nil))) - arms)) + (send self :gripper-action-postprocess actions arm arms wait) )) (:start-grasp (arm &key (width 0.0) (effort 80.0) (tm 500) (wait nil) (inner 0.005) (outer 0.06)) From 39c9c2e1c8f9d926945ebd0532316a4ebf5fc9e2 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Wed, 31 May 2023 20:23:41 +0900 Subject: [PATCH 19/31] Enable to get results of :stop-gripper/:homing-gripper/:start-grasp/:stop-grasp/:move-gripper from separated methods --- .../euslisp/panda-common-interface.l | 54 ++++++++++++++----- 1 file changed, 40 insertions(+), 14 deletions(-) diff --git a/jsk_panda_robot/panda_eus/euslisp/panda-common-interface.l b/jsk_panda_robot/panda_eus/euslisp/panda-common-interface.l index eef643ffab..084d3a742f 100644 --- a/jsk_panda_robot/panda_eus/euslisp/panda-common-interface.l +++ b/jsk_panda_robot/panda_eus/euslisp/panda-common-interface.l @@ -263,24 +263,25 @@ Details: `ErrorRecoveryAction` part of https://frankaemika.github.io/docs/franka (if action (send* action method args) nil)) ) (:gripper-action-postprocess - (actions arm arms wait) - (if wait - (dolist (a arms) - (send self :send-gripper-action-method a actions :wait-for-result))) - (case arm - (:arms - (mapcar #'(lambda (a) - (send self :send-gripper-action-method a actions :get-result)) - arms)) - (t - (send self :send-gripper-action-method arm actions :get-result))) - ) + (arm actions wait) + (let ((arms (send self :arm2arms arm))) + (if wait + (dolist (a arms) + (send self :send-gripper-action-method a actions :wait-for-result))) + (case arm + (:arms + (mapcar #'(lambda (a) + (send self :send-gripper-action-method a actions :get-result)) + arms)) + (t + (send self :send-gripper-action-method arm actions :get-result))) + )) (:gripper-method-helper (action-sender actions arm wait) (let ((arms (send self :arm2arms arm))) (dolist (a arms) (send self action-sender a :wait nil)) - (send self :gripper-action-postprocess actions arm arms wait) + (send self :gripper-action-postprocess arm actions wait) )) (:stop-gripper (arm &key (wait nil)) @@ -294,6 +295,11 @@ Details: `StopAction` part of https://frankaemika.github.io/docs/franka_ros.html (send self :gripper-method-helper :send-gripper-stop-action gripper-stop-actions arm wait) ) + (:get-stop-gripper-result + (arm) + "Return result of :stop-gripper (`franka_gripper::StopActionResult`)" + (send self :gripper-action-postprocess arm gripper-stop-actions t) + ) (:homing-gripper (arm &key (wait nil)) "Home the gripper and update the maximum width given the mounted fingers (i.e., calibrate & initialize the gripper). @@ -304,6 +310,11 @@ Details: `HomingAction` part of https://frankaemika.github.io/docs/franka_ros.ht (send self :gripper-method-helper :send-gripper-homing-action gripper-homing-actions arm wait) )) + (:get-homing-gripper-result + (arm) + "Return result of :homing-gripper (`franka_gripper::HomingActionResult`)" + (send self :gripper-action-postprocess arm gripper-homing-actions t) + ) (:gripper (&rest args) "Get information of gripper @@ -342,7 +353,7 @@ Example: (send self :gripper :rarm :position) => 0.00 (/ (abs (- (send self :gripper a :position) width)) (/ tm 1000.0)) (append args (list :wait nil)))) - (send self :gripper-action-postprocess actions arm arms wait) + (send self :gripper-action-postprocess arm actions wait) )) (:start-grasp (arm &key (width 0.0) (effort 80.0) (tm 500) (wait nil) (inner 0.005) (outer 0.06)) @@ -364,11 +375,21 @@ Details: `GraspAction` part of https://frankaemika.github.io/docs/franka_ros.htm :send-gripper-grasp-action gripper-grasp-actions arm wait width tm effort :inner inner :outer outer) )) + (:get-start-grasp-result + (arm) + "Return result of :start-grasp (`franka_gripper::GraspActionResult`)" + (send self :gripper-action-postprocess arm gripper-grasp-actions t) + ) (:stop-grasp (arm &key (wait nil) (width 0.08)) "Open the gripper to the target `width` [m]" (send self :move-gripper arm width :tm 500 :wait wait) ) + (:get-stop-grasp-result + (arm) + "Return result of :stop-gripper (`franka_gripper::MoveActionResult`)" + (send self :get-move-gripper-result arm) + ) (:move-gripper (arm width &key (tm 500) (wait nil)) "Move the gripper to the target `width` [m] while closing with the desired speed calculated from `tm` [ms]. @@ -379,6 +400,11 @@ Details: `MoveAction` part of https://frankaemika.github.io/docs/franka_ros.html (send self :gripper-method-with-width-helper :send-gripper-move-action gripper-move-actions arm wait width tm) )) + (:get-move-gripper-result + (arm) + "Return result of :move-gripper (`franka_gripper::MoveActionResult`)" + (send self :gripper-action-postprocess arm gripper-move-actions t) + ) ) (provide :panda-common-interface) From e6777316790857813fd6eb3d7cbde8400e0fb1c8 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Wed, 31 May 2023 20:36:38 +0900 Subject: [PATCH 20/31] Consider compatibility with FR3 (Franka Research 3) --- .../jsk_panda_startup/launch/franka.launch | 19 +++++++++++++++++++ .../jsk_panda_startup/launch/panda.launch | 4 +--- .../panda_eus/euslisp/dual_panda-interface.l | 4 ++-- ...-interface.l => franka-common-interface.l} | 10 +++++----- .../panda_eus/euslisp/panda-interface.l | 4 ++-- 5 files changed, 29 insertions(+), 12 deletions(-) create mode 100644 jsk_panda_robot/jsk_panda_startup/launch/franka.launch rename jsk_panda_robot/panda_eus/euslisp/{panda-common-interface.l => franka-common-interface.l} (98%) diff --git a/jsk_panda_robot/jsk_panda_startup/launch/franka.launch b/jsk_panda_robot/jsk_panda_startup/launch/franka.launch new file mode 100644 index 0000000000..689d57318f --- /dev/null +++ b/jsk_panda_robot/jsk_panda_startup/launch/franka.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/jsk_panda_robot/jsk_panda_startup/launch/panda.launch b/jsk_panda_robot/jsk_panda_startup/launch/panda.launch index 689d57318f..be2638609b 100644 --- a/jsk_panda_robot/jsk_panda_startup/launch/panda.launch +++ b/jsk_panda_robot/jsk_panda_startup/launch/panda.launch @@ -7,13 +7,11 @@ - + - - diff --git a/jsk_panda_robot/panda_eus/euslisp/dual_panda-interface.l b/jsk_panda_robot/panda_eus/euslisp/dual_panda-interface.l index 4f90c9b0e9..338e387357 100644 --- a/jsk_panda_robot/panda_eus/euslisp/dual_panda-interface.l +++ b/jsk_panda_robot/panda_eus/euslisp/dual_panda-interface.l @@ -1,8 +1,8 @@ -(require :panda-common-interface "package://panda_eus/euslisp/panda-common-interface.l") +(require :franka-common-interface "package://panda_eus/euslisp/franka-common-interface.l") (require :dual_panda-utils "package://panda_eus/euslisp/dual_panda-utils.l") (defclass dual_panda-robot-interface - :super panda-common-interface + :super franka-common-interface :slots ()) (defmethod dual_panda-robot-interface diff --git a/jsk_panda_robot/panda_eus/euslisp/panda-common-interface.l b/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l similarity index 98% rename from jsk_panda_robot/panda_eus/euslisp/panda-common-interface.l rename to jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l index 084d3a742f..818cbb3fae 100644 --- a/jsk_panda_robot/panda_eus/euslisp/panda-common-interface.l +++ b/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l @@ -3,14 +3,14 @@ (ros::roseus-add-msgs "franka_msgs") (ros::roseus-add-msgs "franka_gripper") -(defclass panda-common-interface +(defclass franka-common-interface :super robot-interface :slots (all-arms all-arm-aliases arm-error-p error-recovery-act gripper-grasp-actions gripper-move-actions gripper-homing-actions gripper-stop-actions ) ) -(defmethod panda-common-interface +(defmethod franka-common-interface (:init (&rest args &key (robot nil) (joint-states-topic nil) @@ -21,8 +21,8 @@ (error-recovery-action nil) (gripper-action-prefixes nil) &allow-other-keys) - "Create robot interface for panda -Arguments specific to panda: + "Create robot interface for franka robot +Arguments specific to franka robot: - all-arms : names of all arms (e.g., (list :rarm)) - all-arm-aliases : aliases of all arms (e.g., (list :arm)) - error-topics : topics used for getting error states of arms (e.g., (list \"/franka_state_controller/franka_states\")) @@ -407,7 +407,7 @@ Details: `MoveAction` part of https://frankaemika.github.io/docs/franka_ros.html ) ) -(provide :panda-common-interface) +(provide :franka-common-interface) #| You can check current joint gains with rqt_reconfigure when you use effort_controllers/JointTrajectoryController. diff --git a/jsk_panda_robot/panda_eus/euslisp/panda-interface.l b/jsk_panda_robot/panda_eus/euslisp/panda-interface.l index d2597f32a9..401d374580 100644 --- a/jsk_panda_robot/panda_eus/euslisp/panda-interface.l +++ b/jsk_panda_robot/panda_eus/euslisp/panda-interface.l @@ -1,8 +1,8 @@ -(require :panda-common-interface "package://panda_eus/euslisp/panda-common-interface.l") +(require :franka-common-interface "package://panda_eus/euslisp/franka-common-interface.l") (require :panda-utils "package://panda_eus/euslisp/panda-utils.l") (defclass panda-robot-interface - :super panda-common-interface + :super franka-common-interface :slots ()) (defmethod panda-robot-interface From 499d90dd18a41b3fa6ffc0e31cd31f513569a97f Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Wed, 31 May 2023 20:40:25 +0900 Subject: [PATCH 21/31] Remove unnecessary method --- jsk_panda_robot/panda_eus/euslisp/panda-interface.l | 3 --- 1 file changed, 3 deletions(-) diff --git a/jsk_panda_robot/panda_eus/euslisp/panda-interface.l b/jsk_panda_robot/panda_eus/euslisp/panda-interface.l index 401d374580..78cb96a842 100644 --- a/jsk_panda_robot/panda_eus/euslisp/panda-interface.l +++ b/jsk_panda_robot/panda_eus/euslisp/panda-interface.l @@ -25,9 +25,6 @@ (cons :controller-state "/position_joint_trajectory_controller/state") (cons :action-type control_msgs::FollowJointTrajectoryAction) (cons :joint-names (send-all (send robot :joint-list) :name))))) - (:gripper-grasp-action - () - (gethash :rarm gripper-grasp-actions)) ) (defun panda-init () From 0066eb7ea240d2471c1f17d89b9bb000225caed9 Mon Sep 17 00:00:00 2001 From: Yuki Asano Date: Thu, 1 Jun 2023 11:44:25 +0900 Subject: [PATCH 22/31] extract action from hash --- jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l b/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l index 818cbb3fae..702f74fe6c 100644 --- a/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l +++ b/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l @@ -263,8 +263,9 @@ Details: `ErrorRecoveryAction` part of https://frankaemika.github.io/docs/franka (if action (send* action method args) nil)) ) (:gripper-action-postprocess - (arm actions wait) - (let ((arms (send self :arm2arms arm))) + (arm actions-hash wait) + (let ((arms (send self :arm2arms arm)) + (actions (gethash arm actions-hash))) (if wait (dolist (a arms) (send self :send-gripper-action-method a actions :wait-for-result))) From fc875a0fe6409c52ff670e55dfb5199c8cc63abe Mon Sep 17 00:00:00 2001 From: Yuki Asano Date: Thu, 1 Jun 2023 14:04:05 +0900 Subject: [PATCH 23/31] Revert "extract action from hash" This reverts commit 0066eb7ea240d2471c1f17d89b9bb000225caed9. --- jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l b/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l index 702f74fe6c..818cbb3fae 100644 --- a/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l +++ b/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l @@ -263,9 +263,8 @@ Details: `ErrorRecoveryAction` part of https://frankaemika.github.io/docs/franka (if action (send* action method args) nil)) ) (:gripper-action-postprocess - (arm actions-hash wait) - (let ((arms (send self :arm2arms arm)) - (actions (gethash arm actions-hash))) + (arm actions wait) + (let ((arms (send self :arm2arms arm))) (if wait (dolist (a arms) (send self :send-gripper-action-method a actions :wait-for-result))) From f58e3015cc73c4fcb5756c56ad8f1daba7369cac Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Thu, 1 Jun 2023 16:20:06 +0900 Subject: [PATCH 24/31] Make errors from :get-*-result readable when * is not called --- .../euslisp/franka-common-interface.l | 28 +++++++++++++++---- jsk_panda_robot/panda_eus/package.xml | 1 + 2 files changed, 23 insertions(+), 6 deletions(-) diff --git a/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l b/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l index 818cbb3fae..1e8ff62c4a 100644 --- a/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l +++ b/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l @@ -1,5 +1,6 @@ (require :robot-interface "package://pr2eus/robot-interface.l") +(ros::roseus-add-msgs "actionlib_msgs") (ros::roseus-add-msgs "franka_msgs") (ros::roseus-add-msgs "franka_gripper") @@ -86,19 +87,23 @@ Arguments specific to franka robot: (sethash arm gripper-grasp-actions (instance ros::simple-action-client :init (format nil "~a/franka_gripper/grasp" prefix) - franka_gripper::GraspAction)) + franka_gripper::GraspAction + :groupname groupname)) (sethash arm gripper-homing-actions (instance ros::simple-action-client :init (format nil "~a/franka_gripper/homing" prefix) - franka_gripper::HomingAction)) + franka_gripper::HomingAction + :groupname groupname)) (sethash arm gripper-move-actions (instance ros::simple-action-client :init (format nil "~a/franka_gripper/move" prefix) - franka_gripper::MoveAction)) + franka_gripper::MoveAction + :groupname groupname)) (sethash arm gripper-stop-actions (instance ros::simple-action-client :init (format nil "~a/franka_gripper/stop"prefix) - franka_gripper::StopAction)))) + franka_gripper::StopAction + :groupname groupname)))) )) (:set-joint-pd-gain (joint-name pgain dgain &key (type :default-controller)) @@ -264,10 +269,21 @@ Details: `ErrorRecoveryAction` part of https://frankaemika.github.io/docs/franka ) (:gripper-action-postprocess (arm actions wait) - (let ((arms (send self :arm2arms arm))) + (let ((arms (send self :arm2arms arm)) (wait-res t)) (if wait (dolist (a arms) - (send self :send-gripper-action-method a actions :wait-for-result))) + (setq wait-res + (and wait-res + (send self :send-gripper-action-method a actions :wait-for-result))))) + (if (not wait-res) + (dolist (a arms) + (if (eq (send self :send-gripper-action-method a actions :get-state) + actionlib_msgs::GoalStatus::*pending*) + (progn + (ros::ros-error + "No goal exists for ~a, send goal to it first (:start-grasp, :stop-grasp, ...)" + (send self :send-gripper-action-method a actions :name)) + (return-from :gripper-action-postprocess nil))))) (case arm (:arms (mapcar #'(lambda (a) diff --git a/jsk_panda_robot/panda_eus/package.xml b/jsk_panda_robot/panda_eus/package.xml index 8bc83b05dd..0867097679 100644 --- a/jsk_panda_robot/panda_eus/package.xml +++ b/jsk_panda_robot/panda_eus/package.xml @@ -22,5 +22,6 @@ franka_gripper franka_control pr2eus + actionlib_msgs From aacbf62a385afabde8612de79ceff12cd33376d9 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Thu, 1 Jun 2023 16:52:07 +0900 Subject: [PATCH 25/31] Force to call all :wait-for-result and postprocess even if some of them fail --- .../euslisp/franka-common-interface.l | 24 +++++++++++-------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l b/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l index 1e8ff62c4a..69f90bf8d9 100644 --- a/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l +++ b/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l @@ -273,17 +273,21 @@ Details: `ErrorRecoveryAction` part of https://frankaemika.github.io/docs/franka (if wait (dolist (a arms) (setq wait-res - (and wait-res - (send self :send-gripper-action-method a actions :wait-for-result))))) + (and (send self :send-gripper-action-method a actions :wait-for-result) + wait-res)))) (if (not wait-res) - (dolist (a arms) - (if (eq (send self :send-gripper-action-method a actions :get-state) - actionlib_msgs::GoalStatus::*pending*) - (progn - (ros::ros-error - "No goal exists for ~a, send goal to it first (:start-grasp, :stop-grasp, ...)" - (send self :send-gripper-action-method a actions :name)) - (return-from :gripper-action-postprocess nil))))) + (progn + (setq wait-res t) + (dolist (a arms) + (if (eq (send self :send-gripper-action-method a actions :get-state) + actionlib_msgs::GoalStatus::*pending*) + (progn + (ros::ros-error + "No goal exists for ~a, send goal to it first (:start-grasp, :stop-grasp, ...)" + (send self :send-gripper-action-method a actions :name)) + (setq wait-res nil)))) + (if (not wait-res) + (return-from :gripper-action-postprocess nil)))) (case arm (:arms (mapcar #'(lambda (a) From 408bce5bf08d2a1677d9221b0c81138835a76101 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Mon, 31 Jul 2023 19:46:32 +0900 Subject: [PATCH 26/31] [panda_eus] Change dual_panda's end-coords to the same as single panda --- jsk_panda_robot/README.md | 2 +- jsk_panda_robot/panda_eus/models/dual_panda.urdf.xacro | 8 ++++---- jsk_panda_robot/panda_eus/models/dual_panda.yaml | 8 ++++---- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/jsk_panda_robot/README.md b/jsk_panda_robot/README.md index eb353bde2b..d035983bce 100644 --- a/jsk_panda_robot/README.md +++ b/jsk_panda_robot/README.md @@ -107,7 +107,7 @@ ``` - Notice - `(send *ri* :recover-error)` is required every time when you press and release the black switch (`activated` -> `monitored stop` -> `activated`). - - `dual_panda`'s `end-coords`, `reset-pose`, and `reset-manip-pose` are different from single `panda`'s ones for historical reasons. + - `dual_panda`'s `reset-pose` and `reset-manip-pose` are different from single `panda`'s ones for historical reasons. #### Record/play rosbag ```bash roslaunch jsk_panda_startup dual_panda1_record.launch # Or roslaunch jsk_panda_startup dual_panda2_record.launch diff --git a/jsk_panda_robot/panda_eus/models/dual_panda.urdf.xacro b/jsk_panda_robot/panda_eus/models/dual_panda.urdf.xacro index d786546287..12165163c6 100644 --- a/jsk_panda_robot/panda_eus/models/dual_panda.urdf.xacro +++ b/jsk_panda_robot/panda_eus/models/dual_panda.urdf.xacro @@ -109,17 +109,17 @@ - + - + - + - + diff --git a/jsk_panda_robot/panda_eus/models/dual_panda.yaml b/jsk_panda_robot/panda_eus/models/dual_panda.yaml index 338af0958f..2190c285f4 100644 --- a/jsk_panda_robot/panda_eus/models/dual_panda.yaml +++ b/jsk_panda_robot/panda_eus/models/dual_panda.yaml @@ -20,12 +20,12 @@ larm: # - pan_tilt_JOINT1 : head-neck-p rarm-end-coords: - parent: rarm_link7 - translate: [0, 0, 0.2] + parent: rarm_link8 + translate: [0, 0, 0.1] rotate : [-0.35740972270209, -0.86285515644477, -0.35740630816298, 98.42112728719206] larm-end-coords: - parent: larm_link7 - translate: [0, 0, 0.2] + parent: larm_link8 + translate: [0, 0, 0.1] rotate : [-0.35740972270209, -0.86285515644477, -0.35740630816298, 98.42112728719206] angle-vector: From 5f3c67336fa19249e5a5320fc66572ac73b6599e Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Fri, 4 Aug 2023 13:11:04 +0900 Subject: [PATCH 27/31] [panda_eus] Move end-coords of single panda and dual_panda to the center of the fingers --- .../panda_eus/models/dual_panda.urdf.xacro | 8 ++++---- jsk_panda_robot/panda_eus/models/dual_panda.yaml | 12 ++++++------ jsk_panda_robot/panda_eus/models/panda.yaml | 6 +++--- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/jsk_panda_robot/panda_eus/models/dual_panda.urdf.xacro b/jsk_panda_robot/panda_eus/models/dual_panda.urdf.xacro index 12165163c6..f47eb6c525 100644 --- a/jsk_panda_robot/panda_eus/models/dual_panda.urdf.xacro +++ b/jsk_panda_robot/panda_eus/models/dual_panda.urdf.xacro @@ -109,17 +109,17 @@ - + - + - + - + diff --git a/jsk_panda_robot/panda_eus/models/dual_panda.yaml b/jsk_panda_robot/panda_eus/models/dual_panda.yaml index 2190c285f4..351154873a 100644 --- a/jsk_panda_robot/panda_eus/models/dual_panda.yaml +++ b/jsk_panda_robot/panda_eus/models/dual_panda.yaml @@ -20,13 +20,13 @@ larm: # - pan_tilt_JOINT1 : head-neck-p rarm-end-coords: - parent: rarm_link8 - translate: [0, 0, 0.1] - rotate : [-0.35740972270209, -0.86285515644477, -0.35740630816298, 98.42112728719206] + parent: rarm_hand_tcp + translate: [0, 0, 0] + rotate : [0, -1, 0, 90] larm-end-coords: - parent: larm_link8 - translate: [0, 0, 0.1] - rotate : [-0.35740972270209, -0.86285515644477, -0.35740630816298, 98.42112728719206] + parent: larm_hand_tcp + translate: [0, 0, 0] + rotate : [0, -1, 0, 90] angle-vector: reset-pose: [ 0.0, 15.0, 0.0, -135.0, 0.0, 150.0, 45.0, diff --git a/jsk_panda_robot/panda_eus/models/panda.yaml b/jsk_panda_robot/panda_eus/models/panda.yaml index 03d3457795..08b55ee3f2 100644 --- a/jsk_panda_robot/panda_eus/models/panda.yaml +++ b/jsk_panda_robot/panda_eus/models/panda.yaml @@ -8,9 +8,9 @@ rarm: - panda_joint7 : rarm-wrist-y rarm-end-coords: - parent: panda_link8 - translate: [0, 0, 0.1] - rotate : [-0.35740972270209, -0.86285515644477, -0.35740630816298, 98.42112728719206] + parent: panda_hand_tcp + translate: [0, 0, 0] + rotate : [0, -1, 0, 90] angle-vector: reset-pose: [ 0.0, -45.0, 0.0, -135.0, 0.0, 90.0, 45.0, From 0a345011835926528255ce644997f6b627e924d3 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Mon, 16 Oct 2023 15:23:57 +0900 Subject: [PATCH 28/31] [panda_eus] Make :gripper method work --- jsk_panda_robot/panda_eus/models/dual_panda.yaml | 8 ++++---- jsk_panda_robot/panda_eus/models/panda.yaml | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/jsk_panda_robot/panda_eus/models/dual_panda.yaml b/jsk_panda_robot/panda_eus/models/dual_panda.yaml index 351154873a..14c48cfbfe 100644 --- a/jsk_panda_robot/panda_eus/models/dual_panda.yaml +++ b/jsk_panda_robot/panda_eus/models/dual_panda.yaml @@ -20,12 +20,12 @@ larm: # - pan_tilt_JOINT1 : head-neck-p rarm-end-coords: - parent: rarm_hand_tcp - translate: [0, 0, 0] + parent: rarm_hand # If rarm_hand_tcp is used to delete the following translation, (send *dual_panda* :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] larm-end-coords: - parent: larm_hand_tcp - translate: [0, 0, 0] + parent: larm_hand # If larm_hand_tcp is used to delete the following translation, (send *dual_panda* :larm :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: diff --git a/jsk_panda_robot/panda_eus/models/panda.yaml b/jsk_panda_robot/panda_eus/models/panda.yaml index 08b55ee3f2..73b9e3f980 100644 --- a/jsk_panda_robot/panda_eus/models/panda.yaml +++ b/jsk_panda_robot/panda_eus/models/panda.yaml @@ -8,8 +8,8 @@ rarm: - panda_joint7 : rarm-wrist-y rarm-end-coords: - parent: panda_hand_tcp - translate: [0, 0, 0] + parent: panda_hand # If panda_hand_tcp is used to delete the following translation, (send *panda* :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: From 72a2bc66edbb3c0a469d78d41fee5d1f66bb3497 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Mon, 16 Oct 2023 23:13:55 +0900 Subject: [PATCH 29/31] [panda_eus] Make URDF follow euslisp model --- jsk_panda_robot/panda_eus/models/dual_panda.urdf.xacro | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/jsk_panda_robot/panda_eus/models/dual_panda.urdf.xacro b/jsk_panda_robot/panda_eus/models/dual_panda.urdf.xacro index f47eb6c525..eac361f09f 100644 --- a/jsk_panda_robot/panda_eus/models/dual_panda.urdf.xacro +++ b/jsk_panda_robot/panda_eus/models/dual_panda.urdf.xacro @@ -109,17 +109,17 @@ - + - + - + - + From 141b6cf8478d30139b01162dbe8f3196c6006b15 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Tue, 17 Oct 2023 18:28:44 +0900 Subject: [PATCH 30/31] [panda_eus] Enable to get gripper method result without waiting --- .../euslisp/franka-common-interface.l | 23 ++++++++++--------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l b/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l index 69f90bf8d9..7ffb3a675c 100644 --- a/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l +++ b/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l @@ -274,7 +274,8 @@ Details: `ErrorRecoveryAction` part of https://frankaemika.github.io/docs/franka (dolist (a arms) (setq wait-res (and (send self :send-gripper-action-method a actions :wait-for-result) - wait-res)))) + wait-res))) + (send self :spin-once)) (if (not wait-res) (progn (setq wait-res t) @@ -316,9 +317,9 @@ Details: `StopAction` part of https://frankaemika.github.io/docs/franka_ros.html :send-gripper-stop-action gripper-stop-actions arm wait) ) (:get-stop-gripper-result - (arm) + (arm &key (wait t)) "Return result of :stop-gripper (`franka_gripper::StopActionResult`)" - (send self :gripper-action-postprocess arm gripper-stop-actions t) + (send self :gripper-action-postprocess arm gripper-stop-actions wait) ) (:homing-gripper (arm &key (wait nil)) @@ -331,9 +332,9 @@ Details: `HomingAction` part of https://frankaemika.github.io/docs/franka_ros.ht :send-gripper-homing-action gripper-homing-actions arm wait) )) (:get-homing-gripper-result - (arm) + (arm &key (wait t)) "Return result of :homing-gripper (`franka_gripper::HomingActionResult`)" - (send self :gripper-action-postprocess arm gripper-homing-actions t) + (send self :gripper-action-postprocess arm gripper-homing-actions wait) ) (:gripper (&rest args) @@ -396,9 +397,9 @@ Details: `GraspAction` part of https://frankaemika.github.io/docs/franka_ros.htm effort :inner inner :outer outer) )) (:get-start-grasp-result - (arm) + (arm &key (wait t)) "Return result of :start-grasp (`franka_gripper::GraspActionResult`)" - (send self :gripper-action-postprocess arm gripper-grasp-actions t) + (send self :gripper-action-postprocess arm gripper-grasp-actions wait) ) (:stop-grasp (arm &key (wait nil) (width 0.08)) @@ -406,9 +407,9 @@ Details: `GraspAction` part of https://frankaemika.github.io/docs/franka_ros.htm (send self :move-gripper arm width :tm 500 :wait wait) ) (:get-stop-grasp-result - (arm) + (arm &key (wait t)) "Return result of :stop-gripper (`franka_gripper::MoveActionResult`)" - (send self :get-move-gripper-result arm) + (send self :get-move-gripper-result arm :wait wait) ) (:move-gripper (arm width &key (tm 500) (wait nil)) @@ -421,9 +422,9 @@ Details: `MoveAction` part of https://frankaemika.github.io/docs/franka_ros.html :send-gripper-move-action gripper-move-actions arm wait width tm) )) (:get-move-gripper-result - (arm) + (arm &key (wait t)) "Return result of :move-gripper (`franka_gripper::MoveActionResult`)" - (send self :gripper-action-postprocess arm gripper-move-actions t) + (send self :gripper-action-postprocess arm gripper-move-actions wait) ) ) From f8d3cc4106d872ef3e9d7b32250c7e407d6f7a41 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Tue, 17 Oct 2023 19:10:56 +0900 Subject: [PATCH 31/31] [panda_eus] Enable to get status of gripper actions --- .../euslisp/franka-common-interface.l | 49 +++++++++++++++---- 1 file changed, 39 insertions(+), 10 deletions(-) diff --git a/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l b/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l index 7ffb3a675c..f7251cf696 100644 --- a/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l +++ b/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l @@ -264,8 +264,14 @@ Details: `ErrorRecoveryAction` part of https://frankaemika.github.io/docs/franka )) (:send-gripper-action-method (arm actions method &rest args) - (let ((action (gethash (send self :expand-arm-alias arm) actions))) - (if action (send* action method args) nil)) + (case arm + (:arms + (mapcar #'(lambda (a) + (send self :send-gripper-action-method a actions method)) + (send self :arm2arms arm))) + (t + (let ((action (gethash (send self :expand-arm-alias arm) actions))) + (if action (send* action method args) nil)))) ) (:gripper-action-postprocess (arm actions wait) @@ -289,13 +295,7 @@ Details: `ErrorRecoveryAction` part of https://frankaemika.github.io/docs/franka (setq wait-res nil)))) (if (not wait-res) (return-from :gripper-action-postprocess nil)))) - (case arm - (:arms - (mapcar #'(lambda (a) - (send self :send-gripper-action-method a actions :get-result)) - arms)) - (t - (send self :send-gripper-action-method arm actions :get-result))) + (send self :send-gripper-action-method arm actions :get-result) )) (:gripper-method-helper (action-sender actions arm wait) @@ -316,6 +316,12 @@ Details: `StopAction` part of https://frankaemika.github.io/docs/franka_ros.html (send self :gripper-method-helper :send-gripper-stop-action gripper-stop-actions arm wait) ) + (:get-stop-gripper-status + (arm) + "Return status of :stop-gripper (`status` of `actionlib_msgs::GoalStatus`)" + (send self :spin-once) + (send self :send-gripper-action-method arm gripper-stop-actions :get-state) + ) (:get-stop-gripper-result (arm &key (wait t)) "Return result of :stop-gripper (`franka_gripper::StopActionResult`)" @@ -331,6 +337,12 @@ Details: `HomingAction` part of https://frankaemika.github.io/docs/franka_ros.ht (send self :gripper-method-helper :send-gripper-homing-action gripper-homing-actions arm wait) )) + (:get-homing-gripper-status + (arm) + "Return status of :homing-gripper (`status` of `actionlib_msgs::GoalStatus`)" + (send self :spin-once) + (send self :send-gripper-action-method arm gripper-homing-actions :get-state) + ) (:get-homing-gripper-result (arm &key (wait t)) "Return result of :homing-gripper (`franka_gripper::HomingActionResult`)" @@ -396,6 +408,12 @@ Details: `GraspAction` part of https://frankaemika.github.io/docs/franka_ros.htm :send-gripper-grasp-action gripper-grasp-actions arm wait width tm effort :inner inner :outer outer) )) + (:get-start-grasp-status + (arm) + "Return status of :start-grasp (`status` of `actionlib_msgs::GoalStatus`)" + (send self :spin-once) + (send self :send-gripper-action-method arm gripper-grasp-actions :get-state) + ) (:get-start-grasp-result (arm &key (wait t)) "Return result of :start-grasp (`franka_gripper::GraspActionResult`)" @@ -406,9 +424,14 @@ Details: `GraspAction` part of https://frankaemika.github.io/docs/franka_ros.htm "Open the gripper to the target `width` [m]" (send self :move-gripper arm width :tm 500 :wait wait) ) + (:get-stop-grasp-status + (arm) + "Return status of :stop-grasp (`status` of `actionlib_msgs::GoalStatus`)" + (send self :get-move-gripper-status arm) + ) (:get-stop-grasp-result (arm &key (wait t)) - "Return result of :stop-gripper (`franka_gripper::MoveActionResult`)" + "Return result of :stop-grasp (`franka_gripper::MoveActionResult`)" (send self :get-move-gripper-result arm :wait wait) ) (:move-gripper @@ -421,6 +444,12 @@ Details: `MoveAction` part of https://frankaemika.github.io/docs/franka_ros.html (send self :gripper-method-with-width-helper :send-gripper-move-action gripper-move-actions arm wait width tm) )) + (:get-move-gripper-status + (arm) + "Return status of :move-gripper (`status` of `actionlib_msgs::GoalStatus`)" + (send self :spin-once) + (send self :send-gripper-action-method arm gripper-move-actions :get-state) + ) (:get-move-gripper-result (arm &key (wait t)) "Return result of :move-gripper (`franka_gripper::MoveActionResult`)"