Turning BlueROV into Spiderman

Hi everyone,

If you read the title and thought “but why??” - let me give you the answer: for science!
However, it doesn’t quite work yet and I would appreciate if someone could point me into the right direction.

In a nutshell, we are trying to modify a BlueROV2 Heavy so that it can drive around on vertical surfaces.
(In our case, on the towers of offshore wind turbines and on retaining walls of harbour basins. These things need to be coated with protective paint every once in a while and the current solution of divers going down there and doing it by hand is both dangerous and expensive. So we’re researching ways to automate this.)

We are building a crawler with wheels that will be mounted underneath the ROV and that will be used to drive around and spread the paint. For this setup to work, the ROV will need to turn (pitch upwards) by 90 degrees and then engage the four downwards thrusters (full throttle). That way, it will stick to the vertical surface like Spiderman!

I am currently trying to find a way to modify the ArduSub code so that the ROV can be moved into the target position by the push of a button, after a human drives it to a location. I have looked at joystick.cpp and tried a few things but nothing has worked so far and I feel like I’m missing a very basic step somewhere.

Approach 1: set target pitch, then engage thrusters

case JSButton::button_function_t::k_custom_1:
    if (!held) {
        attitude_control.input_angle_step_bf_roll_pitch_yaw(0.0F, 90.0F, 0.0F);
        motors.set_throttle(1.0F);
    }
    break;

This will briefly engage the four top thrusters (5,6,7,8) but nothing else happens.

Approach 2: use stabilize mode to keep pitch, then set target position to 10 cm below ROV
(I first tried depth_hold mode and setting a target depth but that uses a “global” depth instead of the ROV as reference frame.)

case JSButton::button_function_t::k_custom_2:
    // user will need to pitch ROV first
    if (!held) {
        set_mode(STABILIZE, MODE_REASON_TX_COMMAND);
        Vector3f v(0.0F, 0.0F, 10.0F);
        pos_control.set_pos_target(inertial_nav.get_position() - v);
    }
    break;

This will move the ROV back to its original (horizontal) position but nothing else happens.

Approach 3: use attitute control

case JSButton::button_function_t::k_custom_3:
    if (!held) {
        attitude_control.rate_bf_pitch_target(90.0F);
        attitude_control.set_throttle_out(1.0F, false, g.throttle_filt);
    }
    break;

Nothing happens with this one either.

Approach 4 would be to implement a new mode or modify an existing mode, but I thought I’d ask here first if that would be more fruitful than my first three approaches.

Now I am out of ideas. There seem to be different functions doing essentially the same thing (setting throttle via motors or attitute control, for example). But I can’t decide which one will solve my problem. Is there a simple and efficient way to do this?

4 Likes

Hi @monsterbacke,

That’s a great application and a really hope to help with that!

First, I need to know which ArduPilot version are you using, some features may be necessary in the newer versions to accomplish what you want.

I’m not sure about input_angle_step_bf_roll_pitch_yaw, maybe @williangalvani can explain it better.
In general it should be control the angle as a step in body-frame, but I’m not sure if this should be used only in autotune mode or in another specific user case.

It appears to be a valid approach, but there are some things that are a bit weird in your code.

set_mode does not receive a second argument (MODE_REASON_TX_COMMAND), and set_pos_target does not exist in ArduPilot source (master branch).
And, the most important, STABILIZE mode does not have altitude control, only attitude control, so if you want to control depth and attitude, you should be using DEPTH HOLD.

You can also accomplish it via MAVLink protocol, with the SET_ATTITUDE_TARGET.

You can check the attitude control in depth hold here:
https://github.com/ardupilot/ardupilot/blob/master/ArduSub/control_althold.cpp#L53-L68

And to move the ROV 10cm up, you should be able to accomplish it with the DEPTH_HOLD mode with:

pos_control.set_pos_target_z_cm(inertial_nav.get_altitude() + 10.0f);

With that said, this approach may be the best option.

There are also a couple of things wrong here.

rate_bf_pitch_target is a rate command, so it’ll not set the ROV in 90º, but move the ROV at a rate of 0.9º/second.

Again, I recommend to use the approach 2, with attitude_control.input_euler_angle_roll_pitch_yaw.

That’s also a good approach if you take your time to understand and learn more about ArduPilot source code.

Some final considerations:

  • Be aware that you may face Gimbal Lock problems
  • Take your time with SITL and some other tools to help you test in a virtual environment, maybe bluesim can help or gazebo.
  • You may be able to accomplish the same thing via mavlink, but if you want to have everything in the ROV firmware, it’s also possible.
    • Check depth_hold mode, it’ll probably help more since have both attitude and altitude control.
1 Like

Hi @monsterbacke ,

That sounds fun! I would try using the attitude control example we have in pymavlink to point the ROV up. Be careful around 90º of pitch because you may experience issues with gimbal lock when using euler angles.

