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.

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

[baxtereus] Compute IK from prepared poses (using :ik-prepared-poses methods) #602

Merged
merged 3 commits into from May 13, 2016
Merged
Changes from all commits
Commits
File filter...
Filter file types
Jump to…
Jump to file or symbol
Failed to load files and symbols.

Always

Just for now

@@ -3,12 +3,18 @@
(defmethod baxter-robot
(:inverse-kinematics
(target-coords &rest args &key (avoid-collision-distance 5) &allow-other-keys)
"Compute Inverse Kinematics with some strategies.
The computation runs until the IK is solved as below:
1. From current pose.
2. Split points between current and target coords from each.
3. From prepared poses. Poses in `:ik-prepared-poses` methods are used,
and if it is not defined `:untuck-pose` is used."
(let ((r) (prev-av (send self :angle-vector)))
(setq r (send-super* :inverse-kinematics target-coords :avoid-collision-distance avoid-collision-distance :warnp nil :dump-command nil args))
(unless r ;;
(format *error-output* "; failed for normal ik, staring from relaxed position~%")
(let ((step 0.0)
(current-coords (send (cadr (memq :move-target args)) :copy-worldcoords)))
(let ((current-coords (send (cadr (memq :move-target args)) :copy-worldcoords)))
(send-super* :inverse-kinematics current-coords :rotation-axis nil :avoid-nspace-gain 0.1 :avoid-weight-gain 0.1 :stop 200 :avoid-collision-distance avoid-collision-distance :debug-view nil args)
(setq r (send-super* :inverse-kinematics target-coords :warnp nil :dump-command nil args))
(if (and (null r) (or (null (memq :revert-if-fail args)) (cadr (memq :revert-if-fail args))))
@@ -25,15 +31,21 @@
(incf step 0.01))
(unless r (send self :angle-vector prev-av))))
(unless r ;; start from ik-frendly position
(format *error-output* "; failed for slow ik, try to start from good position~%")
(format *error-output* "; failed for slow ik, try to start from prepared poses~%")
(let* ((move-joints (send-all (cadr (memq :link-list args)) :joint))
(av (mapcar #'(lambda (j) (send j :joint-angle)) (send self :joint-list))))
(send self :untuck-pose)
(mapcar #'(lambda (j a)
(if (not (memq j move-joints))
(send j :joint-angle a)))
(send self :joint-list) av)
(setq r (send-super* :inverse-kinematics target-coords :avoid-collision-distance avoid-collision-distance :warnp nil :dump-command nil args))
(av (mapcar #'(lambda (j) (send j :joint-angle)) (send self :joint-list)))
(ik-prepared-poses (if (memq :ik-prepared-poses (send self :methods)) (send self :ik-prepared-poses) '(:untuck-pose))))

This comment has been minimized.

Copy link
@k-okada

k-okada May 16, 2016

Member

you can use (find-method self :ik-prepared-poses)

(dolist (pose ik-prepared-poses)
(unless r
(format *error-output* "; starting from prepared pose '~A'~%" pose)
(send self pose)
(mapcar #'(lambda (j a)
(if (not (memq j move-joints))
(send j :joint-angle a)))
(send self :joint-list) av)
(setq r (send-super* :inverse-kinematics target-coords :avoid-collision-distance avoid-collision-distance :warnp nil :dump-command nil args))
)
)
(if (and (null r) (or (null (memq :revert-if-fail args)) (cadr (memq :revert-if-fail args))))
;; when fail and :rever-if-fail is nil
(send self :angle-vector prev-av))
ProTip! Use n and p to navigate between commits in a pull request.
You can’t perform that action at this time.