Skip to content
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.

Sign up for GitHub

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

Jump to bottom

Updates to hand functions #2

Merged
merged 12 commits into from
Apr 8, 2019
Merged

Updates to hand functions #2

Changes from 11 commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
1f86a1c
Change 'set-hand-width' to 'hand-width'
Affonso-Gui Dec 3, 2018
bc3363a
Replace set-hand-width
Affonso-Gui Dec 3, 2018
a0ca0e8
Read robot state on hironxjsk-init
Affonso-Gui Dec 4, 2018
0b9949b
Add :time key to :start-grasp
Affonso-Gui Dec 4, 2018
32ddc9b
Add :time key to :stop-grasp
Affonso-Gui Dec 4, 2018
9f9067f
Add assertation message
Affonso-Gui Dec 4, 2018
eccd724
Allow sequence input on :hand-effort
Affonso-Gui Dec 5, 2018
d987336
Do not load force-moment offset in simulation mode
Affonso-Gui Dec 20, 2018
9a4d49e
Provide hand functions on simulation mode
Affonso-Gui Dec 20, 2018
8a99955
Allow ignoring safetymargin on hand-width
Affonso-Gui Dec 21, 2018
1e70084
Add typecheck to hand functions
Affonso-Gui Apr 3, 2019
4ddedec
Add printing information to typecheck
Affonso-Gui Apr 4, 2019
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
156 changes: 108 additions & 48 deletions hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,8 @@
(if (ros::resolve-ros-path "package://hironx_ros_bridge")
(ros::load-ros-manifest "hironx_ros_bridge"))

(defconstant +hironx-hand-servo-num+ 4)

