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.

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

[pr2eus_moveit] Add timeout arg for computing IK and increase timeout for test #438

Merged
merged 2 commits into from
Jul 21, 2021

Conversation

708yamaguchi
Copy link
Member

@708yamaguchi 708yamaguchi commented May 14, 2020

In this pull request, I added timeout arg to some functions of robot-moveit.l and set the timeout in the travis test longer.

This pull request aims to prevent the travis test failure of pr2eus_moveit, such as:
https://travis-ci.org/github/jsk-ros-pkg/jsk_pr2eus/builds/674758416
https://travis-ci.org/github/jsk-ros-pkg/jsk_pr2eus/builds/686633237

These tests fail here:

(send *ri* :move-end-coords-plan (make-coords :pos #f(700 0 1000)) :move-arm :larm-torso)
(setq tm-0 (ros::time-now))
(send *ri* :wait-interpolation)
(setq tm-1 (ros::time-now))
(setq tm-diff (send (ros::time- tm-1 tm-0) :to-sec))
(ros::ros-info "time for duration ~A" tm-diff)
(assert (> tm-diff 3) (format nil "collsion avoidance motion is too fast ~A" tm-diff))

Error messages are as follows:

  <testcase name="test-move-end-coords-plan" status="run" time="15" classname="test-move-end-coords-plan">
   <failure message="collsion avoidance motion is too fast 0.001" type="AssertionError">
Test:(&gt; tm-diff 3)
Trace:";; ik error at (-31 . NO_IK_SOLUTION)

In detail, I think the reason is that

  1. IK calculation in :move-end-coords-plan fails
  2. PR2 moves the arm little by :move-end-coords-plan
  3. tm-diff becomes too short.

The reason why the IK calculation sometimes fails may be that its timeout is short.
In my environment, the IK calculation sometimes takes ~0.1 [s], but the default timeout is 0.05 [s] in robot-moveit.l.

I checked the IK calculation execution time by this code.
https://github.com/ros-planning/moveit/blob/50e3c21081c03a106ac1157d8a8687710bcc0398/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp#L409-L410

The default timeout in robot-moveit.l:

(frame-id default-frame-id) (timeout 0.05) (scene)

I am not confident, but I believe this pull requests improve success rate of the travis test.

By the way, I also tried to use attempts field in moveit_msgs/PositionIKRequest, but this field seems to be unavailable now.
moveit/moveit#1288

@knorth55
Copy link
Member

👍 for this PR.
good work!

@k-okada k-okada merged commit bedab5d into jsk-ros-pkg:master Jul 21, 2021
@708yamaguchi 708yamaguchi deleted the add-ik-timeout branch November 24, 2021 17:29
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

3 participants