diff --git a/dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot-interface.l b/dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot-interface.l index 15bc6a0a..0ad5575e 100644 --- a/dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot-interface.l +++ b/dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot-interface.l @@ -14,7 +14,7 @@ ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; (defclass dxl-armed-turtlebot-interface :super robot-interface - :slots ()) + :slots (gripper-action)) (eval `(defmethod dxl-armed-turtlebot-interface ;; dxl-7dof-arm-interface, turtlebot-interfaceのメソッドをそれぞれdefmethodする @@ -48,4 +48,4 @@ (send *irtviewer* :change-background #f(0.9 0.9 0.9)) (send *irtviewer* :draw-objects) ) -(warn ";; (dxl-armed-turtlebot-init) ;; for initialize ~%") \ No newline at end of file +(warn ";; (dxl-armed-turtlebot-init) ;; for initialize ~%") diff --git a/dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot.l b/dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot.l index 45cb4e12..1d57d934 100644 --- a/dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot.l +++ b/dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot.l @@ -63,6 +63,8 @@ (send self :method-copying "-pose") (send self :method-copying "_joint") (send self :method-copying "inverse-kinematics" t) + (send self :method-copying "gripper" t) + (send self :method-copying "arm" t) t)) (:method-copying (substr &optional (use-args nil)) @@ -79,10 +81,12 @@ ,(if use-args `(send* arm-robot ,me args) `(send arm-robot ,me)) ) )) - ))) + ) + methods)) + ;; limbs - (:arm (&rest args) - (unless args (setq args (list nil))) (send* self :limb :rarm args)) + ;;(:arm (&rest args) + ;;(unless args (setq args (list nil))) (send* self :limb :rarm args)) ) ;; アーム台車モデル生成関数 diff --git a/dynamixel_7dof_arm/config/dynamixel_joint_controllers.yaml b/dynamixel_7dof_arm/config/dynamixel_joint_controllers.yaml index 001c945d..e5346cd1 100644 --- a/dynamixel_7dof_arm/config/dynamixel_joint_controllers.yaml +++ b/dynamixel_7dof_arm/config/dynamixel_joint_controllers.yaml @@ -76,12 +76,12 @@ arm_j6_controller: min: 0 max: 1023 -arm_j7_controller: +gripper_joint_controller: controller: package: dynamixel_controllers module: joint_position_controller type: JointPositionController - joint_name: arm_joint7 + joint_name: gripper_joint joint_speed: 2.0 motor: id: 7 diff --git a/dynamixel_7dof_arm/euslisp/dxl-7dof-arm-interface-common.l b/dynamixel_7dof_arm/euslisp/dxl-7dof-arm-interface-common.l index c19cd356..3efa6aed 100644 --- a/dynamixel_7dof_arm/euslisp/dxl-7dof-arm-interface-common.l +++ b/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)) ) ) ) diff --git a/dynamixel_7dof_arm/euslisp/dxl-7dof-arm-interface.l b/dynamixel_7dof_arm/euslisp/dxl-7dof-arm-interface.l index e2011415..798209be 100644 --- a/dynamixel_7dof_arm/euslisp/dxl-7dof-arm-interface.l +++ b/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する diff --git a/dynamixel_7dof_arm/euslisp/dxl-7dof-arm-robot.l b/dynamixel_7dof_arm/euslisp/dxl-7dof-arm-robot.l index 37529684..ea119391 100644 --- a/dynamixel_7dof_arm/euslisp/dxl-7dof-arm-robot.l +++ b/dynamixel_7dof_arm/euslisp/dxl-7dof-arm-robot.l @@ -276,7 +276,7 @@ (defclass dxl-7dof-arm-robot :super robot-model - :slots (jc0 jc1 jc2 jc3 jc4 jc5 jc6)) ;; 関節インスタンス + :slots (jc0 jc1 jc2 jc3 jc4 jc5 gripper_jc)) ;; 関節インスタンス (defmethod dxl-7dof-arm-robot (:init @@ -304,13 +304,13 @@ (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 jc6 (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 ;; links and joint-list for cascaded-link. - (setq links (append (list aroot-link) rarm)) - (setq joint-list (list jc0 jc1 jc2 jc3 jc4 jc5 jc6)) + (setq links (append (list aroot-link) (butlast rarm))) + (setq joint-list (list jc0 jc1 jc2 jc3 jc4 jc5)) ;; These are for robot-model. (setq rarm-root-link (car rarm)) ;; end-coords @@ -320,6 +320,9 @@ ;; 5. call :init-ending after defining links and joint-list and return "self" (send self :init-ending) + + ;; 6. overwrite bodies to return draw-things links not (send link :bodies) + (setq bodies (flatten (mapcar #'(lambda (b) (if (find-method b :bodies) (send b :bodies))) (append links (last rarm))))) self)) ;; links (:make-root-link @@ -349,32 +352,57 @@ (:arm_joint4 () jc3) (:arm_joint5 () jc4) (:arm_joint6 () jc5) - (:arm_joint7 () jc6) + (: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 () "Reset pose." - (send self :angle-vector (float-vector 0.0 0.0 -90.0 0.0 90.0 0.0 0.0))) + (let () + (send self :angle-vector (float-vector 0.0 0.0 -90.0 0.0 90.0 0.0)) + (send (send self :gripper_joint) :joint-angle 0))) (:reset-pose2 () "Reset pose2." - (send self :angle-vector (float-vector 0.0 150.0 -90.0 0.0 90.0 0.0 0.0))) + (let () + (send self :angle-vector (float-vector 0.0 150.0 -90.0 0.0 90.0 0.0)) + (send (send self :gripper_joint) :joint-angle 0))) (:tuckarm-pose () "Folding arm pose." - (send self :angle-vector (float-vector 0.0 150.0 -90.0 0.0 0.0 0.0 0.0))) + (send self :angle-vector (float-vector 0.0 150.0 -90.0 0.0 0.0 0.0))) (:tuckarm-pose2 () "Folding arm pose2." - (send self :angle-vector (float-vector 90.0 90.0 5.0 0.0 -90.0 90.0 0.0))) + (send self :angle-vector (float-vector 90.0 90.0 5.0 0.0 -90.0 90.0))) (:tuckarm-pose3 () "Folding arm pose3." - (send self :angle-vector (float-vector 0.0 90.0 0.0 -20.0 90.0 0.0 0.0))) + (send self :angle-vector (float-vector 0.0 90.0 0.0 -20.0 90.0 0.0))) ;; inverse-kinematics (:inverse-kinematics (target-coords &rest args &key (link-list) @@ -394,7 +422,6 @@ (setq move-target (send self :arm :end-coords))) (unless link-list (setq link-list (send self :link-list (send move-target :parent)))) - ;; use base (cond (use-base diff --git a/dynamixel_7dof_arm/launch/dynamixel_7dof_arm_bringup.launch b/dynamixel_7dof_arm/launch/dynamixel_7dof_arm_bringup.launch index 3e57e2f8..afd6a178 100644 --- a/dynamixel_7dof_arm/launch/dynamixel_7dof_arm_bringup.launch +++ b/dynamixel_7dof_arm/launch/dynamixel_7dof_arm_bringup.launch @@ -28,7 +28,7 @@ arm_j4_controller arm_j5_controller arm_j6_controller - arm_j7_controller" + gripper_joint_controller" output="screen"/> diff --git a/dynamixel_7dof_arm/launch/gripper_controller_spawner.launch b/dynamixel_7dof_arm/launch/gripper_controller_spawner.launch index 48942938..25228493 100644 --- a/dynamixel_7dof_arm/launch/gripper_controller_spawner.launch +++ b/dynamixel_7dof_arm/launch/gripper_controller_spawner.launch @@ -8,6 +8,6 @@ --port=7dof_arm_port --type=meta gripper_controller - arm_j7_controller" + gripper_joint_controller" output="screen"/>