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

Jump to bottom

[hrpsys_ros_bridge_tutorials] Add collision-free-init-pose method for hrp2jsk #507

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

[hrpsys_ros_bridge_tutorials] Add collision-free-init-pose method for hrp2jsk #507

wants to merge 2 commits into from

Conversation

k-kimura
Copy link
Contributor

Because of the collision between thumb links and hand links at init-pose, added eus method :collision-free-init-pose for hrp2jsk.
Similarly, the following is executed from dashboard (init-pose button).
https://github.com/start-jsk/rtmros_tutorials/blob/master/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py#L56-#L60

@k-okada
Copy link
Member

k-okada commented Mar 30, 2017 via email

@k-kimura
Copy link
Contributor Author

For the case of JAXON (JAXON_RED), there seems to be separately two different methods about init-pose (:init-pose and :collision-free-init-pose).

I think the definition of :init-pose angle-vector is considered as zero vector...

(:init-pose () "Set robot to initial posture." (send self :angle-vector (instantiate float-vector (send self :calc-target-joint-dimension (cdr (send self :links))))))

So, where should we override :init-pose (as non-zero vector) for hrp2jsk, JAXON, JAXON_RED, etc without using new method :collision-free-init-pose?

@k-okada
Copy link
Member

k-okada commented Mar 30, 2017 via email

@k-kimura
Copy link
Contributor Author

I also believe that it would be better way to use existing :init-pose without the collision problem of hrp2jsk thumbs.
However, by using existing :init-pose method, the collision between thumb links and hand links causes the problem of thumb joints ThermoLimit error on real robot (hrp2jsk).

If someone have trouble when robot moves to undesired (or dangerous) posture, as he/she forget to use this method and use :init-pose instead.

(Just now we are facing this undesired (or dangerous) posture trouble...)

Then, what is the best way to solve this problem (on the assumption to use :init-pose without new method :collision-free-init-pose)?
If overwriting desired pose to :init-pose function (return zero vector as default) is the best way, how and which program files should we renew and define the angle-vector of :init-pose for hrp2jsk?

@k-okada
Copy link
Member

k-okada commented Mar 30, 2017 via email

@k-kimura
Copy link
Contributor Author

Simply renaming :collision-free-init-pose -> :init-pose on this yaml file sounds better.
(That was overwritten and worked well.)

I'm concerned to duplicate the definition of :init-pose in (send *robot* :methods) list.
Is this OK? If OK, I'll modify PR.

@k-okada k-okada requested a review from mmurooka March 31, 2017 00:33
@mmurooka
Copy link
Member

mmurooka commented Mar 31, 2017 •
edited
Loading

I believe so, because if this change is important, everyone should use this
new angle-vector when they want to move to initial pose.
If someone have trouble when robot moves to undesired (or dangerous)
posture, as he/she forget to use this method and use :init-pose instead. We
could not blame him/her for not changing his/her code to
:collision-free-init-pose.
If this new posture is not so important as I wrote, I think we do not have
to put this in yaml file.

I agree with this comment, but for the following reason, I think keeping :init-pose without change and adding new pose method is more safe and I prefer it. so I approve this PR.

:init-pose method is defined here https://github.com/euslisp/jskeus/blob/master/irteus/irtrobot.l#L383 in robot-model class. In this pose all joint angle is zero.

:init-pose is used in some places of other repositories, and I've not checked yet that these codes assume that :init-pose is all-joint-zero-pose, but may be some program is influenced unintended by changing :init-pose.
I think that If we could go back to the time when :init-pose definition was written, the method name should be :zero-pose instead of :init-pose..

# jskeus
leus@cygnus:~/ros/indigo_parent/src/euslisp/jskeus$ find ./ -name "*.l" -type f | xargs grep -iIns ":init-pose" *
./irteus/test/joint.l:202:                    (send robot :init-pose)
./irteus/test/test-irt-motion.l:585:    (send robot :init-pose)
./irteus/test/test-irt-motion.l:605:    (send robot :init-pose)
./irteus/test/test-irt-motion.l:626:    (send robot :init-pose)
./irteus/test/test-irt-motion.l:991:    ;; case 0 : side step with initialize and with :init-pose-function                                                                                                                                                                                                             
./irteus/test/test-irt-motion.l:999:    ;; case 1 : side step with initialize but without :init-pose-function                                                                                                                                                                                                          
./irteus/test/test-irt-motion.l:1006:                        :init-pose-function #'(lambda ())))
./irteus/test/test-irt-motion.l:1008:    ;; case 2 : side step without initialize and with :init-pose-function                                                                                                                                                                                                         
./irteus/irtrobot.l:383:  (:init-pose () "Set robot to initial posture." (send self :angle-vector (instantiate float-vector (send self :calc-target-joint-dimension (cdr (send self :links))))))
./irteus/irtmodel.l:900:     (send self :init-pose)
./irteus/irtmodel.l:988:          (send self :init-pose)
./irteus/demo/closed-loop.l:235:                (send *robot* :init-pose)
./irteus/demo/walk-motion.l:72:          :init-pose-function #'(lambda ()))
./irteus/demo/walk-motion.l:108:          :init-pose-function #'(lambda ()))
./irteus/demo/walk-motion.l:181:            :init-pose-function

