Skip to content

Updates to hand functions #2

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

Merged
pazeshun merged 12 commits into from
Apr 8, 2019
Merged

Updates to hand functions #2

Changes from 10 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
133 changes: 87 additions & 46 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 @@ -24,11 +24,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 @@ -252,12 +253,35 @@
;; 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 &optional av (tm 1000))
;; 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)))))
(if (and (eql hand :hands) (= (length av) 4))
(setq av (concatenate float-vector av av)))
(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
(when (= (length av) 4) (setq av-rad-list (concatenate float-vector av-rad-list av-rad-list)))
(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 +307,77 @@
(* 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
(cond
((numberp effort)
;; setmaxtorque with same effort value
(mapcar
#'(lambda (id) (send self :call-operation-return :servocontrollerservice_setmaxtorque :id id :percentage effort))
ids)
;; getmaxtorque
ids))
;; setmaxtorque with different effort values
(effort
Comment thread
Affonso-Gui marked this conversation as resolved.
Outdated
(map cons
#'(lambda (id val)
(if val (send self :call-operation-return :servocontrollerservice_setmaxtorque :id id :percentage val)))
ids effort))
;; getmaxtorque
(t
(mapcar
#'(lambda (id) (send (send self :call-operation-return :servocontrollerservice_getmaxtorque :id id) :percentage))
ids))))
ids)))))
(: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) 4) "Expecting vector of length 4~%")
(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*)