Skip to content

Commit 9f64ae9

Browse files
k-okadak-okada
authored
Merge pull request #552 from Naoki-Hiraoka/fix-bag-of-go-pos-params-footstep-list
fix bug of go-pos-params->footstep-list
2 parents 36d6866 + 3661c08 commit 9f64ae9

File tree

2 files changed

+99
-24
lines changed

2 files changed

+99
-24
lines changed

‎irteus/irtrobot.l‎

Lines changed: 30 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -832,47 +832,53 @@
832832
(send cc :name leg)
833833
cc))))
834834
"Calculate foot step list from goal x position [mm], goal y position [mm], and goal yaw orientation [deg]."
835-
(let* ((x-sign (if (> xx 0.0) 1.0 -1.0))
836-
(y-sign (if (> yy 0.0) 1.0 -1.0))
837-
(th-sign (if (> th 0.0) 1.0 -1.0))
838-
(dx 0.0) (dy 0.0) (dth 0.0) (cnt 0)
835+
(let* ((dx xx) (dy yy) (dth th)
839836
(leg (if (eps= (float yy) 0.0)
840837
(if (> th 0.0) :lleg :rleg)
841838
(if (> yy 0.0) :lleg :rleg)))
842839
(mc (apply #'midcoords 0.5 (send self :legs :end-coords :copy-worldcoords)))
843-
;;(mc (make-coords))
840+
(gc (send (make-coords :pos (float-vector xx yy 0) :rpy (float-vector (deg2rad th) 0 0)) :transform mc :world))
844841
(leg-translate-pos
845842
(mapcar #'(lambda (l)
846843
(list l (scale (case l (:rleg -1) (:lleg 1)) defp)))
847844
'(:rleg :lleg)))
848845
(ret (list (funcall gen-go-pos-step-node-func mc (case leg (:lleg :rleg) (:rleg :lleg)) leg-translate-pos))))
849846
(labels ((do-push-steps
850-
(max-cnt func)
851-
(setq cnt 0)
852-
(dotimes (i max-cnt)
847+
(finish-check func)
848+
(until (funcall finish-check)
853849
(funcall func)
854850
(push (funcall gen-go-pos-step-node-func mc leg leg-translate-pos) ret)
855851
(setq leg (case leg (:lleg :rleg) (:rleg :lleg)))
856-
(incf cnt)
857-
))
858-
(calc-max-count
859-
(val val-max)
860-
(1+ (round (floor (- (/ (abs val) val-max) 1e-5))))))
861-
(do-push-steps (max (calc-max-count xx xx-max)
862-
(- (* (calc-max-count yy yy-max) 2) 1)
863-
(calc-max-count th th-max))
852+
)))
853+
(do-push-steps #'(lambda ()
854+
(and (eps= (float dx) 0.0)
855+
(eps= (float dy) 0.0)
856+
(eps= (float dth) 0.0)))
864857
#'(lambda ()
865-
(let ((ddx (if (> xx-max (abs (- dx xx))) (- xx dx) (* xx-max x-sign)))
866-
(ddy (cond
867-
((oddp cnt) 0.0)
868-
((> yy-max (abs (- dy yy))) (- yy dy))
869-
(t (* yy-max y-sign))))
858+
(let ((ddx (cond
859+
((< xx-max dx) xx-max)
860+
((> (- xx-max) dx) (- xx-max))
861+
(t dx)))
862+
(ddy (case leg
863+
(:rleg
864+
(cond
865+
((< 0.0 dy) 0.0)
866+
((> (- yy-max) dy) (- yy-max))
867+
(t dy)))
868+
(:lleg
869+
(cond
870+
((< yy-max dy) yy-max)
871+
((> 0.0 dy) 0.0)
872+
(t dy)))
873+
))
870874
(ddth (cond
871-
((> th-max (abs (- dth th))) (- th dth))
872-
(t (* th-max th-sign)))))
875+
((< th-max dth) th-max)
876+
((> (- th-max) dth) (- th-max))
877+
(t dth))))
873878
(send mc :translate (float-vector ddx ddy 0.0))
874879
(send mc :rotate (deg2rad ddth) :z)
875-
(setq dx (+ dx ddx) dy (+ dy ddy) dth (+ dth ddth)))))
880+
(let ((diff (send mc :transformation gc :local)))
881+
(setq dx (elt (send diff :pos) 0) dy (elt (send diff :pos) 1) dth (rad2deg (elt (car (rpy-angle (send diff :rot))) 0)))))))
876882
(push (funcall gen-go-pos-step-node-func mc leg leg-translate-pos) ret)
877883
(reverse ret))))
878884
(:go-pos-quadruped-params->footstep-list

‎irteus/test/test-irt-motion.l‎

Lines changed: 69 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -915,6 +915,21 @@
915915
0 5))))
916916
))
917917

