Update the ros related function for gripper joint (e.g., actionlib, s…

…ubscriber, rosservice).

Check with both roseus keinamtic simulator and real robot.

dynamixel_7dof_arm/euslisp/dxl-7dof-arm-interface-common.l

@@ -14,13 +14,17 @@
   (:initialize-arm-robot-ros
    ()
    ;; subscriber
-   (dotimes (i (length (send robot :angle-vector)))
+
+   (dotimes (i (+ (length (send robot :angle-vector)) 1))
      (ros::subscribe
-      (format nil "/arm_j~d_controller/state" (1+ i))
+      (if (eq i 6)
+          (format nil "/gripper_joint_controller/state")
+        (format nil "/arm_j~d_controller/state" (1+ i)))
       dynamixel_msgs::JointState
       #'send self :dynamixel-motor-states-callback :groupname groupname))
+
    (unless (send self :simulation-modep)
-   ;; services
+   ;; services for arm joints
      (dotimes (i (length (send robot :angle-vector)))
        (ros::wait-for-service
         (format nil "/arm_j~d_controller/set_compliance_slope" (1+ i)))
@@ -29,11 +33,17 @@
        (ros::wait-for-service
         (format nil "/arm_j~d_controller/set_torque_limit" (1+ i)))
        )
+     ;; services for gripper joint
+     (ros::wait-for-service
+      (format nil "/gripper_joint_controller/set_compliance_slope"))
+     (ros::wait-for-service
+      (format nil "/gripper_joint_controller/torque_enable"))
+     (ros::wait-for-service
+      (format nil "/gripper_joint_controller/set_torque_limit"))
      )
-   ;; define actions
+   ;; arm controller action
    (dolist (l (list
                (cons :fullbody-controller "fullbody_controller/follow_joint_trajectory")
-               (cons :gripper-controller "gripper_controller/follow_joint_trajectory")
                ))
      (let ((type (car l))
            (name (cdr l))
@@ -48,7 +58,18 @@
                                control_msgs::followjointtrajectoryaction
                                :groupname groupname))))
        ))
+
+   ;; gripper controller action
+   (setq gripper-action (instance ros::simple-action-client :init
+                                    "/gripper_controller/follow_joint_trajectory"
+                                    control_msgs::FollowJointTrajectoryAction
+                                    :groupname groupname))
+   ;; wait for gripper-action server
+   (unless (and joint-action-enable (send gripper-action :wait-for-server 3))
+     (setq joint-action-enable nil)
+     (ros::ros-warn "~A is not respond, robot-interface is disabled" gripper-action))
    )
+
   ;; TODO
   ;;  This method is tempolary code.
   ;;  dynamixel_controller_manager should publish /dxl_7dof_arm/joint_states
@@ -58,11 +79,11 @@
    (dolist (key '(:position :velocity :effort :name))
      ;; neglect /joint_states from turtlebot
      (unless (and (cdr (assoc key robot-state))
-                  (= (length (send robot :angle-vector)) (length (cdr (assoc key robot-state)))))
+                  (= (1+ (length (send robot :angle-vector))) (length (cdr (assoc key robot-state)))))
        (send self :set-robot-state1 key
              (if (eq key :name)
-                 (make-list (length (send robot :angle-vector)))
-               (instantiate float-vector (length (send robot :angle-vector)))))))
+                 (make-list (1+ (length (send robot :angle-vector))))
+               (instantiate float-vector (1+ (length (send robot :angle-vector))))))))
    ;; update values
    (dolist (key '(:position :velocity :effort :name))
      (setf (elt (cdr (assoc key robot-state)) (1- (elt (send msg :motor_ids) 0)))
@@ -74,6 +95,7 @@
            )
      )
    )
+
   (:fullbody-controller
    ()
    (list
@@ -82,14 +104,11 @@
      (cons :controller-state "fullbody_controller/state")
      (cons :action-type control_msgs::FollowJointTrajectoryAction)
      (cons :joint-names (mapcar #'(lambda (n) (if (symbolp n) (symbol-name n) n))
-                                (send-all
-                                 (remove-if #'(lambda (x)
-                                                (member x (send robot :gripper :arm :joint-list)))
-                                            (send robot :joint-list))
-                                 :name)
+                                (send-all (send robot :joint-list) :name)
                                 )))
     )
    )
+  #|
   (:gripper-controller
    ()
    (list
@@ -102,6 +121,8 @@
                                 )))
     )
    )