(defclass hironxjsk-interface
:super rtm-ros-robot-interface
:slots ())
Expand All @@ -24,11 +26,12 @@
(send self :add-controller (read-from-string (format nil "~A-controller" limb))
:joint-enable-check t :create-actions t))
;; Load param to erase offset of force moment sensor
(send self :load-forcemoment-offset-param
(format nil "~A/models/~A-force-moment-offset.l"
(ros::resolve-ros-path "package://hrpsys_ros_bridge_tutorials")
(send robot :name))
:set-offset t)))
(unless (send self :simulation-modep)
(send self :load-forcemoment-offset-param
(format nil "~A/models/~A-force-moment-offset.l"
(ros::resolve-ros-path "package://hrpsys_ros_bridge_tutorials")
(send robot :name))
:set-offset t))))
(:define-all-ROSBridge-srv-methods
(&key (debug-view nil) (ros-pkg-name "hrpsys_ros_bridge"))
;; First, define ROSBridge method for old impedance controller
Expand Down Expand Up @@ -251,13 +254,41 @@
;; Based on https://github.com/start-jsk/rtmros_hironx/blob/2.1.0/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py
;; and https://github.com/start-jsk/rtmros_tutorials/blob/0.1.6/hrpsys_ros_bridge_tutorials/euslisp/hrp2-common-interface.l
(defmethod hironxjsk-interface
(:hand-angle-vector
(&optional (hand :hands) av (tm 1000))
(:hand-angle-vector (hand &optional av (tm 1000))
;; check type
(when av
(case hand
(:hands
(if (= (length av) +hironx-hand-servo-num+) (setq av (concatenate float-vector av av)))
(assert (= (length av) (* 2 +hironx-hand-servo-num+))))
pazeshun marked this conversation as resolved.
Show resolved Hide resolved
((:rhand :lhand)
(assert (= (length av) +hironx-hand-servo-num+)))))
Affonso-Gui marked this conversation as resolved.
Show resolved Hide resolved

;; simulation mode
(when (send self :simulation-modep)
(flet ((get-joint-list (hand)
(let (acc)
(dotimes (i 4) (push (read-from-string (format nil "~a_joint~a" hand i)) acc))
(nreverse acc))))
(let ((joint-list (case hand
(:hands (append (get-joint-list :rhand) (get-joint-list :lhand)))
((:rhand :lhand) (get-joint-list hand))
(t (error ";; No such hand: ~A~%." hand)))))
(return-from :hand-angle-vector
(if av
;; setjointangles
(map nil #'(lambda (joint angle) (send robot joint :joint-angle angle))
joint-list av)
;; getjointangles
(map float-vector #'(lambda (joint) (send robot joint :joint-angle))
joint-list))))))
;; real robot
(if av
;; setjointangles
(let ((av-rad-list (map cons #'deg2rad av)))
(case hand
(:hands (send self :call-operation-return :servocontrollerservice_setjointangles
(:hands
(send self :call-operation-return :servocontrollerservice_setjointangles
:jvs av-rad-list :tm (/ tm 1000.0)))
((:rhand :lhand)
(send self :call-operation-return :servocontrollerservice_setjointanglesofgroup
Expand All @@ -283,60 +314,89 @@
(* dir (send (send self :call-operation-return :servocontrollerservice_getjointangle :id id) :jv)))
ids dirs))))
(:hand-servo-on ()
(send self :call-operation-return :servocontrollerservice_servoon))
(unless (send self :simulation-modep)
(send self :call-operation-return :servocontrollerservice_servoon)))
(:hand-servo-off ()
(send self :call-operation-return :servocontrollerservice_servooff))
(unless (send self :simulation-modep)
(send self :call-operation-return :servocontrollerservice_servooff)))
(:hand-effort (&optional (hand :hands) effort)
"effort is percentage"
"effort is percentage or sequence of percentages"
(if (send self :simulation-modep) (return-from :hand-effort nil))
(let ((ids (case hand
(:hands (list 2 3 4 5 6 7 8 9))
(:rhand (list 2 3 4 5))
(:lhand (list 6 7 8 9))
(t (error ";; No such hand: ~A~%." hand)))))
(if effort
;; setmaxtorque
(mapcar
#'(lambda (id) (send self :call-operation-return :servocontrollerservice_setmaxtorque :id id :percentage effort))
ids)
(cond
((null effort)
;; getmaxtorque
(mapcar
#'(lambda (id) (send (send self :call-operation-return :servocontrollerservice_getmaxtorque :id id) :percentage))
ids))))
ids))
((and (numberp effort) (plusp effort))
;; setmaxtorque with same effort value
(mapcar
#'(lambda (id) (send self :call-operation-return :servocontrollerservice_setmaxtorque :id id :percentage effort))
ids))
((or (consp effort) (vectorp effort))
;; check length
(case hand
(:hands
(if (= (length effort) +hironx-hand-servo-num+)
(setq effort (concatenate float-vector effort effort)))
(assert (= (length effort) (* 2 +hironx-hand-servo-num+))))
pazeshun marked this conversation as resolved.
Show resolved Hide resolved
((:rhand :lhand)
(assert (= (length effort) +hironx-hand-servo-num+))))
pazeshun marked this conversation as resolved.
Show resolved Hide resolved
;; setmaxtorque with different effort values
(map cons
#'(lambda (id val)
(if val (send self :call-operation-return :servocontrollerservice_setmaxtorque :id id :percentage val)))
ids effort))
(t
;; unsupported type
(error "number or sequence expected")))))
(:hand-width2angles (width)
(let ((safetymargin 3) (l1 41.9) (l2 19) xpos a2pos a1radh a1rad a1deg)
(when (or (< width 0) (> width (* (- (+ l1 l2) safetymargin) 2)))
(let ((safetymargin 3) (w0 19) (l1 41.9))
(unless (<= (- safetymargin) width %(2 * (w0 + l1 - safetymargin)))
(warn ";; width value ~a is off margins~%" width)
(return-from :hand-width2angles nil))
(setq xpos (+ (/ width 2.0) safetymargin))
(setq a2pos (- xpos l2))
(setq a1radh (acos (/ a2pos l1)))
(setq a1rad (- (/ pi 2.0) a1radh))
(setq a1deg (rad2deg a1rad))
(float-vector a1deg (- a1deg) (- a1deg) a1deg)))
(:set-hand-width (hand width &key (tm 1000) effort)
(when effort
(send self :hand-effort hand effort))
(send self :hand-angle-vector hand (send self :hand-width2angles width) tm))
(:start-grasp (&optional (arm :arms) &key effort)
(cond ((eq arm :rarm)
(send self :set-hand-width :rhand 0 :effort effort))
((eq arm :larm)
(send self :set-hand-width :lhand 0 :effort effort))
((eq arm :arms)
(send self :set-hand-width :rhand 0 :effort effort)
(send self :set-hand-width :lhand 0 :effort effort))
(t (error ";; No such arm: ~A~%." arm))))
(:stop-grasp (&optional (arm :arms) &key effort)
(cond ((eq arm :rarm)
(send self :set-hand-width :rhand 100 :effort effort))
((eq arm :larm)
(send self :set-hand-width :lhand 100 :effort effort))
((eq arm :arms)
(send self :set-hand-width :rhand 100 :effort effort)
(send self :set-hand-width :lhand 100 :effort effort))
(t (error ";; No such arm: ~A~%." arm)))))
(let ((a (rad2deg %(pi/2 - acos((width / 2.0 + safetymargin - w0) / l1)))))
(float-vector a (- a) (- a) a))))
(:hand-angles2width (vec)
(assert (= (length vec) +hironx-hand-servo-num+)
(format nil "Expecting vector of length ~a~%" +hironx-hand-servo-num+))
(let ((safetymargin 3) (w0 19) (l1 41.9) (l2 20))
(flet ((get-width (r1 r2) %( w0 + l1 * cos(pi/2 - r1) + l2 * cos(pi/2 - r1 - r2) - safetymargin)))
(multiple-value-bind (a1 a2 b1 b2) (map cons #'deg2rad vec)
(+ (get-width a1 a2)
(get-width (- b1) (- b2)))))))
(:hand-width (hand &optional width &key (time 1000) effort)
(if width
;; set hand width
(progn
(when effort (send self :hand-effort hand effort))
(send self :hand-angle-vector hand (send self :hand-width2angles width) time))
;; get hand width
(send self :hand-angles2width (send self :hand-angle-vector hand))))
(:start-grasp (&optional (arm :arms) &key (time 1000) effort)
(case arm
(:arms (setq arm :hands))
(:rarm (setq arm :rhand))
(:larm (setq arm :lhand)))
(send self :hand-width arm 0 :time time :effort effort))
(:stop-grasp (&optional (arm :arms) &key (time 1000) effort)
(case arm
(:arms (setq arm :hands))
(:rarm (setq arm :rhand))
(:larm (setq arm :lhand)))
(send self :hand-width arm 100 :time time :effort effort)))

(defun hironxjsk-init (&rest args)
(if (not (boundp '*ri*))
(setq *ri* (instance* hironxjsk-interface :init args)))
(if (not (boundp '*hironxjsk*))
(setq *hironxjsk* (instance hironxjsk-robot :init))))
(setq *hironxjsk* (instance hironxjsk-robot :init)))
;; read initial robot state
(send *hironxjsk* :angle-vector (send *ri* :state :potentio-vector))
;; return robot instance
*hironxjsk*)