UniversalRobots / Universal_Robots_ROS_Driver

Universal Robots ROS driver supporting CB3 and e-Series
Apache License 2.0
741 stars 398 forks source link

The robot move slower via ros control #598

Closed rennico closed 1 year ago

rennico commented 1 year ago

Summary

I've succeed to control an UR10 robot via ROS-moveit with Universal_robot_ros_driver. After several tests I found that the robot was moving slower than its origin control (via UR program). Even if I set the same start and the same goal and both the paths are simple and quite similar. I have checked the joint_limit.yaml file and set the max_acceleration_limits to true with value: 6.28. But it seems make no changes.

In my code, I also use the setMaxAccelerationFactor(1) and setMaxVelocityScalingFactor(1) to ensure the scaling of joint speed is 100%. Do you have any clue what can cause that? Or it is just normal to move slower? Than you

Versions

Use Case and Setup

using UR10 with AZURE kinect to catch a moving object

fmauch commented 1 year ago

That could be linked with https://github.com/UniversalRobots/Universal_Robots_ROS2_Description/issues/46 (though that issue is about ROS2). Out of my head I am not sure where we set the acceleration limits, but those definitively influence the generated trajectories.

RobertWilbrandt commented 1 year ago

There is no inherent reason why moving through MoveIt should be slower. Do you have any more data to show for that? If you just want to verify the speed reachable through ROS, you can use a simpler forward controller and test your velocities and accelerations that way (see #595 as reference).

For MoveIt, one possible reasons for non-optimal accelerations is the use of the default time parametrization algorithm (see MoveIt documentation for reference). I just measured some velocities and accelerations while moving between two fixed poses as an example.

With the default time parametrization:

default_velocity default_acceleration

You can see how this parametrization tries to use some acceleration far beyond the limit, which could be one source of your problem. After changing to time optimal trajectory generation (can be set in ur10_moveit_config/launch/ompl_planning_pipeline.launch.xml):

top_velocity top_acceleration

Some small deviations from the optimal trajectory will always be present when using a sampling-based planner through moveit, you might also want to look at optimizing planners (either in OMPL or some other plugin like CHOMP/STOMP/TrajOpt...) if this is relevant to you.

In case you want to evaluate different choices for these i recommend using plotjuggler (as seen in the images). You can store joint states in the moveit rviz plugin under the "stored states" tab, which also helps.

If you can provide more detailed data on your movement we might find other reasons for the problem.

github-actions[bot] commented 1 year ago

This issue has not been updated for a long time. If no further updates are added, this will be closed automatically. Comment on the issue to prevent automatic closing.

github-actions[bot] commented 1 year ago

This issue has been closed due to inactivity. Feel free to comment or reopen if this is still relevant.