+  |#
+
   (:default-controller
    ()
    (send self :fullbody-controller)
@@ -113,9 +134,11 @@
   ;;  http://www.besttechnology.co.jp/modules/knowledge/?Dynamixel%E3%82%B3%E3%83%B3%E3%83%88%E3%83%AD%E3%83%BC%E3%83%AB%E3%83%86%E3%83%BC%E3%83%96%E3%83%AB%28DX%2CRX%2CAX%E3%82%B7%E3%83%AA%E3%83%BC%E3%82%BA%E7%94%A8%29#m041ac16
   (:set-compliance-slope ;; for one joint
    (id slope)
-   "Set compliance slope for one joint. id should be 1-7. slope is 32 by default."
+   "Set compliance slope for one joint. id should be 1-6. slope is 32 by default."
    (ros::service-call
-    (format nil "/arm_j~d_controller/set_compliance_slope" id)
+    (if (eq id 7)
+        (format nil "/gripper_joint_controller/set_compliance_slope")
+      (format nil "/arm_j~d_controller/set_compliance_slope" id))
     (instance dynamixel_controllers::setcomplianceslopeRequest :init
               :slope (round slope)))
    )
@@ -129,14 +152,18 @@
    (id torque-limit)
    "Set torque limit for one joint. id should be 1-7. torque-limit should be within [0, 1]."
    (ros::service-call
-    (format nil "/arm_j~d_controller/set_torque_limit" id)
+    (if (eq id 7)
+        (format nil "/gripper_joint_controller/set_torque_limit" id)
+      (format nil "/arm_j~d_controller/set_torque_limit" id))
     (instance dynamixel_controllers::SetTorqueLimitRequest :init
               :torque_limit torque-limit)))
   (:torque-enable
    (id torque-enable)
    "Configure joint torque mode for one joint. id sohuld be 1-7. If torque-enable is t, move to torque control mode, otherwise, move to joint positoin mode."
    (ros::service-call
-    (format nil "/arm_j~d_controller/torque_enable" id)
+    (if (eq id 7)
+        (format nil "/gripper_joint_controller/torque_enable" id)
+      (format nil "/arm_j~d_controller/torque_enable" id))
     (instance dynamixel_controllers::TorqueEnableRequest :init
               :torque_enable torque-enable)))
 
