Skip to content

Fix calc static balance point #541

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
k-okada merged 2 commits into from
Jul 10, 2019
Merged

Fix calc static balance point #541

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
1f4ef9e
[irtrobot.l test-irt-motion.l]fix sign and unit of moment in :calc-st…
Naoki-Hiraoka May 19, 2019
5653165
Merge branch 'master' into fix-calc-static-balance-point
k-okada Jun 6, 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
4 changes: 2 additions & 2 deletions irteus/irtrobot.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1017,7 +1017,7 @@
"Calculate static balance point which is equivalent to static extended ZMP.
The output is expressed by the world coordinates.
target-points are end-effector points on which force-list and moment-list apply.
force-list [N] and moment-list [Nm] are list of force and moment at target-points.
force-list [N] and moment-list [Nm] are list of force and moment that robot receives at target-points.
static-balance-point-height is height of static balance point [mm]."
(let* ((sbp (float-vector 0 0 static-balance-point-height))
(mg (* 1e-6 (send self :weight update-mass-properties) (elt *g-vec* 2))))
Expand All @@ -1029,7 +1029,7 @@
(+ nume
(- (* (- (elt tp 2) static-balance-point-height) (elt f idx))
(* (elt tp idx) (elt f 2)))
(case idx (0 (- (elt m 1))) (1 (elt m 0))))
(* 1e3 (case idx (0 (elt m 1)) (1 (- (elt m 0))))))
denom (- denom (elt f 2))))
force-list moment-list target-points)
(setf (elt sbp idx) (/ nume denom))
Expand Down
48 changes: 48 additions & 0 deletions irteus/test/test-irt-motion.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -871,6 +871,50 @@
(send robot :calc-static-balance-point :force-list (list #f(10 20 0) #f(25 20 0))))))
))

;; test for static balance point
(defun fullbody-ik-with-forces-moments (robot &key (debug-view :no-message))
(send robot :newcoords (make-coords))
(send robot :reset-pose)
(send robot :fix-leg-to-coords (make-coords))
(send robot :legs :move-end-pos (float-vector 0 0 50))
(send robot :fix-leg-to-coords (make-coords))
(objects (list robot))
(let* ((centroid-thre 10)
(force-list '(#f(10 20 0) #f(25 20 0)))
(moment-list '(#f(20 -10 0) #f(10 -20 0)))
(result
(send robot :fullbody-inverse-kinematics
(list (send robot :rleg :end-coords :copy-worldcoords)
(send robot :lleg :end-coords :copy-worldcoords)
(make-coords :pos #f(150 -300 600))
(make-coords :pos #f(150 300 600)))
:move-target (mapcar #'(lambda (x)
(send robot x :end-coords))
(list :rleg :lleg :rarm :larm))
:link-list (mapcar #'(lambda (x)
(send robot :link-list (send robot x :end-coords :parent)))
(list :rleg :lleg :rarm :larm))
:centroid-thre centroid-thre
:centroid-offset-func #'(lambda () (send robot :calc-static-balance-point :force-list force-list :moment-list moment-list))
:debug-view debug-view)))
(and result
(> centroid-thre (distance (apply #'midpoint 0.5 (send robot :legs :end-coords :worldpos))
(send robot :calc-static-balance-point :force-list force-list :moment-list moment-list)))
;;check static balance
;;The sum of force and moment that robot receives at target-points, ZMP and centroid should be almost zero (except yaw moment).
(> 1.0
Copy link
Member

@mmurooka mmurooka May 24, 2019

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please add a comment about the meaning of this condition checking.

Copy link
Member

@mmurooka mmurooka May 24, 2019

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I follow this code, and now understand this condition check and think this is OK. (But still add comment please.)

(norm (subseq (transform (send robot :calc-grasp-matrix
(append (mapcar #'(lambda (x) (send robot x :end-coords :worldpos)) (list :rarm :larm))
(list (send robot :centroid)
(send (send robot :foot-midcoords) :worldpos))))
(concatenate float-vector
(mapcan #'(lambda (f m) (concatenate cons f m))
force-list moment-list)
(v- (scale (* 1e-6 (send robot :weight)) *g-vec*)) #F(0 0 0)
(v- (scale (* 1e-6 (send robot :weight)) *g-vec*) (reduce #'v+ force-list)) #F(0 0 0)))
0 5))))
))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; unit tests
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
Expand Down Expand Up @@ -954,6 +998,10 @@
(assert
(fullbody-ik-with-forces *sample-robot*)))

(deftest test-fullbody-ik-with-forces-moments-samplerobot
(assert
(fullbody-ik-with-forces-moments *sample-robot*)))

(deftest test-fullbody-ik-draw-param-check-samplerobot
(dotimes (i 5) (send *sample-robot* :move-centroid-on-foot :both '(:rleg :lleg)))
(assert
Expand Down