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

[pr2eus_moveit] fix typo in total-time condition #310

Merged
merged 1 commit into from
Jul 10, 2017
Merged

[pr2eus_moveit] fix typo in total-time condition #310

merged 1 commit into from
Jul 10, 2017

Conversation

knorth55
Copy link
Member

@knorth55 knorth55 commented Jul 9, 2017 •
edited 

       (when (and total-time ;; [msec]
                  (< orig-total-time
                     (/ total-time 1000)))

https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/0.3.10/pr2eus_moveit/euslisp/robot-moveit.l#L541-L543

@knorth55 knorth55 mentioned this pull request Jul 9, 2017
Closed
@k-okada k-okada merged commit 98f716d into jsk-ros-pkg:master Jul 10, 2017
@knorth55 knorth55 deleted the fix-typo-in-condition branch July 10, 2017 05:06
@k-okada
Copy link
Member

k-okada commented Jul 12, 2017

@knorth55 are you sure about this change ???? I think this is the cause of recent jsk_robot failure,

orig-total-time is execution time calculated by moveit
and total-time is given command,

and since moveit uses max velocity value from the urdf model, so we can assume the planned motion is the fastest motion, and if we specify the total time, then we'd like to follow the instruction, to avoid when human wrongly send small value for total time

       (when (and total-time ;; [msec]
                  (< orig-total-time
                     (/ total-time 1000)))

when total time is given and moveit output faster command then user command, then we'll use moveit command. but if moveit output slower command than user command, then robot moves moveit command speed

((or (null total-time) (> orig-total-time (/ total-time 1000))) (* orig-total-time 1000))

if total time is not given or moveit output is slower then user command ===> if total time is given and moveit output is faster than user command

then we'll use moveit command speed

@k-okada k-okada mentioned this pull request Jul 12, 2017
Closed
@knorth55
Copy link
Member Author

knorth55 commented Jul 12, 2017 •
edited

I thought so first, but I read old robot-moveit.l and :angle-vector-motion-plan prefer to use total-time arg than MoveIt! total-time.
Actually, sometimes MoveIt! returns really slow trajectory like 100 seconds even when I set velocity limit as max velocity written in URDF.

@k-okada k-okada mentioned this pull request Jul 12, 2017
Merged
@knorth55
Copy link
Member Author

@k-okada I dumped all rosparm https://gist.github.com/knorth55/0f3bdf1784a71dccac35d95e2baa0800
It looks like max_velocities are not overwritten.

@k-okada
Copy link
Member

k-okada commented Jul 12, 2017 via email

@knorth55
Copy link
Member Author

I thought moveit consider /robot_description_planning/joint_limits as max_verlocity, but does moveit also read URDF, too?

@knorth55
Copy link
Member Author

I move real robot with no gripper loaded URDF and SRDF, and moveit! trajectory is not slow.
Now I'm working on increasing max_velocity.

@k-okada k-okada mentioned this pull request Jul 13, 2017
Merged
@k-okada
Copy link
Member

k-okada commented Jul 13, 2017

note: it seems moveit trajectory is now slow at this moment, so now it's ok to revert this

k-okada added a commit that referenced this pull request Jul 13, 2017
@k-okada
Merge pull request #312 from k-okada/fix_310
a3060d9 
add test for #310 (comment)
@knorth55
Copy link
Member Author

It occurs again! MoveIt! trajectory is really slow.

@knorth55
Copy link
Member Author

I tried to do (send *ri* :angle-vector (send *baxter* :fold-pose-back :rarm) 3000 :rarm-head-controller 0), and it took 12 seconds.
video: https://drive.google.com/open?id=0B5DV6gwLHtyJYnhiQmNHZkg4OTQ

@k-okada
Copy link
Member

k-okada commented Jul 15, 2017 via email

@knorth55
Copy link
Member Author

[3] I dumped parameters when MoveIt! move properly.
https://gist.github.com/knorth55/8bbbd0f0ae35c6a4c4efcc6011ce8e5b

@k-okada
Copy link
Member

k-okada commented Jul 15, 2017 via email

@knorth55
Copy link
Member Author

[1] I restart all launches, but it takes 11 seconds for the trajectory. https://drive.google.com/open?id=0B5DV6gwLHtyJV0pWU1k5MGR6OVk

@knorth55
Copy link
Member Author

when you had slow motion, what's happens if you plan same motion again? if
this is stochastic behavior, then what we can do is just to reject such
plan and re-plan again before we send to joint trajectory controller.

I tried 5 times, but it takes 11-12 seconds every time.

8.irteusgl$ (send *ri* :angle-vector (send *baxter* :fold-pose-back) 3000 :rarm-controller 0)
[ INFO] [1500105235.365517256]: [/default_robot_interface_1500104945877121993] [ROSEUS_ROSINFO] ;; Planned Trajectory Total Time  11.502 [sec]
[ INFO] [1500105235.366184304]: [/default_robot_interface_1500104945877121993] [ROSEUS_ROSINFO] ;; Scaled Trajectory Total Time 11.512(11.502) [sec]
[ INFO] [1500105235.366266454]: [/default_robot_interface_1500104945877121993] [ROSEUS_ROSINFO] ;; generated 10 points for 11.5024 sec using [both_arms] group
[ INFO] [1500105235.366326736]: [/default_robot_interface_1500104945877121993] [ROSEUS_ROSINFO] ;; will send to (left_s0 left_s1 left_e0 left_e1 left_w0 left_w1 left_w2 left_gripper_prismatic_joint left_gripper_vacuum_pad_joint right_s0 right_s1 right_e0 right_e1 right_w0 right_w1 right_w2 right_gripper_prismatic_joint right_gripper_vacuum_pad_joint)
[ INFO] [1500105235.366544316]: [/default_robot_interface_1500104945877121993] [ROSEUS_ROSINFO] ;; send self :send-trajectory :rarm-controller
(#f(0.0 97.4707 -2.39502 -94.5483 134.67 91.4062 8.70117 0.0 0.0 0.0 -97.4707 -2.39502 94.5483 134.67 -91.4062 8.70117 0.0 0.0 0.0))
9.irteusgl$ (send *ri* :angle-vector (send *baxter* :fold-pose-back) 3000 :rarm-controller 0)
[ INFO] [1500105413.205499120]: [/default_robot_interface_1500104945877121993] [ROSEUS_ROSINFO] ;; Planned Trajectory Total Time  11.125 [sec]
[ INFO] [1500105413.206171256]: [/default_robot_interface_1500104945877121993] [ROSEUS_ROSINFO] ;; Scaled Trajectory Total Time 11.135(11.125) [sec]
[ INFO] [1500105413.206266439]: [/default_robot_interface_1500104945877121993] [ROSEUS_ROSINFO] ;; generated 10 points for 11.1247 sec using [both_arms] group
[ INFO] [1500105413.206335429]: [/default_robot_interface_1500104945877121993] [ROSEUS_ROSINFO] ;; will send to (left_s0 left_s1 left_e0 left_e1 left_w0 left_w1 left_w2 left_gripper_prismatic_joint left_gripper_vacuum_pad_joint right_s0 right_s1 right_e0 right_e1 right_w0 right_w1 right_w2 right_gripper_prismatic_joint right_gripper_vacuum_pad_joint)
[ INFO] [1500105413.206567699]: [/default_robot_interface_1500104945877121993] [ROSEUS_ROSINFO] ;; send self :send-trajectory :rarm-controller
(#f(0.0 97.4707 -2.39502 -94.5483 134.67 91.4062 8.70117 0.0 0.0 0.0 -97.4707 -2.39502 94.5483 134.67 -91.4062 8.70117 0.0 0.0 0.0))
10.irteusgl$ (send *ri* :angle-vector (send *baxter* :fold-pose-back) 3000 :rarm-controller 0)
[ INFO] [1500105433.811971018]: [/default_robot_interface_1500104945877121993] [ROSEUS_ROSINFO] ;; Planned Trajectory Total Time  12.203 [sec]
[ INFO] [1500105433.812660196]: [/default_robot_interface_1500104945877121993] [ROSEUS_ROSINFO] ;; Scaled Trajectory Total Time 12.213(12.203) [sec]
[ INFO] [1500105433.812746950]: [/default_robot_interface_1500104945877121993] [ROSEUS_ROSINFO] ;; generated 10 points for 12.2031 sec using [both_arms] group
[ INFO] [1500105433.812815665]: [/default_robot_interface_1500104945877121993] [ROSEUS_ROSINFO] ;; will send to (left_s0 left_s1 left_e0 left_e1 left_w0 left_w1 left_w2 left_gripper_prismatic_joint left_gripper_vacuum_pad_joint right_s0 right_s1 right_e0 right_e1 right_w0 right_w1 right_w2 right_gripper_prismatic_joint right_gripper_vacuum_pad_joint)
[ INFO] [1500105433.813035460]: [/default_robot_interface_1500104945877121993] [ROSEUS_ROSINFO] ;; send self :send-trajectory :rarm-controller
(#f(0.0 97.4707 -2.39502 -94.5483 134.67 91.4062 8.70117 0.0 0.0 0.0 -97.4707 -2.39502 94.5483 134.67 -91.4062 8.70117 0.0 0.0 0.0))
11.irteusgl$ (send *ri* :angle-vector (send *baxter* :fold-pose-back) 3000 :rarm-controller 0)
[ INFO] [1500105462.940100325]: [/default_robot_interface_1500104945877121993] [ROSEUS_ROSINFO] ;; Planned Trajectory Total Time  12.251 [sec]
[ INFO] [1500105462.940794776]: [/default_robot_interface_1500104945877121993] [ROSEUS_ROSINFO] ;; Scaled Trajectory Total Time 12.261(12.251) [sec]
[ INFO] [1500105462.940880417]: [/default_robot_interface_1500104945877121993] [ROSEUS_ROSINFO] ;; generated 10 points for 12.2514 sec using [both_arms] group
[ INFO] [1500105462.940950519]: [/default_robot_interface_1500104945877121993] [ROSEUS_ROSINFO] ;; will send to (left_s0 left_s1 left_e0 left_e1 left_w0 left_w1 left_w2 left_gripper_prismatic_joint left_gripper_vacuum_pad_joint right_s0 right_s1 right_e0 right_e1 right_w0 right_w1 right_w2 right_gripper_prismatic_joint right_gripper_vacuum_pad_joint)
[ INFO] [1500105462.941183257]: [/default_robot_interface_1500104945877121993] [ROSEUS_ROSINFO] ;; send self :send-trajectory :rarm-controller
(#f(0.0 97.4707 -2.39502 -94.5483 134.67 91.4062 8.70117 0.0 0.0 0.0 -97.4707 -2.39502 94.5483 134.67 -91.4062 8.70117 0.0 0.0 0.0))
12.irteusgl$ (send *ri* :angle-vector (send *baxter* :fold-pose-back) 3000 :rarm-controller 0)
[ INFO] [1500105508.396256157]: [/default_robot_interface_1500104945877121993] [ROSEUS_ROSINFO] ;; Planned Trajectory Total Time  11.296 [sec]
[ INFO] [1500105508.396936005]: [/default_robot_interface_1500104945877121993] [ROSEUS_ROSINFO] ;; Scaled Trajectory Total Time 11.306(11.296) [sec]
[ INFO] [1500105508.397021819]: [/default_robot_interface_1500104945877121993] [ROSEUS_ROSINFO] ;; generated 10 points for 11.2963 sec using [both_arms] group
[ INFO] [1500105508.397086436]: [/default_robot_interface_1500104945877121993] [ROSEUS_ROSINFO] ;; will send to (left_s0 left_s1 left_e0 left_e1 left_w0 left_w1 left_w2 left_gripper_prismatic_joint left_gripper_vacuum_pad_joint right_s0 right_s1 right_e0 right_e1 right_w0 right_w1 right_w2 right_gripper_prismatic_joint right_gripper_vacuum_pad_joint)
[ INFO] [1500105508.397300991]: [/default_robot_interface_1500104945877121993] [ROSEUS_ROSINFO] ;; send self :send-trajectory :rarm-controller
(#f(0.0 97.4707 -2.39502 -94.5483 134.67 91.4062 8.70117 0.0 0.0 0.0 -97.4707 -2.39502 94.5483 134.67 -91.4062 8.70117 0.0 0.0 0.0))
13.irteusgl$

@k-okada
Copy link
Member

k-okada commented Jul 15, 2017 via email

@k-okada
Copy link
Member

k-okada commented Jul 15, 2017 via email

@knorth55
Copy link
Member Author

knorth55 commented Jul 15, 2017 •
edited

I tested with no-gripper model, and moveit trajectory is 2-3 seconds.
https://drive.google.com/open?id=0B5DV6gwLHtyJSGNFN2tDc0hRWGc
https://gist.github.com/knorth55/007d3dc675f8879c4ce9434ca9600535

@knorth55
Copy link
Member Author

knorth55 commented Jul 15, 2017 •
edited

  1. restart all launch file and confirm if this happens again
  1. restart all launch file without no gripper loaded URDF and SRDF and check if this happens again

After i did [2], moveit works properly and trajectory is like 2-3 seconds.

2.irteusgl$ (send *ri* :angle-vector (send *baxter* :reset-pose :rarm) 3000 :rarm-controller 0)
[ INFO] [1500107143.047457906]: [/default_robot_interface_1500107113246331232] [ROSEUS_ROSINFO] ;; Planned Trajectory Total Time   1.684 [sec]
[ INFO] [1500107143.048016846]: [/default_robot_interface_1500107113246331232] [ROSEUS_ROSINFO] ;; Scaled Trajectory Total Time 3.010(3.000) [sec]
[ INFO] [1500107143.048095209]: [/default_robot_interface_1500107113246331232] [ROSEUS_ROSINFO] ;; generated 10 points for 3.0 sec using [both_arms] group
[ INFO] [1500107143.048157291]: [/default_robot_interface_1500107113246331232] [ROSEUS_ROSINFO] ;; will send to (left_s0 left_s1 left_e0 left_e1 left_w0 left_w1 left_w2 left_gripper_prismatic_joint left_gripper_vacuum_pad_joint right_s0 right_s1 right_e0 right_e1 right_w0 right_w1 right_w2 right_gripper_prismatic_joint right_gripper_vacuum_pad_joint)
[ INFO] [1500107143.048328366]: [/default_robot_interface_1500107113246331232] [ROSEUS_ROSINFO] ;; send self :send-trajectory :rarm-controller
(#f(71.1475 97.4048 -2.43896 -94.5264 134.67 91.3623 8.9209 -0.021973 0.515418 0.0 -20.0 -25.0 40.0 60.0 20.0 80.0 0.0 0.0 0.0))
3.irteusgl$ (send *ri* :angle-vector (send *baxter* :fold-pose-back) 3000 :rarm-controller 0)
[ INFO] [1500107162.003263880]: [/default_robot_interface_1500107113246331232] [ROSEUS_ROSINFO] ;; Planned Trajectory Total Time   2.006 [sec]
[ INFO] [1500107162.003834693]: [/default_robot_interface_1500107113246331232] [ROSEUS_ROSINFO] ;; Scaled Trajectory Total Time 3.010(3.000) [sec]
[ INFO] [1500107162.003920688]: [/default_robot_interface_1500107113246331232] [ROSEUS_ROSINFO] ;; generated 10 points for 3.0 sec using [both_arms] group
[ INFO] [1500107162.003986814]: [/default_robot_interface_1500107113246331232] [ROSEUS_ROSINFO] ;; will send to (left_s0 left_s1 left_e0 left_e1 left_w0 left_w1 left_w2 left_gripper_prismatic_joint left_gripper_vacuum_pad_joint right_s0 right_s1 right_e0 right_e1 right_w0 right_w1 right_w2 right_gripper_prismatic_joint right_gripper_vacuum_pad_joint)
[ INFO] [1500107162.004245342]: [/default_robot_interface_1500107113246331232] [ROSEUS_ROSINFO] ;; send self :send-trajectory :rarm-controller
(#f(0.0 97.4707 -2.39502 -94.5483 134.67 91.4062 8.70117 0.0 0.0 0.0 -97.4707 -2.39502 94.5483 134.67 -91.4062 8.70117 0.0 0.0 0.0))
4.irteusgl$

dumped rosparam : https://gist.github.com/knorth55/0802aa9926ee6d403226a75f63615f2f

@knorth55
Copy link
Member Author

I diff dumped rosparam.

└─▪ diff rosparam_dump_201707151700.txt  rosparam_dump_201707151734.txt
2403,2404c2403,2404
<               max_angle: 1023, max_torque: 1.845, max_velocity: 10.411761640875001,
<               min_angle: 0, model_name: AX-18, model_number: 18, radians_per_encoder_tick: 0.0051132692929521375,
---
>               max_angle: 1023, max_torque: 1.815, max_velocity: 10.242464703625, min_angle: 0,
>               model_name: AX-18, model_number: 18, radians_per_encoder_tick: 0.0051132692929521375,
2445c2445
<           motor: {id: 1, init: 194, max: -3106, min: 194}
---
>           motor: {id: 1, init: 187, max: -3113, min: 187}
2483c2483
<               max_angle: 4095, max_torque: 2.5208333333333335, max_velocity: 5.807583079374999,
---
>               max_angle: 4095, max_torque: 2.5416666666666665, max_velocity: 5.855579633749999,
2532c2532
<           motor: {id: 1, init: 3623, max: 6923, min: 3623}
---
>           motor: {id: 1, init: 3638, max: 6938, min: 3638}
2828a2829,2830
> mongo_wrapper_ros_sheeta_89779_4657280310112146096: {database_path: /home/knorth55/ros/indigo/src/FOR_MOVEIT/moveit_robots/baxter/baxter_moveit_config/default_warehouse_mongo_db,
>   overwrite: false}
10487c10489,10490
<     right_s0: {has_acceleration_limits: true, has_velocity_limits: true, max_velocity: 1.5}
---
>     right_s0: {has_acceleration_limits: true, has_velocity_limits: true, max_acceleration: 2.0,
>       max_velocity: 1.5}
11967c11970,11972
<     host_133_11_216_160__45154: 'http://133.11.216.160:45154/', host_133_11_216_160__56583: 'http://133.11.216.160:56583/',
---
>     host_133_11_216_160__45154: 'http://133.11.216.160:45154/', host_133_11_216_160__46578: 'http://133.11.216.160:46578/',
>     host_133_11_216_160__47191: 'http://133.11.216.160:47191/', host_133_11_216_160__47293: 'http://133.11.216.160:47293/',
>     host_133_11_216_160__55362: 'http://133.11.216.160:55362/', host_133_11_216_160__56583: 'http://133.11.216.160:56583/',
11982a11988,11994
> rviz_sheeta_89779_1634722453128040910:
>   left_arm: {kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin, kinematics_solver_attempts: 3,
>     kinematics_solver_search_resolution: 0.005, kinematics_solver_timeout: 0.005}
>   motionplanning_planning_scene_monitor: {publish_geometry_updates: true, publish_planning_scene: false,
>     publish_planning_scene_hz: 4.0, publish_state_updates: false, publish_transforms_updates: false}
>   right_arm: {kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin, kinematics_solver_attempts: 3,
>     kinematics_solver_search_resolution: 0.005, kinematics_solver_timeout: 0.005}
12286c12298,12301
<   types: [item_location, order]
\ No newline at end of file
---
>   types: [item_location, order]
> warehouse_exec: mongod
> warehouse_host: localhost
> warehouse_port: 33829

and I found a big diff!

10487c10489,10490
<     right_s0: {has_acceleration_limits: true, has_velocity_limits: true, max_velocity: 1.5}
---
>     right_s0: {has_acceleration_limits: true, has_velocity_limits: true, max_acceleration: 2.0,
>       max_velocity: 1.5}

When I set this param correctly, moveit trajectory time is 2-3 seconds!.

@knorth55 knorth55 mentioned this pull request Jul 15, 2017
Merged
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers
No reviews
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.

None yet
2 participants
@knorth55 @k-okada