918+
;;test for go-pos-params->footstep-list
919+
(defun test-go-pos-params->footstep-list (robot x y th)
920+
(let* ((sc (send (send robot :foot-midcoords) :copy-worldcoords))
921+
(tc (send (make-coords :pos (float-vector x y 0) :rpy (float-vector (deg2rad th) 0 0)) :transform sc :world))
922+
(footstep-list (send robot :go-pos-params->footstep-list x y th))
923+
(gc (midcoords 0.5 (car (reverse footstep-list)) (cadr (reverse footstep-list))))
924+
(diff (send tc :transformation gc :local))
925+
)
926+
(and
927+
(eps= (elt (send diff :pos) 0) 0.0)
928+
(eps= (elt (send diff :pos) 1) 0.0)
929+
(eps= (rad2deg (elt (car (rpy-angle (send diff :rot))) 0)) 0.0))
930+
)
931+
)
932+
918933
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
919934
;; unit tests
920935
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
@@ -1042,6 +1057,60 @@
10421057
(fullbody-ik-with-look-at robot :debug-view nil))
10431058
))
10441059

1060+
(deftest test-go-pos-params->footstep-list-test_0_500_0_0
1061+
(send *sample-robot* :reset-pose)
1062+
(send *sample-robot* :fix-leg-to-coords (make-coords))
1063+
(assert (test-go-pos-params->footstep-list *sample-robot* 500 0 0)))
1064+
1065+
(deftest test-go-pos-params->footstep-list-test_0_0_150_0
1066+
(send *sample-robot* :reset-pose)
1067+
(send *sample-robot* :fix-leg-to-coords (make-coords))
1068+
(assert (test-go-pos-params->footstep-list *sample-robot* 0 150 0)))
1069+
1070+
(deftest test-go-pos-params->footstep-list-test_0_0_-150_0
1071+
(send *sample-robot* :reset-pose)
1072+
(send *sample-robot* :fix-leg-to-coords (make-coords))
1073+
(assert (test-go-pos-params->footstep-list *sample-robot* 0 -150 0)))
1074+
1075+
(deftest test-go-pos-params->footstep-list-test_0_0_0_45
1076+
(send *sample-robot* :reset-pose)
1077+
(send *sample-robot* :fix-leg-to-coords (make-coords))
1078+
(assert (test-go-pos-params->footstep-list *sample-robot* 0 0 45)))
1079+
1080+
(deftest test-go-pos-params->footstep-list-test_0_0_0_-45
1081+
(send *sample-robot* :reset-pose)
1082+
(send *sample-robot* :fix-leg-to-coords (make-coords))
1083+
(assert (test-go-pos-params->footstep-list *sample-robot* 0 0 -45)))
1084+
1085+
(deftest test-go-pos-params->footstep-list-test_0_0_0_180
1086+
(send *sample-robot* :reset-pose)
1087+
(send *sample-robot* :fix-leg-to-coords (make-coords))
1088+
(assert (test-go-pos-params->footstep-list *sample-robot* 0 0 180)))
1089+
1090+
(deftest test-go-pos-params->footstep-list-test_0_0_0_-180
1091+
(send *sample-robot* :reset-pose)
1092+
(send *sample-robot* :fix-leg-to-coords (make-coords))
1093+
(assert (test-go-pos-params->footstep-list *sample-robot* 0 0 -180)))
1094+
1095+
(deftest test-go-pos-params->footstep-list-test_0_500_150_45
1096+
(send *sample-robot* :reset-pose)
1097+
(send *sample-robot* :fix-leg-to-coords (make-coords))
1098+
(assert (test-go-pos-params->footstep-list *sample-robot* 500 150 45)))
1099+
1100+
(deftest test-go-pos-params->footstep-list-test_pi/2_500_150_45
1101+
(send *sample-robot* :reset-pose)
1102+
(send *sample-robot* :fix-leg-to-coords (make-coords :rpy (float-vector pi/2 0 0)))
1103+
(assert (test-go-pos-params->footstep-list *sample-robot* 500 150 45)))
1104+
1105+
(deftest test-go-pos-params->footstep-list-test_-pi/2_500_150_45
1106+
(send *sample-robot* :reset-pose)
1107+
(send *sample-robot* :fix-leg-to-coords (make-coords :rpy (float-vector -pi/2 0 0)))
1108+
(assert (test-go-pos-params->footstep-list *sample-robot* 500 150 45)))
1109+
1110+
(deftest test-go-pos-params->footstep-list-test_pi_500_150_45
1111+
(send *sample-robot* :reset-pose)
1112+
(send *sample-robot* :fix-leg-to-coords (make-coords :rpy (float-vector pi 0 0)))
1113+
(assert (test-go-pos-params->footstep-list *sample-robot* 500 150 45)))
10451114

10461115
;; Test calling calc-walk-pattern-from-footstep-list N times
10471116
;; This test is for bug reported in https://github.com/euslisp/jskeus/issues/286.

0 commit comments

Comments
 (0)