Skip to content

Update the robot model of dxl_7dof_arm #244

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 4 commits into from
Nov 30, 2018
Merged
Show file tree
Hide file tree
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Update the ros related function for gripper joint (e.g., actionlib, s…
…ubscriber, rosservice).

Check with both roseus keinamtic simulator and real robot.
  • Loading branch information
tongtybj committed Sep 19, 2018
commit 5caec8baa617d7f499dac8bdbdfc277b8f8377b9
136 changes: 99 additions & 37 deletions dynamixel_7dof_arm/euslisp/dxl-7dof-arm-interface-common.l
Original file line number Diff line number Diff line change
Expand Up @@ -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)))
Expand All @@ -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))
Expand All @@ -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
Expand All @@ -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)))
Expand All @@ -74,6 +95,7 @@
)
)
)

(:fullbody-controller
()
(list
Expand All @@ -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
Expand All @@ -102,6 +121,8 @@
)))
)
)
|#

(:default-controller
()
(send self :fullbody-controller)
Expand All @@ -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)))
)
Expand All @@ -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)))

Expand All @@ -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"
Expand All @@ -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))
)
)
)
2 changes: 1 addition & 1 deletion dynamixel_7dof_arm/euslisp/dxl-7dof-arm-interface.l
Original file line number Diff line number Diff line change
Expand Up @@ -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する
Expand Down
23 changes: 22 additions & 1 deletion dynamixel_7dof_arm/euslisp/dxl-7dof-arm-robot.l
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
()
Expand Down