Direct Kinematics control concept

[woensug-choi]

Initial commit for a 'Direct Kinematics control'

Status

Capabilities

• Read ROS topic (velocity, horizontal angle, vertical angle)
• Do not consider p,q,r rotations YET. (Prove of concept atm)
• GaussMarkovProcess noise usable (The source inherited capability of ocean current plugin)

Notes

• It does not use a hydrodynamic model plugin (the one this repository is trying to develop that includes added mass, dampings, thruster forces, etc) since the dynamics are to be calculated separately (maybe using python scripts with rospy?)

Start-up Command

roslaunch glider_hybrid_whoi_gazebo empty_underwater_world_kinematics.launch paused:=true
roslaunch glider_hybrid_whoi_gazebo start_demo_teleop_kinematics.launch joy_id:=0

Screenshots

Left top rqt_plot : the original ROStopic used for hydrodynamic model plugin Left bottom rqt_plot : NEW ROS topic used for kinematic control

Moving straight forward

rosservice call /kinematics/set_current_velocity "velocity: 0.1"

Moving down and up (vertical angle from -0.3 to +0.3)

rosservice call /kinematics/set_current_velocity "velocity: 0.5 
horizontal_angle: 0 
vertical_angle: -0.3"

rosservice call /kinematics/set_current_velocity "velocity: 0.5 
horizontal_angle: 0 
vertical_angle: 0.3"

Source code explanations

Developed code consists of two parts.

• Kinematics ROS plugin (ROS topic connector modified from the 'ocean current plugin')
• Direct kinematics plugin (Reads ROS topic and push the vehicle accordingly)

Kinematics ROS plugin

glider_hybrid_whoi/hybrid_glider_gazebo_plugin_kinematics/kinematics_plugins glider_hybrid_whoi/hybrid_glider_gazebo_plugin_kinematics/kinematics_ros_plugins glider_hybrid_whoi/hybrid_glider_gazebo_plugin_kinematics/kinematics_ros_plugins_msgs

The code is modified from the 'ocean current plugin' which is included in the UUV_Simulator. The original plugin takes velocity,horizontal_angle,vertical_angle as inputs, and publish them. The hydrodynamic plugin (that of UUV_Simulator and this repository's plugin) reads and gives affects as a ocean current with <flow_velocity_topic> tag. It inherited all the juice there is (including noise model).

The plugin is defined at world file and describes the topic name (e.g. kinematics/velocity_control) with <direct_kinematics_topic>

Direct kinematics plugin

glider_hybrid_whoi/hybrid_glider_gazebo_plugin_kinematics/kinematics_control_plugins

The code is developed by mixing the Gazebo tutorials (plugins_model, Control plugin).

The plugin is defined at the location where the vehicle is defined. (Which replaces the hydrodynamic plugin)

[woensug-choi]

• Added orientation (pitch and yaw) change according to velocity_control angle input (horizontal and vertical angle).
  • Roll not considered
• base_link_ned lookupTransform target_frame exists but the plugin works ok.
  • this msg could be related to sensors..

    [Msg] Transform between glider_hybrid_whoi/base_link_ned and glider_hybrid_whoi/base_link
    [Msg] "glider_hybrid_whoi/base_link_ned" passed to lookupTransform argument target_frame does not exist.

• SphericalCoordinatesROSInterfacePlugin exists (inherited from ocean current plugin) but not sure how to utilize it

[woensug-choi]

Major changes are required to close the gap between the current ocean current inputs basis (velocity, horizontal/vertical angle) and frl_vehicle_msg/UwGliderCommand basis (target pitch/heading, thruster power) structure. The current kinematics plugin cannot change its orientation without if velocity is zero. Thinking how to move forward... (Option 1. rewrite everything, Option 2. modify current plugin)

[bsb808]

The documentation on the PR is great. We should capture this as a tutorial wiki in this project - https://github.com/Field-Robotics-Lab/glider_hybrid_whoi/wiki

Please add documentation of the interface (topics, services, actions, etc.) Recommend following the template used in the ROS wiki - e.g., http://wiki.ros.org/microstrain_3dm_gx5_45 Note the Published/subscribed topics, services, etc. Both the topics/services and message types are included with an explanation of meanings of the variables.

[woensug-choi]

@bsb808

We might not need this at all. There's generic gazebo function that does this

Correct me if I misunderstood anything here.

I came to understand that this PR is unnecessarily complex. On my voyage to simplifications by reorganizing it from the ground up, I have found gazebo_msg/SetModelState.srv (link) and gazebo_msg/GetModelState.srv(link). I believe it does exactly what we are trying to do. You may try the manipulation on the updated master branch of this repository.

roslaunch glider_hybrid_whoi_gazebo empty_underwater_world.launch
roslaunch glider_hybrid_whoi_gazebo start_demo_teleop.launch joy_id:=0

rosservice call /gazebo/set_model_state '{model_state: { model_name: glider_hybrid_whoi, twist: { linear: {x: 2.0 , y: 0 ,z:0 } }} }'

you may also change orientation, position, and reference frame.

#e.g.
#rosservice call /gazebo/set_model_state '{model_state: { model_name: glider_hybrid_whoi, pose: { position: { x: 0, y: 0 ,z: 0 }, orientation: {x: 0, y: 0, z: 0, w: 0 } }, twist: { linear: {x: 0 , y: 0 ,z: 0 } , angular: { x: 0 , y: 0 , z: 0 } } , reference_frame: world } }'

My conclusion here is that we do not need any plugins to manipulate kinematically. The only required thing(not sure how to) is interface that connects model_state topics and our frl_msgs .

[bsb808]

@woensug-choi Well that would be great. As an existence proof, lets see if we can use the interface to do the following.

I think it would be ideal to set velocities (translational and rotational) through ROS interface.

Have an external ROS node that sends commands to the u/w glider so that the following happens:

1. The vehicle is spawned near the water surface.
2. The vehicle pitches down at a specified rate until it reaches a desired downward pitch
3. The vehicle moves forward (body frame) at a specified speed for a specified time.
4. The vehicle pitches up at specified rate util it reaches desired upward pitch, continuing at same forward speed.
5. Repeat

This would be a very basic dive cycle for the glider.