Import drag calculation into Trajectory Manager

[agutier4]

Overview

Import the prototype drag calculations from pongrobot/pongrobot#2 into the Trajectory Manger. Refactor the existing code to allow users to enable/disable the drag calculation using a config flag. This will likely involve breaking the existing visualization code into a function and creating another version that includes the drag equations.

Issue dependencies:

Depends on pongrobot/pongrobot#2 which implements the prototype solver in MATLAB.

Solution Approach:

The code for the trajectory calculation all lives in src/TrajectoryManager.cpp and include/TrajectoryManager.h. The TrajectoryManager is an object that handles all operation in the trajectory_manager_node. It takes in a position command from that main game node, calculates the launcher pitch from the TF if available and then uses that fixed launch angle to calculate the initial velocity. It also calculates the yaw position needed and solves the trajectory in cylindrical coordinates. The code should be setup so drag is controlled through a config value.

Step 1 - Config setup and Refactoring

The first step is setting up the TrajectoryManager to enable drag through a config. Start be adding a new config value to launcher_config.yaml and add a variable to the TrajectoryManager to store it in once read. Next, create a new function for the velocity calculation with drag TrajectoryManager::calculateInitialVelocityDrag() and setup TrajectoryManager::trajectoryPoseCallback() to switch between the two velocity calculation functions based on the configuration. Repeat the same process for TrajectoryManager::buildTrajectoryMarker() adding TrajectoryManager::buildTrajectoryMarkerDrag().

Step 2 - Redo Initial Velocity Calculation

Now that TrajectoryManager::calculateInitialVelocityDrag(), it needs to be populated with the drag calculation from the prototype. This self contained function has access to all the data it needs ( tf, target pose, launch angle) and just needs to return a velocity in m/s. It must also set the contact_time_ member variable with the time the ball hits the cup, this is used as an endpoint for the simulation in the next step.

Step 3 - Simulate the Shot for Visualization

The simulation/visualization code all lives in TrajectoryManager::buildTrajectoryMarker[Drag]() and basically creates a list of points which ROS uses to create a line marker. The way this is currently done is by finding the contact time (done in step 2) then diving the time it takes up into a fixed number of steps (i.e. 100) and then plugging in the time into the equations of motion to find the position and create a point from it. This is easiest when done in cylindrical coordinates and can then be converted into Cartesian coordinates from plotting.

Additional Considerations

Without having looked too hard at the initial MATLAB prototype, it sounds like there is a need for a Non-Linear solver library. These might not be the right fit but its a starting place:

• Eigen-Unsupported Non-Linear Optimization Module
• ifopt ROS wrapper

[agutier4]

@benmartell93 a few notes:

• I updated the CMakeLists.txtto pull in the Eigen libraries to build correctly with the default Eigen3 installation from apt, make sure everything still builds for you
• I've updated manager.launch so now it should load the correct reference frame for rviz by default. It also now has options to enable the visualization and drag with args
• I pulled out a few unnecessary test code and cleaned up a few formatting issues, we can do a better pass at this when we go to merge this back
• When I ran a test with the drag enabled, the trajectory manager died, likely because the eigen solver failed to find a solution to the drag equation. I left the drag disabled by default so you can test the workflow first

Here is the test procedure I would recommend (both with and without graphics enabled)

Trajectory Testing (with graphics)

1. Navigate to your catkin_ws
2. Make sure your code is build and the workspace has be correctly sourced (catkin_make & . source devel/setup.zsh)
3. Launch the trajectory manager launchfile roslaunch pongrobot_actuation manager.launch, rviz should open with the trajectory visualization config
4. In a new shell, run rostopic echo /launcher/velocity_cmd to subscribe to the velocity output command
5. In yet another shell use rostopic pub to send a target command to /launcher/target_pose, make sure to set the frame_id to launcher and edit the x&y position as desired.
6. The TrajectoryManager should log messages showing it has received the command and calculated the correct velocity. Rviz should have rendered the target pose with trajectory, and the velocity command subscriber from step 4 should echothe calculated velocity
7. To run with drag, replace the command from step 3 with roslaunch pongrobot_actuation manager.launch use_drag:=true

Note: If your system does not have graphics setup, you can disable the rviz trajectory visualization by running the manager launchfile with the use_graphics:=false argument

Example command for drag and no graphics roslaunch pongrobot_actuation manager.launch use_drag:=true use_graphics:=false