Implemented the weighted pseudo inverse.

[rafaelsaback]

I've implemented this new method that is quite similar to the previous svd, but now we can set weights for the thrusters. In this method we can choose, for example, which thrusters should be used for a yaw rotation (surge or sway thrusters).

I've created two exception states concerning the thrusters weights, one to avoid the number of weights to be different from the number of thrusters, and one to avoid negative or null weight values. The formula implemented on this commit is the following one:

More information about it can be seen in Fossen (1994, p. 98).

I've tested the code and when the user provides a proper vector of weights, the code is working good. But I'm having problems with the exception states. If an exception is detected (wrong size or wrong values) and then "false" is returned, the code breaks with the following message:

"`do_configure': failed to configure the '/AccelerationController' task of type auv_control::AccelerationController (Orocos::StateTransitionFailed)"

If I remove the line "return false", this problem doesn't happen but the system doesn't stay on exception state and it goes to CONTROLLING state. I'm suspicious this has something to do with the inheritance, but you guys might know it better.

[doudou]

I'll review the PR in more details, but just a bit of information about reporting failed configurations.

There would in principle be 3 ways to report configuration errors (who said "confusing" ?)

1. return false in configureHook
2. throw an exception in configureHook
3. transition to an exception state
4. seems to be buggy in many ways. First, the state transition seem to be ignored (that's what you are reporting). Second, the failure won't be picked up by orocos.rb (since the handler of configure() only checks on configure()'s return value)

I would also say that 3. is actually pretty much the clearer and nicer way, but it seems that nobody ever thought of using it before you ...

TL;DR: for now, return false.

[rafaelsaback]

@doudou, I removed the transition to the exception state and kept only the "return false", however, I still have the same problem.

I couldn't exactly understand when you said "but it seems that nobody ever thought of using it before you...", because inside the AccelerationController's task there are some transitions to exception state. And by the way, I have quickly checked here (only did for invalid name size), if those other exceptions happen, the script breaks on the same way (Orocos::StateTransitionFailed).

[doudou]

The case I think that is buggy is if you only transition to exception, without returning false.

The "perfect" behaviour would IMO be=:

1. if you call exception() and return false, you should get the StateTransitionFailed and the task in the expected exception state
2. if you only return false, you should get StateTransitionFailed and the task in PRE_OPERATIONAL
3. if you only call exception(), you should get the same as 1.

What I understood from your message is that (1) and (2) work as expected but (3) is broken.

What would be interesting is to test the behaviour of https://github.com/rock-control/control-orogen-auv_control/blob/master/tasks/AccelerationController.cpp#L74.

[rafaelsaback]

Well, I've just tested the behavior you've suggested, and again it doesn't go to the exception state.

Sorry, I might not have been clear enough about the exception problem. For the behaviours (1) and (2), the system doesn't go to the exception state (1) nor to PRE_OPERATIONAL (2). In both cases the error message "StateTransitionFailed" is shown but the task process is killed automatically.

[doudou]

but the task process is killed automatically.

In scripts, the state transition failed is caused by an exception, which makes the script (and the process) quit or the code crashes on exception (e.g. because you have an exceptionHook, not the case here I believe). If you want to ignore the error (or do something about it), use begin/rescue/end

begin
   task.configure
rescue Orocos::StateTransitionFailed
  # Do something here
end

[rafaelsaback]

Ok, I did some tests with the begin/rescue/end structure, I see it works properly and it's related to the "return false" (StateTransitionFailed). At this point the exception state transition inside configureHook() seems useless for me due to its bug.

For now I will wait for you to evaluate the rest of the pull request. Thanks for all the explanation.

[rafaelsaback]

I added the log messages using std::runtime_error + try/throw/catch structure. I have also dealt with the problems mentioned by you @doudou.

Two things worth mentioning:

1) I put the font color for the log message in red 2) I have also added an information to warn the user that the log message comes from the AccelerationCotnroller with the pattern "[AccelerationController] Log message...".

Let me know if that's fine.

[doudou]

1) I put the font color for the log message in red

Don't. ASCII codes are not usable everywhere, and especially not in text files.

2) I have also added an information to warn the user that the log message comes from the AccelerationCotnroller with the pattern "[AccelerationController] Log message...".

You should use either the RTT logger facility, or Rock's logging macros (with a preference to the latter)

[rafaelsaback]

Done!

[doudou]

Looks good to me ... @saarnold ?

[saarnold]

Looks good :+1: