add single panda

jsk_panda_robot/jsk_panda_startup/launch/panda_control.launch

@@ -0,0 +1,32 @@
+<?xml version="1.0" ?>
+
+<!-- this file is modified version of franka_control.launch -->
+<!-- https://github.com/frankaemika/franka_ros/blob/30e598aa6fb703cc80a203481e6427f397337b4c/franka_control/launch/franka_control.launch -->
+
+<launch>
+  <arg name="robot_ip" default="192.168.0.2"/>  <!-- add ip -->
+  <arg name="robot" default="panda" doc="choose your robot. Possible values: [panda, fr3]"/>
+  <arg name="arm_id" default="$(arg robot)" />
+  <arg name="load_gripper" default="true" />
+  <arg name="xacro_args" default="" />
+
+  <param name="robot_description" command="$(find xacro)/xacro $(find panda_eus)/models/panda.urdf.xacro hand:=$(arg load_gripper) arm_id:=$(arg arm_id) $(arg xacro_args)"/>  <!-- change xacro -->
+
+  <include file="$(find franka_gripper)/launch/franka_gripper.launch" if="$(arg load_gripper)">
+    <arg name="robot_ip" value="$(arg robot_ip)" />
+    <arg name="arm_id"   value="$(arg arm_id)" />
+  </include>
+
+  <node name="franka_control" pkg="franka_control" type="franka_control_node" output="screen" required="true">
+    <rosparam command="load" file="$(find franka_control)/config/franka_control_node.yaml" subst_value="true" />
+    <param name="robot_ip" value="$(arg robot_ip)" />
+  </node>
+  <rosparam command="load" file="$(find franka_control)/config/default_controllers.yaml" subst_value="true" />
+  <node name="state_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="franka_state_controller position_joint_trajectory_controller" />  <!-- add controller -->
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen"/>
+  <node name="joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher" output="screen">
+    <rosparam if="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states, franka_gripper/joint_states] </rosparam>
+    <rosparam unless="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states] </rosparam>
+    <param name="rate" value="30"/>
+  </node>
+</launch>

jsk_panda_robot/panda_eus/CMakeLists.txt

@@ -34,14 +34,30 @@ 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}")
   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 ##
 #############

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
+|#

jsk_panda_robot/panda_eus/models/panda.urdf.xacro

@@ -0,0 +1,5 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda" >
+  <xacro:arg name="hand" default="true" />
+  <xacro:include filename="$(find franka_description)/robots/panda/panda.urdf.xacro" />
+</robot>

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 ]