# pr2eus
leus@cygnus:~/ros/indigo/src/jsk-ros-pkg/jsk_pr2eus$ find ./ -name "*.l" -type f | xargs grep -iIns ":init-pose" *
# not used

# rtmros_common
leus@cygnus:~/ros/indigo/src/rtm-ros-robotics/rtmros_common/hrpsys_ros_bridge$ find ./ -name "*.l" -type f | xargs grep -iIns ":init-pose" *
./test/hrpsys-samples/samplerobot-sequence-player.l:117:           (jpos1 (progn (send *sr* :init-pose) (send *sr* :fix-leg-to-coords (make-coords) '(:rleg :lleg)) (send *sr* :angle-vector)))

# rtmros_tutorials
leus@cygnus:~/ros/indigo/src/rtm-ros-robotics/rtmros_tutorials/hrpsys_ros_bridge_tutorials$ find ./ -name "*.l" -type f | xargs grep -iIns ":init-pose" *
./test/test-kf/check-kf-precision.l:9:  (send *robot* :init-pose)
./models/hrp4r.l:3781:    (:init-pose

cc @YoheiKakiuchi , @snozawa , @furushchev

@k-okada
Copy link
Member

k-okada commented Apr 4, 2017 via email

@mmurooka
Copy link
Member

mmurooka commented Apr 7, 2017 •
edited
Loading

but these are not related to jaxon robot, mostly for sample programs

Yes. Only the following two part is NOT sample or test in the range I can find.
https://github.com/euslisp/jskeus/blob/master/irteus/irtmodel.l#L900
in irtmodel :make-joint-min-max-table method.
https://github.com/euslisp/jskeus/blob/master/irteus/irtmodel.l#L988
in irtmodel :plot-joint-min-max-table-common method.

I think min-max-table is not generated for hrp2jsk thumb joint, so I think there is no effect now.
I don't realize that :init-pose in these methods means just neutral pose or angle zero pose.
In the latter case, we need to replace (send self :init-pose) with (send self :angle-vector (fill (send self :angle-vector) 0).

Another problem is that someone who assume that :init-pose is all-joint-zero-pose and does not know it is overwritten, maybe he/she might use a little time for debugging. But keeping real robot safe is also important.

I'm concerned to duplicate the definition of :init-pose in (send robot :methods) list.

I think this is OK. But I'd like euslisp specialist to doublecheck.

@k-okada
Copy link
Member

k-okada commented Apr 10, 2017 via email

@k-kimura
Copy link
Contributor Author

So if you have trouble on thumb joint for zero-pose, another solution might be change zero-position of joint in hardware level. I think that is better solution.

This hardware level solution seems much better without adding new method :collision-free-init-pose and overwriting :init-pose on yaml.

hack :init-pose is better than add new methods, named :collision-free-init-pose,

From another viewpoint, I also agree with this idea because of the difficulty to share common understanding of :collision-free-init-pose for hrp2jsk.

So briefly I'll modify this PR as follows.

Simply renaming :collision-free-init-pose -> :init-pose on this yaml file

@mmurooka Thanks for approving and checking.

@mmurooka
Copy link
Member

Thank you for detailed explanation. I need more more training for deciding souce code policy..

@mmurooka
Copy link
Member

Maybe following parts related to min-max-table are no problem even if :init-pose is changed.
cc @snozawa

Yes. Only the following two part is NOT sample or test in the range I can find.
https://github.com/euslisp/jskeus/blob/master/irteus/irtmodel.l#L900
in irtmodel :make-joint-min-max-table method.
https://github.com/euslisp/jskeus/blob/master/irteus/irtmodel.l#L988
in irtmodel :plot-joint-min-max-table-common method.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@mmurooka mmurooka mmurooka approved these changes

Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
3 participants
@k-kimura @k-okada @mmurooka