@@ -152,12 +179,12 @@
   (:servo-on-all
    ()
    "Servo On for all joints."
-   (dotimes (i (length (send robot :angle-vector)))
+   (dotimes (i (+ (length (send robot :angle-vector)) 1))
      (send self :servo-on-off (1+ i) t)))
   (:servo-off-all
    ()
    "Servo Off for all joints."
-   (dotimes (i (length (send robot :angle-vector)))
+   (dotimes (i (+ (length (send robot :angle-vector)) 1))
      (send self :servo-on-off (1+ i) nil)))
   (:servo-on-off
    (id on/off) ;; id = 1-7, t -> "On", nil -> "Off"
@@ -166,32 +193,67 @@
    (if on/off ;; just for servo off->on
        (send self :set-torque-limit id 1.0)))
 
+  (:move-gripper
+   (pos &key (tm 1000)  (wait t))
+   (unless joint-action-enable
+     (send (send robot :gripper_joint) :joint-angle pos )
+     (send self :publish-joint-state)
+     (if viewer (send self :draw-objects))
+     (return-from :move-gripper nil))
+   ;; for real robot
+   (let* ((goal (send gripper-action :make-goal-instance))
+          (goal-points nil)
+          (joint-names (list (send (send robot :gripper_joint) :name))))
+     (send goal :header :seq 1)
+     (send goal :header :stamp (ros::time-now))
+
+     (send goal :goal :trajectory :joint_names joint-names)
+     (send goal :goal :trajectory :header :stamp (ros::time-now))
+
+     (push (instance trajectory_msgs::JointTrajectoryPoint
+                     :init
+                     :positions (float-vector (deg2rad pos))
+                     :velocities (float-vector 0)
+                     :time_from_start (ros::time (/ tm 1000.0)))
+           goal-points)
+
+     (send self :spin-once)
+     (send goal :goal :trajectory :points goal-points)
+     (send gripper-action :send-goal goal)
+     )
+   #|
+   (send self :send-ros-controller
+         gripper-action (list (send (send robot :gripper_joint) :name)) ;; action server and joint-names
+         0  ;; start time
+         (list
+          (list (float-vector pos)                    ;; positions
+                (instantiate float-vector 1) ;; velocities
+                (/ tm 1000.0))))
+   |#
+   (if wait (send gripper-action :wait-for-result))
+   )
+
   ;; 把持モード開始メソッド
   (:start-grasp
-   (&optional (arm :arm) &key ((:gain g) 0.5) ((:objects objs) objects))
+   (&key ((:gain g) 0.5))
    "Start grasp mode."
-   (send self :set-compliance-slope 7 1023)
-   (send self :set-torque-limit 7 g)
-   (send robot :gripper arm :angle-vector
-         (send-all (send robot :gripper arm :joint-list) :min-angle))
-   (send self :angle-vector (send robot :angle-vector) 1000 :gripper-controller)
-   (send self :wait-interpolation :gripper-controller)
+   (unless (send self :simulation-modep)
+     (send self :set-compliance-slope 7 1023)
+     (send self :set-torque-limit 7 g))
+   (send self :move-gripper (send (send robot :gripper_joint) :min-angle) :tm 1000 :wait t)
    (send self :state)
-   (send robot :gripper arm :angle-vector
-         (mapcar #'(lambda (x) (- x 5)) (send-all (send robot :gripper arm :joint-list) :joint-angle))) ;; 5[deg]
-   (send self :angle-vector (send robot :angle-vector) 200 :gripper-controller)
-   (send self :wait-interpolation :gripper-controller)
+   (send robot :gripper :arm :joint-angle (- (send robot :gripper :arm :joint-angle) 5)) ;; 5[deg]
+   (send self :move-gripper (send robot :gripper :arm :joint-angle) :tm 200 :wait t)
    )
+
   ;; 把持モード停止メソッド
   (:stop-grasp
-   (&optional (arm :arm) &key (wait nil))
+   ()
    "Stop grasp mode."
-   (send robot :gripper arm :angle-vector
-         (send-all (send robot :gripper arm :joint-list) :max-angle))
-   (send self :angle-vector (send robot :angle-vector) 1000 :gripper-controller)
-   (send self :set-compliance-slope 7 32)
-   (send self :set-torque-limit 7 1.0)
-   (send self :wait-interpolation :gripper-controller)
+   (send self :move-gripper (send (send robot :gripper_joint) :max-angle) :tm 1000 :wait t)
+   (unless (send self :simulation-modep)
+     (send self :set-compliance-slope 7 32)
+     (send self :set-torque-limit 7 1.0))
    )
   )
 )

dynamixel_7dof_arm/euslisp/dxl-7dof-arm-interface.l

@@ -13,7 +13,7 @@
 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
 (defclass dxl-7dof-arm-interface
   :super robot-interface
-  :slots ())
+  :slots (gripper-action))
 
 (eval `(defmethod dxl-7dof-arm-interface
          ;; dxl-7dof-arm-interfaceのメソッドをdefmethodする

dynamixel_7dof_arm/euslisp/dxl-7dof-arm-robot.l

@@ -304,7 +304,7 @@
        (setq jc3 (instance rotational-joint :init :parent-link (elt rarm 2) :child-link (elt rarm 3) :name "arm_joint4" :axis :x :min -180 :max 180 :max-joint-velocity max-joint-velocity :max-joint-torque max-joint-torque))
        (setq jc4 (instance rotational-joint :init :parent-link (elt rarm 3) :child-link (elt rarm 4) :name "arm_joint5" :axis :y :min -90 :max 90 :max-joint-velocity max-joint-velocity :max-joint-torque max-joint-torque))
        (setq jc5 (instance rotational-joint :init :parent-link (elt rarm 4) :child-link (elt rarm 5) :name "arm_joint6" :axis :x :min -180 :max 180 :max-joint-velocity max-joint-velocity :max-joint-torque max-joint-torque))
-       (setq gripper_jc (instance rotational-joint :init :parent-link (elt rarm 5) :child-link (elt rarm 6) :name "arm_joint7" :axis :z :min -55 :max 50 :max-joint-velocity max-joint-velocity :max-joint-torque max-joint-torque))
+       (setq gripper_jc (instance rotational-joint :init :parent-link (elt rarm 5) :child-link (elt rarm 6) :name "gripper_joint" :axis :z :min -55 :max 50 :max-joint-velocity max-joint-velocity :max-joint-torque max-joint-torque))
        )
 
      ;; 4. define slots for robot class
@@ -353,10 +353,31 @@
   (:arm_joint5 () jc4)
   (:arm_joint6 () jc5)
   (:gripper_joint () gripper_jc)
+
   ;; limbs
   (:arm (&rest args)
         "Accessor for arm methods."
         (unless args (setq args (list nil))) (send* self :limb :rarm args))
+
+  (:gripper
+   (limb &rest args)
+   (let ()
+     (unless (or (eq limb :arm) (eq limb :rarm))
+       (error "you can only use ~A, not ~A " ":arm" limb)
+       (return-from :gripper nil))
+     (cond
+      ((memq :links args)
+       (list (send gripper_jc :child-link)))
+      ((memq :joint-list args)
+       (list gripper_jc))
+      ((memq :joint-angle args)
+       (if (null (cdr args))
+           (send gripper_jc :joint-angle)
+         (send gripper_jc :joint-angle (cadr args))
+         ))
+      (t (send-super* :gripper limb args))
+      )))
+
   ;; poses
   (:reset-pose
    ()