Add :make-joint-min-max-table method

[snozawa]

Add :make-joint-min-max-table method by copying and reducing codes from rbrain/basicmodel.l.
Add require of :pqp because irtmodel.l uses pqp resources.

[k-okada]

the problem of original implementation is that it uses hash-table and when we dump hash table, then it will not able to load again.

see hrp2-simple-dump-file in rbrian/wrl2eus

[snozawa]

The problem of original implementation is that it uses hash-table and when we dump hash table, then it will not able to load again.
see hrp2-simple-dump-file in rbrian/wrl2eus

I think this part corresponds not to rbrain/wrl2eus.l but to rbrain/basicmodel.l code
and I copied these codes from rbrain/basicmodel.l.

I remember that gensym value is a problem,
so original rbrain/basicmodel.l code incdlues removing lisp::empty as nil.
https://github.com/euslisp/jskeus/pull/77/files#diff-84175dc49eace6b214a353e6d2b5fb34R924

Dumping joint-min-max-table and loading worked on rbrain/basicmoel.l and
it seems to work on irteus/irtmodel.l.

(require :hrp2jsk "package://hrpsys_ros_bridge_tutorials/models/hrp2jsk.l")
(unless (boundp '*hrp2jsk*)
  (hrp2jsk)
  ;; for debug
  (dolist (j (send *hrp2jsk* :joint-list))
    (send j :put :hard-max-angle (send j :max-angle))
    (send j :put :hard-min-angle (send j :min-angle))
    ))

(defun test-min-max ()
  (if (and (send *hrp2jsk* :torso :waist-y) (send *hrp2jsk* :torso :waist-p))
      (send *hrp2jsk* :make-joint-min-max-table
            (send *hrp2jsk* :torso :waist-y :parent-link)
            (send *hrp2jsk* :torso :waist-p :child-link)
            (send *hrp2jsk* :torso :waist-y)
            (send *hrp2jsk* :torso :waist-p)
            :debug nil))
  ;;(plot-min-max *hrp2jsk* (send *hrp2jsk* :torso :waist-p) (send *hrp2jsk* :torso :waist-y))
  )

(defun test-min-max-with-calc-time ()
  (bench (null-output (test-min-max))))

(defun test-dump-min-max-table ()
  (dump-structure "/tmp/joint-min-max-table.l"
                  (mapcar #'(lambda (j) (list (send j :name) (send (send j :joint-min-max-target) :name) (send j :joint-min-max-table)))
                          (remove-if-not #'(lambda (x) (send x :joint-min-max-table)) (send *hrp2jsk* :joint-list)))))

(defun test-load-min-max-table ()
  (let ((jml (with-open-file
              (f "/tmp/joint-min-max-table.l" :direction :input)
              (read f)
              )))
    (dolist (jm jml)
      (send (send *hrp2jsk* :joint (car jm)) :joint-min-max-target (send *hrp2jsk* :joint (cadr jm)))
      (send (send *hrp2jsk* :joint (car jm)) :joint-min-max-table (caddr jm))
      )))

(defun plot-min-max (robot joint0 joint1 &optional (ff ""))
  (let* ((ret))
    (setq min-max-table-view
          (instance x::panel :create
                    :width  (round (- (send joint0 :max-angle) (send joint0 :min-angle)))
                    :height (round (- (send joint1 :max-angle) (send joint1 :min-angle)))
                    :atitle "min-max-table-view"))
    (send min-max-table-view :color #xffffff)
    (send min-max-table-view :flush)
    (null-output
     (do ((j0 (get joint0 :hard-min-angle) (+ 1 j0))) ((> j0 (get joint0 :hard-max-angle)))
      (send robot :init-pose)
      (send joint0 :joint-angle j0)
      (let ((flg0 (eps= (float (send joint0 :joint-angle)) (float j0))))
        (do ((j1 (get joint1 :hard-min-angle)(+ 1 j1))) ((> j1 (get joint1 :hard-max-angle)))
          (send joint1 :joint-angle j1)
          (let ((flg1 (eps= (float (send joint1 :joint-angle)) (float j1))))
            (send min-max-table-view :color #xff0000)
            (if (and flg0 flg1)
                (send min-max-table-view :draw-line
                      (float-vector (- (round j0) (get joint0 :hard-min-angle)) (- (round j1) (get joint1 :hard-min-angle)))
                      (float-vector (- (round j0) (get joint0 :hard-min-angle)) (- (round j1) (get joint1 :hard-min-angle)))))
            (push (list j0 j1 (if (and flg0 flg1) 1 0)) ret)
            ))
        )))
    (send min-max-table-view :flush)
    ))

• Dump joint-min-max-table

roseus
;; load avobe codes
(test-min-max-with-calc-time)
(test-dump-min-max-table)
(exit)

• Load joint-min-max-table and use it

roseus
;; load avobe codes
(test-load-min-max-table)
(plot-min-max *hrp2jsk* (send *hrp2jsk* :torso :waist-p) (send *hrp2jsk* :torso :waist-y))

(However, I think lisp::deleted seems to be gensym?)

After this commit, I'll send PRs for:

1. test code for euslisp/test/joint.l
2. another PR to check gethash range
3. min-max-table generation in hrpsys_ros_bridge_tutorials

Just for reference, it takes 1.5-1.9[s] to calculate all min-max tables currently and
it is possible to use joint-min-max-table without dump-and-load.

[snozawa]

1. another PR to check gethash range

I added this commit to this PR.
snozawa@d6f912f

Without this commit, gethash returns (list nil nil) by invalid hash key.

          (joint-range (mapcar #'(lambda (x) (funcall car-or-cadr (gethash x joint-min-max-table)))                                                                                                                                           

For example, if "joint-min-max-target"'s joint-angle 100[deg] exceeds range of joint-angle [-90, 90][deg], gethash returns (list nil nil).

So I should limit joint-min-max-targe's joint-angle by hash key range.
For almost all robots, the range equals to hardware-limit joint range.

In this commit,
snozawa@d6f912f
I added slots variable for joint class for limitation:

joint-min-max-target-min-angle
joint-min-max-target-max-angle

This solution works on rbrain/basicmodel.l, but I think this solution is not so smart.
Is there any ways to obtain max key value and min key value of hash-table?

[k-okada]

I'm not sure if I can understand correctly, but

1. use original min/max-angle before use gethash
2. use gethash's has key method (or sort of)
3. use matrix or list as data structure

[snozawa]

1. use original min/max-angle before use gethash

Yes, this is preferable than "joint-min-max-target-[min/max]-angle".
:min-angle and :max-angle are overwrited by joint-min-max-table,
so we need slots variables for original min/max-angle.

Adding hardware min/max angle is possible solution [1],
but hardware min/max is not available for all robots
as @YouheiKakiuchi says [2].

2. use gethash's has key method (or sort of)

It is not sufficient to check existence of hash key.
It is necessary to both to check existence os hash key
and to limit joint-angle by original min/max-angle if not exist
as you proposed in 1).

3. use matrix or list as data structure

I think this might be better than using hash table.
Matrix or list is simpler than hash table
in terms of portability and loadability.

I'll try fix my PR by

1. use original min/max-angle before use gethash

and

3. use matrix or list as data structure

[1] #74
[2] jsk-ros-pkg/jsk_model_tools#28

[snozawa]

I fixed PR.

1. use original min/max-angle before use gethash

I added org-min-angle and org-max-angle.

3. use matrix or list as data structure

I use assoc list instead of hash table.

[snozawa]

Travis passed and I reflected the following discussion:

1. use original min/max-angle before use gethash
2. use matrix or list as data structure

Can I merge this?
After that, I'll add test code for make-joint-min-max-table to euslisp/test/joint.l.

[k-okada]

Sorry that my comment on 1)2)3) did not mean that we have to consider all condition, just an idea of another possibility of three different( or mixed) direction
do we really need 'org-min-angle org-max-angle'? if we have min-max table, can we know the boundary?
I'm just afraid that no one will understand what is the meaning orig-min-angle in 3 month later.

sorry that i always not easy to merge the PR but since we created the irteus is because euslib becomes too large and complex, if we just copy existing code/function form euslisb to irteus, then irteus become just another euslib and we'll have to create irteus2, so I'd like to be very careful on adding new features.

[k-okada]

Btw how did euslib checks angle limit for min-max angle?

[snozawa]

Sorry that my comment on 1)2)3) did not mean that we have to consider all condition, just an idea of another possibility of three different( or mixed) direction
do we really need 'org-min-angle org-max-angle'? if we have min-max table, can we know the boundary?
I'm just afraid that no one will understand what is the meaning orig-min-angle in 3 month later.

I'm using original min/max angle for some codes .. etc.
(We need to know original min-max angle, for example, in model-converter test codes.)
But these codes are not fully included in irteus or jsk-ros-pkg,
so we'll neglect those and discuss the necessity of min/max angle
just for joint-min-max-table.

do we really need 'org-min-angle org-max-angle'? if we have min-max table, can we know the boundary?

Yes, as you said,
we know the boundary of table and we just need to limit values by boundary here:

          (joint-range (mapcar #'(lambda (x) (funcall car-or-cadr (gethash x joint-min-max-table)))

To do this,

• (a) Find boundary everytime like (find-extream table-key #'>) :
  To implement this, we do not need any slots variable to joint class.
  I was afraid that calling boundary-finding function everytime requires a lot of time.
  We do not need to calculate boundary everytime because boundary of min-max table is static.
• (b) Add joint-min-max-target-min/max-angle (My first PR) :
  To implement this, we need to add slots variable for boundary of min-max table.
• (c) Add org-min/max-angle (My second PR?) :
  To implement this, we need to add slots variable for boundary of min-max table.
  The difference between (b) and (c) is just a naming.
  I thought org-min/max-angle is better naming than (b) after your suggestion.
  (I think I reflected your suggestion (1), but is this correct??)
• (d) Use hash table functionality?? :
  If gethash returns boundary value instead of nil for non-exist key value,
  this is preferable.
  However, i think this is not original functionality of hash table and
  I could not find the methods or functions.

Btw how did euslib checks angle limit for min-max angle?

euslib codes checks angle limit by using plist.
The implementation is simillar to (c).

sorry that i always not easy to merge the PR but since we created the irteus is because euslib becomes too large and complex,
if we just copy existing code/function form euslisb to irteus, then irteus become just another euslib and we'll have to create irteus2, so I'd like to be very careful on adding new features.

I completely agree with your opinion.
I'd like to discuss and brush up codes.

Min-max table is important feature in euslib codes.
Currently, the experiments which we are doing now are quite dangerous
becuase we have no min-max table for irteus robots.

[garaemon]

is it possible to merge this PR?

[k-okada]

no, @snozawa said there are redundant codes within commit

[snozawa]

@garaemon, sorry for late.

As @k-okada said, I'll fix some codes:

• Implement table as matrix instead of assoc list.
• Use matrix first and last element instead of org-min-angle and org-max-angle
• Reduce codes (merge methods?)

Please wait for the PR update.

[k-okada]

• Implement table as matrix instead of assoc list.

This is just my guess. Please confirm ;

• Hash and matrix is faster than assoc
• Hash is not relocatable ( could not dump and load)
• Assoc requires org-min/max-angle
  etc

2014年6月10日火曜日、Shunichi Nozawanotifications@github.comさんは書きました:

@garaemon https://github.com/garaemon, sorry for late.

As @k-okada https://github.com/k-okada said, I'll fix some codes:

• Implement table as matrix instead of assoc list.
• Use matrix first and last element instead of org-min-angle and
  org-max-angle
• Reduce codes (merge methods?)

Please wait for the PR update.

—
Reply to this email directly or view it on GitHub
#77 (comment).

[snozawa]

I upgraded this PR codes.

I reduced codes and checked these points:

• Hash and matrix is faster than assoc
• Hash is not relocatable ( could not dump and load)
• Assoc requires org-min/max-angle etc

• Hash and matrix is faster than assoc
  Matrix was fastest because we can access values by index and I used matrix for joint-min-max-table.
  Access speed was matrix > hash table > assoc list.
  I calculated these speed like (dotimes (i 1000) (access-xxx)) using this test code:
  https://gist.github.com/snozawa/ebc04e2b3b89821fec0c#file-test-codes-for-joint-min-max-table
  Detailed results is here:

  table-type	time
  hash table	0.906402[s]
  matrix	0.418467[s]
  assoc list	1.0407[s]

  The speed of hash table is not so different from that of assoc list
  because the size of joint-min-max-table is small (100-200).

• Hash is not relocatable ( could not dump and load)
  If I dump hash-table joint-min-max-table and load it on another PC (Euslisp version was different?),
  I could not access joint-min-max-table because of Segfo because of gensym value, pointer, ...etc.
  I confirmed Hash table is not relocatable.

• Assoc requires org-min/max-angle etc
  Althoug org-min/max-angle is not necessary,
  without org-min/max-angle, we need to find range of table key everytime.
  It will become problem in the case of hash table because
  hash table key is not sorted.

  However, if we use matrix or assoc list, keys can be sorted and
  org-min/max-angle is not so important.
  Therefore, in the latest PR codes, I did not add org-min/max-angle.

[garaemon]

Why do we still this PR not to be merged?

[garaemon]

So, now min-max table is supported?