Assignment 8: VBAP, single USV tuning and multiple (2 ...
Transcript of Assignment 8: VBAP, single USV tuning and multiple (2 ...
1. 2. 3.
Assignment 8: VBAP, single USV tuning and multiple (2) USVs (21-3)
References and Prerequisites
You're expected to work through (actually do the tutorials on your computer!) before doing the exercises below!
ROS
Names: Read the following documentation. Names will become very important when dealing with multiple robots. We will use ROS Namespacesto compartmentalize the ROS network for each robot.
ROS Names - documentationA Gentle Introduction to ROS - a Chapter 5: Graph Resource Names Roslaunch tag - notice the namespace (ns) attribute <group>
Cloudsim
Persistent Storage: Setting Executable Bit / Permissions - for convenience.
Waypoint Guidance
The book Robotics, Vision and Control, by Peter Corke is available electronically through the NPS library
Go to the library website, search for the title, and follow the links to download a PDF of the bookSee Chapter 4: Mobile Robot Vehicles
Section 4.2.1, Moving to a Point describes the waypoint guidance method we will use.
Goal
The objective of this assignment is to implement a potential field based guidance algorithm based on the virtual body and artificial potentials (VBAP) described in "Cooperative Control of Mobile Sensor Networks: Adaptive Gradient Climbing in a Distributed Environment". This week we should be able to...
Tune the single-leader, single-vehicle MATLAB implementation.Setup a two-vehicle case with low-level control for each USV.Extend our VBAP implementation for a "Collective" scenario, where each vehicle is attracted to the virtual leader (rabbit), but now aware of the other vehicles.
4.
1. 2.
Next Week: Extend the VBAP implementation to a "Cooperative" scenario, where the vehicles attempt to maintain a safe distance from each other.
Source: Parker, Distributed Intelligence
Assignment Setup
Assignment on Github
As with Assignment 1, this assignment will be submitted as a branch of the github repository for the assignment in the MultiRobotControl organization: https://github.com/MultiRobotControlEach student should work only in their branch of the repository.
Clone your repository to the src directory within your catkin workspace and make it a ROS package. Add the CMakelists.txt and package.xml files to your git repository.
Update Packages
There are new additions to the package, so you will need to the latest changes from the github remote to your local copy.mrc_examples pull
# Move into the directory where you have mrc_examplescd catkin_ws/src/mrc_examples# Pull changes from remote (origin) to your local copygit pull origin main
Exercise 1: VBAP SLSV Tuning
An oft quoted approach to developing software that applies to many other development scenarios, such as developing control systems, is Make it work, (see the associated article, )., . make it right, make it fast Kent Beck "Coding faster: Make it work, then make it good"
In the previous assignment you should have made the VBAP SLSV algorithm work - now we'll work on making it right/fast/good.
Setup
Copy over the pertinent MATLAB files from the previous exercise to your new repository. In put the following files:ay21_hw8/matlab
vbap.m - The main script/program to setup the callbacks, call the algorithm, etc.vbap_slsv.m - Implementation of the single leader, single vehicle VBAP algorithm as a function.rabbit_callback.m - Callback function for virtual leader position subscriber.usv_odom_callback.m - Callback function for vehicle navigation subscriber.
Create a new launch file: that is based on the from the previous ay21_hw8/launch/vbap_slsv.launch cora_closedloop.launchassignment. This launch file should do the following:
Include the to start Gazebo with the marina world and spawn our single CoRa USV.mrc_examples/launch/vorc_marina.launchStart the node with your PID gain values from previous assignments. Or, you can use these gain values.usv_control_diff_drive.py
<!-- Surge --> <param name="velKp" value="1.1"/>
2.
3.
4.
<param name="velKi" value="0.0"/> <param name="velKd" value="0.0"/> <!-- Yaw Rate --> <param name="yawKp" value="2.5"/> <param name="yawKi" value="1.0"/> <param name="yawKd" value="0.0"/>
Spawn the model described by the description (see mrc_examples/urdf/rabbit.urdf mrc_examples/launch/rabbit_w_visual. for an example)launch
Start the node to connect the rabbit publication to update the red sphere rabbit visualization (again see pointstamped2modelstate.py mrc_ for an example)examples/launch/rabbit_w_visual.launch
You should have a scenario similar to the image below. Note that the rabbit starts at (0, 0) which is top-center of the image.
Modify Algorithm and Scenario
To simplify the virtual leader portion of the algorithm, we will use the Moving to a Point algorithm from P. Corke discussed above. This is simpler to tune as it only has two gain parameters:
k_h: The yaw-rate (heading) gaink_v: The surge gain (velocity) gain.
Video Examples, Tuning and Debugging:
Moving the rabbit using rostopic pub, turning off control output to verify outputMoving the USV (instead of the rabbit), translation and rotation, tuning the heading portion of the waypoint guidanceTuning the surge component of waypoint guidance with stationary virtual leader.Running the rabbit to verify waypoint guidance.Following the rabbit - two scenarios - 8x speedup
Change the rabbit waypoints to a 100x100 m box:
1. 2. 3.
# 2D Array of waypointswaypoints: - [10.0 , -372.0, 0.0] - [110.0 , -372.0, 0.0] - [110.0 , -272.0, 0.0] - [10.0 , -272.0, 0.0]
Deliverable
When you have a working version...
Record a bag file that inlcudesThe rabbit positionsThe USV odometry (pose and velocity)The commanded twist message sent to the low-level controller.
Process the bag file with MATLAB to generate the same figures from the previous assignment, i.e.,A plot of the x vs. y positions of the rabbit and the USV. Save as ay21_hw8/images/vbap_slsv_xy.pngA plot with two subplots - upper subplot of x (rabbit and USV) vs. time, lower subplot of y (rabbit and USV) vs time. Save ay21_hw8/images/vbap_slsv_xy_time.png
Exercise 2: Multiple (N=2) USVs with Low-Level Control
Working Example
As a starting point, you should be able to run the following launch file
roslaunch mrc_examples marina_2x.launch verbose:=true
To generate a simulation scenario described by the following two images:
Add Low-Level (Autopilot) Control for Each USV
Copy the working example file into your assignment repository as ay21_hw8/launch/marina_2x.launch
Edit the launch file to
Start two instances of the usv_control_diff_drive.py node, with your PID gains, within the namespace of each USV (cora1 and cora2)Spawn the model described by the description (see mrc_examples/urdf/rabbit.urdf mrc_examples/launch/rabbit_w_visual.
for an example)launchStart the node to connect the rabbit publication to update the red sphere rabbit visualization (again see pointstamped2modelstate.py mrc_
for an example)examples/launch/rabbit_w_visual.launch
Exercise 3: Collective VBAP Now that we have two independent USV's in the simulation, cora1 and cora2, we will extend the MATLAB implementation of the VBAP algorithm to service two USVs simultaneously. To start with the two vessels will independently pursue the virtual leader / rabbit and make no effort to avoid each other.
Start with the program you created for the SLSV case. Copy it as The program should...matlab/vbap.m matlab/vbap_2x.mSubscribe to the navigation information for both USVs. (You can create two separate callback functions, or have both subscribers call the same callback function.)Subscribe to the rabbit position informationRun the VBAP algorithm for each USV to have each vessel follow the rabbit. The VBAP algorithm should be unchanged from Exercise 1Publish Twist commands to both USVs
The final architecture should look something like this...
.Video: VBAP, single leader, two vehicle, collective demonstration at 4x speed
Once you have working solution,
record a bag file with position information for both USVs and the rabbit, e.g.,
rosbag record /cora1/cora/sensors/p3d /cora2/cora/sensors/p3d /rabbit /cora1/cora/cmd_vel /cora2/cora/cmd_vel
Process the bag file with MATLAB to generate the same figures from the previous assignment, i.e.,A plot of the x vs. y positions of the rabbit and both USVs. Save as ay21_hw8/images/vbap_sltv_xy.pngA plot with two subplots - upper subplot of x (rabbit and both USVs) vs. time, lower subplot of y (rabbit and USV) vs time. Save ay21_hw8/images/vbap_sltv_xy_time.png