I would do it like this:

  1. Drive the rov to the structure you want to “attach” to.
  2. Use pymavlink to set put it in stabilize and command an attitude of 80º pitch up so you don’t hit the gimbal lock issue. Note that these messages have a timeout, so you would need to send them continuously until you hit the wall.
  3. Either manually or via a script, send manual_control messages to set negative throttle. This should move the rov towards the wall.
  4. Switch to manual mode and keep throttle negative. This way the rov won’t try to control attitude anymore, and you should be able to control it freely with your wheels.

We have an example for controlling both depth and attitude. you can comment the depth part out.

2 Likes

It may also be worth following the progress of this post, which seems to be trying to do something similar :slight_smile:

1 Like

Thank you everyone for your suggestions! :slight_smile:

Once a fortnight or so I am pulling the latest ArduSub-stable from GitHub, so I hope I have the newest features available. But looking at your suggestion about set_pos_target_z_cm, maybe I should be pulling master instead? :thinking: Let me know please.

I copied this way of calling set_mode from joystick.cpp:

But I am happy to leave out the second argument if it is not required.

Okay, I will experiment into this direction some more then. I have tried using DEPTH HOLD mode before and I set the target depth to 10 cm below the current depth in the same way that you suggested:

pos_control.set_pos_target_z_cm(inertial_nav.get_altitude() - 10.0f);

However, the result of this was that the ROV moved towards the floor of our testing basin, not towards the wall. So I assumed this approach would not work for our purpose.

Good to know, thanks! So this one is out then.

Okay, I will do that then and report back once I have the opportunity to test this on the ROV.

Good point, I do expect we will be working with near 90° angles most of the time, given that we want to drive around on vertical surfaces. Once we are in that position, we will use our crawling skid to move around, but any locked axes might of course become an issue when we want to turn the ROV back into its standard (horizontal) position and swim back to shore.

I thought about this as well and found @williangalvani’s reply really helpful on this topic. I already use pymavlink and ROS to drive the crawling skid using the two trigger buttons on the back of our controller. So I could just extend my code there. The only reason I tried to implement this “spiderman function” in joystick.cpp was that I did not want to interfere with any existing button assignments in QGC (in case some future user forgets to disable the button in QGC.) So Approach 5 will be pymavlink.

Approach 6
I found a file called UserCode.cpp and thought maybe this might be useful for my purpose. What do you think? Is there a dev guide or example for this in ArduSub or can I re-use what I found related to ArduCopter?

Ah I think I understood this now. So in order to avoid gimbal lock, I could change the orientation of the board + IMU inside the ROV? Or simply set the AHRS_ORIENTATION parameter to Roll90Pitch315?

I believe either of those approaches should work.

From what I understand, internally ArduSub uses quaternions instead of Euler angles, so the firmware itself can’t experience gimbal lock. The issues come about when commanding Euler angles in or reading them out, because the conversion reintroduces the gimbal lock issues. To handle that you can effectively lie to the autopilot about the orientation it’s in, so that you shift the gimbal lock to orientations you don’t expect to operate in, which takes it away from those you do (horizontal and vertical).

I just tried setting Roll90Pitch315 and the vehicle by default shows an orientation of 45, and straight down shows negative 45. Going straight upwards goes through gimbal lock at 45 degrees on the way to pointing upwards, so if you want to be operating vertically upwards then you either need to rotate the Pixhawk instead, or need to add a custom orientation of Roll90Pitch45 to your ArduSub build. Hope that’s helpful :slight_smile:

1 Like

Hello everyone,

So today I got some time with the robot in our water basin and I was able to test a few things. I focused on using only ArduSub for now.

Test 1: set throttle to stick to the wall

case JSButton::button_function_t::k_custom_1:
    if (held) {
        motors.set_throttle(1.0F); 
    }
    break;

This worked so-so. When I kept pressing the assigned button, the four horizontal thrusters kept spinning, although not with full speed and not simultaneously. The four thrusters did not spin all the time, more like half the time (quickly alternating between spinning and stopping), and two thrusters spun more slowly than the other two for no obvious reason. It seems like Sub::handle_jsbutton_press() is not called with high enough frequency for this to work permanently.
But in any case, my intention was that I would just push the button once and the throttle thrusters would keep spinning at full speed until I push another button to turn them off again. So this does not seem like my pathway to success.

By the way, what is the difference between
motors.set_throttle(1.0F);
and
attitude_control.set_throttle_out(1.0F, false, g.throttle_filt);
?
I tried both versions with exactly the same result as decribed above, so I’m wondering if they function the same in the background.

Test 2: set pitch to turn vehicle

case JSButton::button_function_t::k_custom_2:
    if (held) {
        set_mode(STABILIZE, MODE_REASON_TX_COMMAND);  //copied from line 163
        attitude_control.input_euler_angle_roll_pitch_yaw(0.0F, 80.0F, 0.0F, false);
    }
    break;

