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 2 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
50 changes: 29 additions & 21 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 @@ -252,7 +252,7 @@
;; 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))
(if av
;; setjointangles
(let ((av-rad-list (map cons #'deg2rad av)))
Expand Down Expand Up @@ -303,36 +303,44 @@
#'(lambda (id) (send (send self :call-operation-return :servocontrollerservice_getmaxtorque :id id) :percentage))
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 (<= 0 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))
(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))
Affonso-Gui marked this conversation as resolved.
Show resolved Hide resolved
(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 (tm 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) tm))
;; get hand width
(send self :hand-angles2width (send self :hand-angle-vector hand))))
(:start-grasp (&optional (arm :arms) &key effort)
(cond ((eq arm :rarm)
(send self :set-hand-width :rhand 0 :effort effort))
(send self :hand-width :rhand 0 :effort effort))
((eq arm :larm)
(send self :set-hand-width :lhand 0 :effort effort))
(send self :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))
(send self :hand-width :rhand 0 :effort effort)
(send self :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))
(send self :hand-width :rhand 100 :effort effort))
((eq arm :larm)
(send self :set-hand-width :lhand 100 :effort effort))
(send self :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))
(send self :hand-width :rhand 100 :effort effort)
(send self :hand-width :lhand 100 :effort effort))
(t (error ";; No such arm: ~A~%." arm)))))

(defun hironxjsk-init (&rest args)
Expand Down