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

Not use toe joint in fullbody ik #71

Closed
wants to merge 1 commit into from
Closed

Not use toe joint in fullbody ik #71

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
86 changes: 86 additions & 0 deletions hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-utils.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,3 +6,89 @@
(eval
`(defmethod hrp2jsknt-robot
,@(get-hrp2-with-hand-class-methods)))

(defmethod hrp2jsknt-robot
(:move-centroid-on-foot
(leg fix-limbs
&rest args
&key (mid 0.5)
(use-toes (make-list (length (remove-if-not #'(lambda (x) (memq x '(:rleg :lleg))) fix-limbs)) :initial-element nil))
(target-centroid-pos (send self :calc-target-centroid-pos leg (remove-if-not #'(lambda (x) (memq x '(:rleg :lleg))) fix-limbs) use-toes mid))
&allow-other-keys)
(send-super*
:move-centroid-on-foot leg fix-limbs
:use-toes use-toes :target-centroid-pos target-centroid-pos
args))
(:calc-target-centroid-pos-for-one-leg
(fix-leg &optional (use-toe) (offset))
(if use-toe
(cadr (send (send self :foot-polygon fix-leg '((:toe-p))) :centroid))
(send (send self fix-leg :end-coords) :transform-vector offset)))
(:calc-target-centroid-pos
(centroid-leg fix-legs
&optional (use-toes (make-list (length fix-legs) :initial-element nil))
(mid 0.5)
(offsets
(mapcar
#'(lambda (x)
(cond
((substringp "HRP2JSKNT" (send self :name))
(float-vector -20 0 0))
(t
(float-vector 0 0 0))))
(range (length fix-legs)))))
(if (eq centroid-leg :both)
(apply
#'midpoint mid
(mapcar
#'(lambda (tmp-leg tmp-ut tmp-of)
(send self :calc-target-centroid-pos-for-one-leg tmp-leg tmp-ut tmp-of))
fix-legs use-toes offsets))
(send self :calc-target-centroid-pos-for-one-leg centroid-leg (car use-toes) (car offsets))))
(:fullbody-inverse-kinematics
(target-coords
&rest args
&key (fix-limbs '(:rleg :lleg))
(use-toes (make-list (length fix-limbs) :initial-element nil)) ;; :use-toes '(t nil) -> use toe-joint of :rleg and does not use toe-joint of :lleg
(target-centroid-pos (send self :calc-target-centroid-pos (if (= (length fix-limbs) 2) :both (car fix-limbs)) fix-limbs use-toes))
(additional-weight-list)
&allow-other-keys)
(let* ((leg-ul (send self :calc-union-link-list
(mapcar #'(lambda (fl ut)
(remove-if #'(lambda (x) (unless ut (eq x (send (send self fl :toe-p) :child-link))))
(send self fl :links)))
fix-limbs use-toes))))
(send self :reset-joint-angle-limit-weight-old leg-ul)
(send-super* :fullbody-inverse-kinematics
target-coords
:target-centroid-pos target-centroid-pos
:additional-weight-list
(append
additional-weight-list
(remove nil
(mapcar #'(lambda (l u)
(if (send self l :toe-p)
(list (send self l :toe-p :child-link) (if u 1.0 0.0))))
fix-limbs use-toes))
(list
(list (car (send self :links))
#'(lambda ()
;; set root-link 6dof-joint's weight based on legs' joint limit
(let* ((min-weight
(reduce #'(lambda (x y) (min x y))
(coerce (send self :calc-inverse-kinematics-weight-from-link-list
leg-ul :union-link-list leg-ul) cons))))
(fill (instantiate float-vector 6) min-weight)))
)
))
args)))
)

(defun range (n)
(cond ((<= n 0)
(return-from range nil))
((= n 1)
(list 0))
(t
(append (range (- n 1)) (list (- n 1))))))

84 changes: 84 additions & 0 deletions hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-utils.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,3 +7,87 @@
`(defmethod hrp2jsknts-robot
,@(get-hrp2-with-hand-class-methods)))

(defmethod hrp2jsknt-robot
(:move-centroid-on-foot
(leg fix-limbs
&rest args
&key (mid 0.5)
(use-toes (make-list (length (remove-if-not #'(lambda (x) (memq x '(:rleg :lleg))) fix-limbs)) :initial-element nil))
(target-centroid-pos (send self :calc-target-centroid-pos leg (remove-if-not #'(lambda (x) (memq x '(:rleg :lleg))) fix-limbs) use-toes mid))
&allow-other-keys)
(send-super*
:move-centroid-on-foot leg fix-limbs
:use-toes use-toes :target-centroid-pos target-centroid-pos
args))
(:calc-target-centroid-pos-for-one-leg
(fix-leg &optional (use-toe) (offset))
(if use-toe
(cadr (send (send self :foot-polygon fix-leg '((:toe-p))) :centroid))
(send (send self fix-leg :end-coords) :transform-vector offset)))
(:calc-target-centroid-pos
(centroid-leg fix-legs
&optional (use-toes (make-list (length fix-legs) :initial-element nil))
(mid 0.5)
(offsets
(mapcar
#'(lambda (x)
(cond
((substringp "HRP2JSKNT" (send self :name))
(float-vector -20 0 0))
(t
(float-vector 0 0 0))))
(range (length fix-legs)))))
(if (eq centroid-leg :both)
(apply
#'midpoint mid
(mapcar
#'(lambda (tmp-leg tmp-ut tmp-of)
(send self :calc-target-centroid-pos-for-one-leg tmp-leg tmp-ut tmp-of))
fix-legs use-toes offsets))
(send self :calc-target-centroid-pos-for-one-leg centroid-leg (car use-toes) (car offsets))))
(:fullbody-inverse-kinematics
(target-coords
&rest args
&key (fix-limbs '(:rleg :lleg))
(use-toes (make-list (length fix-limbs) :initial-element nil)) ;; :use-toes '(t nil) -> use toe-joint of :rleg and does not use toe-joint of :lleg
(target-centroid-pos (send self :calc-target-centroid-pos (if (= (length fix-limbs) 2) :both (car fix-limbs)) fix-limbs use-toes))
(additional-weight-list)
&allow-other-keys)
(let* ((leg-ul (send self :calc-union-link-list
(mapcar #'(lambda (fl ut)
(remove-if #'(lambda (x) (unless ut (eq x (send (send self fl :toe-p) :child-link))))
(send self fl :links)))
fix-limbs use-toes))))
(send self :reset-joint-angle-limit-weight-old leg-ul)
(send-super* :fullbody-inverse-kinematics
target-coords
:target-centroid-pos target-centroid-pos
:additional-weight-list
(append
additional-weight-list
(remove nil
(mapcar #'(lambda (l u)
(if (send self l :toe-p)
(list (send self l :toe-p :child-link) (if u 1.0 0.0))))
fix-limbs use-toes))
(list
(list (car (send self :links))
#'(lambda ()
;; set root-link 6dof-joint's weight based on legs' joint limit
(let* ((min-weight
(reduce #'(lambda (x y) (min x y))
(coerce (send self :calc-inverse-kinematics-weight-from-link-list
leg-ul :union-link-list leg-ul) cons))))
(fill (instantiate float-vector 6) min-weight)))
)
))
args)))
)

(defun range (n)
(cond ((<= n 0)
(return-from range nil))
((= n 1)
(list 0))
(t
(append (range (- n 1)) (list (- n 1))))))