This also did not work as expected. When I kept pressing the button, the ROV rolled very slightly to the left, reaching an angle of maybe 10 degrees. There was no movement along the pitch axis. Again in the same manner as in the first test with the throttle, the thrusters did not spin with full speed, however, due to STABILIZE mode, the ROV held the 10 degrees angle when I let go of the button.

I also tried setting the pitch using the Quaternion function
attitude_control.input_quaternion(Quaternion(sqrtf(0.5F), 0.0F, 0.0F, sqrtf(0.5F)));
but this behaved in the same way as the euler angle function.

Test 3: manual “docking”, then STABILZE

We drove the ROV to our wall in MANUAL mode, toggled the roll-pitch button and held both sticks on the controller down, so that the ROV was vertical and pushing into the wall as intended. Then I changed into STABILIZE mode and let go of the sticks. The ROV stayed in roughly the same position - it did not hold the pitch angle of 90 degrees, more like 80 degrees - however, it stopped pushing into the wall. We tried exactly the same thing with DEPTH_HOLD mode and got exactly the same behaviour. So for our purpose, neither of these approaches will not work unless we find a way to also make the ROV move towards the wall continuously.

Conclusion
I am giving up on joystick.cpp and will try pymavlink next, as described by @williangalvani in his answer. If that doesn’t work either, I will try making use of UserCode.cpp or implement a new mode.

Hello everyone,

Today, I have finally found a solution to this: I have overwritten Acrobatics Mode with my own code (because we weren’t using it anyways and it seemed closest to what we were trying to achieve). This is now my version of Sub::acro_run() which is located in control_acro.cpp.

// acro_run - runs the acro controller
// should be called at 100hz or more
void Sub::acro_run()
{
    // if not armed set throttle to zero and exit immediately
    if (!motors.armed()) {
        motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
        attitude_control.set_throttle_out(0,true,g.throttle_filt);
        attitude_control.relax_attitude_controllers();
        return;
    }

    motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

    if (pitch_and_dock) {
        //we're still trying to maneuver ourselves into the correct position
        //pitch to 90 degrees and let the ROV move around
        attitude_control.input_euler_angle_roll_pitch_yaw(0.0F, 8900.0F, 0.0F, true);
        motors.set_throttle(channel_throttle->norm_input());
        motors.set_forward(channel_forward->norm_input());
        motors.set_lateral(channel_lateral->norm_input());
    } else {
        //we are ready to dock to the wall
        motors.set_throttle(0.5 - (gain/2));
    }

    //set speed of direction via these functions
    //motors.set_throttle(0.5F); //neutral throttle
    //motors.set_forward(0.0F);  //positive is forward, negative is backward
    //motors.set_lateral(0.0F);  //positive is right, negative is left

    //setting roll/pitch/yaw via motors functions does NOT work
    //they seem to ignore the values and instead send the ROV shooting downwards
    //motors.set_roll(0.0F);
    //motors.set_pitch(0.0F);
    //motors.set_yaw(0.0F);

    //attempt to set pitch, avoid gimbal lock
    //attitude_control.input_euler_angle_roll_pitch_yaw(0.0F, 89.0F, 0.0F, true);
    //apparently it wants degrees * 100 for some reason
    //attitude_control.input_euler_angle_roll_pitch_yaw(0.0F, 8900.0F, 0.0F, true);
}

I have also added a bool pitch_and_dock to Sub.h that gets toggled in case JSButton::button_function_t::k_custom_1 of joystick.cpp.

If anybody wants to play around with this in the future, here is a list of things that didn’t work:

  • implementing a custom function in joystick.cpp - because it is not called frequently enough to keep the thrusters spinning continuously.
  • implementing UserCode.cpp - because it seems to compete with whatever mode is running to control the ROV.
  • using pymavlink - because it does not want to run at the same time as QGC, even if I change the UDP port.
  • using any other mode - see my answer above.

Hope this helps :slight_smile:

4 Likes

Hi Elliot, quick question, we want to modify the ardusub code based on the post you kindly attach, could you give us a brief intro on how to do it? For what i know, the ardusub code is on the Pixhawk, but i see a folder on the Raspberry containing a companion code. Only modifying it on the Pixhawk will give us the changes we want?, should the code be compiled (or the Pixhawk does it)? Thanks.

Hi @SaferHarbors,

Please see our ArduSub developer documentation :slight_smile:

The Raspberry Pi runs BlueOS (or our old Companion Software).
The Pixhawk runs ArduSub firmware.
They are independent of each other.

The Pixhawk is not capable of compiling / building its own firmware - it can only run it.

To run a modified version of ArduSub you will need to follow the developer instructions (generally on your topside computer) to get the code, set up the build environment, make your modifications, build the firmware, and upload the compiled result to your Pixhawk.

1 Like