The objective is to find collision free path from start to goal position even in the presence of multiple obstacles. RRT based (rapidly-exploring random tree) method is used to explore the collision free movement space.
ROS controller template and Elfin robot assets were adapted from https://github.com/modulabs/arm-control.
Implemented for this project:
Fig 1: Robot finding its way past the solid green obstacles.
Fig 1 demonstrates how the space is explored using two trees, one starting at start position and the other at goal. Connections between the trees' points are not visualized because in 3D these connections would be non-linear.
The green see-through field is the robot's approximate collision volume. This is the volume that is considered when computing collisions with the obstacles.
Solid green spheres are the obstacles that must be avoided when moving towards goal position. When collision free path is found it is forwarded to the motion controller and the robot begins moving.
Fig 2: Tree expansion in 2D. Animation from Wikipedia.
This project uses two trees: one begins at start position and the other at goal position. Both trees are expanded for set amount of iterations. Also known as bidirectional RRT.
Expansion is implemented by randomly generating candidate robot poses. The algorithm searches for its closest tree branch and tries to connect to it. The connection is tested for collission. If the whole connection is collision free then the candidate pose is added to the tree.
After the expansion phase an attempt to merge these two trees is made. If any collision free path is found between these trees the trees are merged to form a single tree and path from start to goal can be generated. Following this path guarantees collision free movement.
Install gazebo-ros-pkgs and gazebo-ros-control
$ sudo apt-get install ros-kinetic-gazebo-ros-pkgs ros-kinetic-gazebo-ros-control
Install effort-controllers to use torque-control interface
$ sudo apt-get install ros-kinetic-effort-controllers
Build project
$ catkin_make -DCMAKE_BUILD_TYPE=Debug
To build and run tests
$ catkin_make run_tests -DCMAKE_BUILD_TYPE=Debug
$ source devel/setup.bash
$ roslaunch elfin_launch manipulator_controller.launch
# In another terminal
$ source devel/setup.bash
$ roslaunch rrt_solver rrt.launch
# Set joint space target (joint degrees)
rostopic pub -1 /elfin/manipulator_controller/command_jog std_msgs/Float64MultiArray "{layout: {dim: [], data_offset: 0}, data: [0, 0, 120, 0, -30, 0]}"
# Target to get back to initial position
rostopic pub -1 /elfin/manipulator_controller/command_jog std_msgs/Float64MultiArray "{layout: {dim: [], data_offset: 0}, data: [0, 0, 0, 0, 0, 0]}"
此处可能存在不合适展示的内容,页面不予展示。您可通过相关编辑功能自查并修改。
如您确认内容无涉及 不当用语 / 纯广告导流 / 暴力 / 低俗色情 / 侵权 / 盗版 / 虚假 / 无价值内容或违法国家有关法律法规的内容,可点击提交进行申诉,我们将尽快为您处理。