Skip to content

Feedback Linearization parameters cfg #3

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
VBorjaDD wants to merge 4 commits into
base: master
Choose a base branch
from
Open

Feedback Linearization parameters cfg #3

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
3a880e4
Feedback Linearization parameters cfg
VBorjaDD Nov 25, 2020
4e63598
Introduced changes in message types and Kugle_driver_node.cpp to
Nov 30, 2020
ea8dcbf
Include proper parameters in cfg with name of variables and typos cor…
Nov 30, 2020
50f7b65
Reconfigure parameters values change
Dec 1, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 17 additions & 0 deletions .vscode/settings.json
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,17 @@
{
"files.associations": {
"array": "cpp",
"chrono": "cpp",
"compare": "cpp",
"functional": "cpp",
"future": "cpp",
"memory": "cpp",
"ratio": "cpp",
"tuple": "cpp",
"type_traits": "cpp",
"utility": "cpp",
"xmemory": "cpp",
"xtr1common": "cpp",
"xutility": "cpp"
}
}
29 changes: 27 additions & 2 deletions kugle_driver/cfg/Parameters.cfg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,11 +58,12 @@ behavioural.add( "PowerButtonMode", int_t, 0,

controllerType_t = gen.enum([
gen.const("LQR_CONTROLLER", int_t, 1, "Linear Quadratic Regulator"),
gen.const("SLIDING_MODE_CONTROLLER", int_t, 2, "Non-linear Sliding Mode")
gen.const("SLIDING_MODE_CONTROLLER", int_t, 2, "Non-linear Sliding Mode"),
gen.const("FEEDBACK_MODE_CONTROLLER", int_t, 3, "Feedback-Linearization Controller")
], "Controller type enum")
controller.add( "type", int_t, 0,
"Controller type",
2, 0, 2,
1, 0, 3,
edit_method=controllerType_t)

controllerMode_t = gen.enum([
Expand Down Expand Up @@ -180,6 +181,30 @@ controller_Kvi.add( "gamma", double_t, 0,
"Sliding manifold general integral gain",
0, 0, 2)

controller.add( "Kf", double_t, 0,
"Common Feedback Linearization ggain",
100, 0, 2000)
controller_Kf = controller.add_group("Kf_")
controller_Kf.add("Kfp_x", double_t, 0,
"Position pole gain",
100, 0, 2000)
controller_Kf.add("Kfp_y", double_t, 0,
"Position pole gain",
100, 0, 2000)
controller_Kf.add("Kfp_z", double_t, 0,
"Position pole gain",
100, 0, 2000)
controller_Kf.add("Kfv_x", double_t, 0,
"Position pole gain",
100, 0, 2000)
controller_Kf.add("Kfv_y", double_t, 0,
"Position pole gain",
100, 0, 2000)
controller_Kf.add("Kfv_z", double_t, 0,
"Position pole gain",
100, 0, 2000)


controller_AngularVelocityClamp = controller.add_group("AngularVelocityClamp")
controller_AngularVelocityClamp.add("AngularVelocityClampsEnabled", bool_t, 0,
"",
Expand Down
12 changes: 10 additions & 2 deletions kugle_driver/libs/lspc/MessageTypes.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -86,6 +86,13 @@ namespace lspc
Kx,
Ky,
Kz,
Kf,
Kfp_x,
Kfp_y,
Kfp_z,
Kfv_x,
Kfv_y,
Kfv_z,
Kv_x,
Kv_y,
Kvi_x,
Expand Down Expand Up @@ -181,7 +188,8 @@ namespace lspc
typedef enum: uint8_t {
UNKNOWN_CONTROLLER = 0x00,
LQR_CONTROLLER = 0x01,
SLIDING_MODE_CONTROLLER
SLIDING_MODE_CONTROLLER = 0x02,
FEEDBACK_LINEARIZATION_CONTROLLER = 0x03
} controllerType_t;

typedef enum: uint8_t {
Expand Down Expand Up @@ -673,4 +681,4 @@ namespace lspc

} // namespace lspc

#endif // LSPC_MESSAGE_TYPES_HPP
#endif // LSPC_MESSAGE_TYPES_HPP
63 changes: 63 additions & 0 deletions kugle_driver/src/kugle_driver_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -264,13 +264,15 @@ std::string ParseControllerType(lspc::ParameterTypes::controllerType_t type)
{
if (type == lspc::ParameterTypes::LQR_CONTROLLER) return "LQR_CONTROLLER";
else if (type == lspc::ParameterTypes::SLIDING_MODE_CONTROLLER) return "SLIDING_MODE_CONTROLLER";
else if (type == lspc::ParameterTypes::FEEDBACK_LINEARIZATION_CONTROLLER) return "FEEDBACK_LINEARIZATION_CONTROLLER";
else return "UNKNOWN_CONTROLLER";
}

lspc::ParameterTypes::controllerType_t ParseControllerType2(std::string type)
{
if (!type.compare("LQR_CONTROLLER")) return lspc::ParameterTypes::LQR_CONTROLLER;
else if (!type.compare("SLIDING_MODE_CONTROLLER")) return lspc::ParameterTypes::SLIDING_MODE_CONTROLLER;
else if (!type.compare("FEEDBACK_LINEARIZATION_CONTROLLER")) return lspc::ParameterTypes::FEEDBACK_LINEARIZATION_CONTROLLER;
else return lspc::ParameterTypes::UNKNOWN_CONTROLLER;
}

Expand Down Expand Up @@ -849,6 +851,34 @@ bool ParseParamTypeAndID(const std::string in_type, const std::string in_param,
out_param = lspc::ParameterLookup::Kz;
out_valueType = lspc::ParameterLookup::_float;
}
else if (!in_param.compare("Kf")) {
out_param = lspc::ParameterLookup::Kf;
out_valueType = lspc::ParameterLookup::_float;
}
else if (!in_param.compare("Kfp_x")) {
out_param = lspc::ParameterLookup::Kfp_x;
out_valueType = lspc::ParameterLookup::_float;
}
else if (!in_param.compare("Kfp_y")) {
out_param = lspc::ParameterLookup::Kfp_x;
out_valueType = lspc::ParameterLookup::_float;
}
else if (!in_param.compare("Kfp_z")) {
out_param = lspc::ParameterLookup::Kfp_z;
out_valueType = lspc::ParameterLookup::_float;
}
else if (!in_param.compare("Kfv_x")) {
out_param = lspc::ParameterLookup::Kfv_x;
out_valueType = lspc::ParameterLookup::_float;
}
else if (!in_param.compare("Kfv_y")) {
out_param = lspc::ParameterLookup::Kfv_y;
out_valueType = lspc::ParameterLookup::_float;
}
else if (!in_param.compare("Kfv_z")) {
out_param = lspc::ParameterLookup::Kfv_z;
out_valueType = lspc::ParameterLookup::_float;
}
else if (!in_param.compare("Kv_x")) {
out_param = lspc::ParameterLookup::Kv_x;
out_valueType = lspc::ParameterLookup::_float;
Expand Down Expand Up @@ -1609,6 +1639,23 @@ void reconfigureCallback(kugle_driver::ParametersConfig &config, uint32_t level,
config.K_z = config.K;
}

if (config.Kfp_x != reconfigureConfig.Kfp_x) reconfigureModifyParameter("controller", "Kf", std::to_string(config.Kfp_x) + " " + std::to_string(config.Kfp_y) + " " + std::to_string(config.Kfp_z) + " " + std::to_string(config.Kfv_x) + " " + std::to_string(config.Kfv_y) + " " + std::to_string(config.Kfv_z), lspcMutex, lspcObj);
if (config.Kfp_y != reconfigureConfig.Kfp_y) reconfigureModifyParameter("controller", "Kf", std::to_string(config.Kfp_x) + " " + std::to_string(config.Kfp_y) + " " + std::to_string(config.Kfp_z) + " " + std::to_string(config.Kfv_x) + " " + std::to_string(config.Kfv_y) + " " + std::to_string(config.Kfv_z), lspcMutex, lspcObj);
if (config.Kfp_z != reconfigureConfig.Kfp_z) reconfigureModifyParameter("controller", "Kf", std::to_string(config.Kfp_x) + " " + std::to_string(config.Kfp_y) + " " + std::to_string(config.Kfp_z) + " " + std::to_string(config.Kfv_x) + " " + std::to_string(config.Kfv_y) + " " + std::to_string(config.Kfv_z), lspcMutex, lspcObj);
if (config.Kfv_x != reconfigureConfig.Kfv_x) reconfigureModifyParameter("controller", "Kf", std::to_string(config.Kfp_x) + " " + std::to_string(config.Kfp_y) + " " + std::to_string(config.Kfp_z) + " " + std::to_string(config.Kfv_x) + " " + std::to_string(config.Kfv_y) + " " + std::to_string(config.Kfv_z), lspcMutex, lspcObj);
if (config.Kfv_y != reconfigureConfig.Kfv_y) reconfigureModifyParameter("controller", "Kf", std::to_string(config.Kfp_x) + " " + std::to_string(config.Kfp_y) + " " + std::to_string(config.Kfp_z) + " " + std::to_string(config.Kfv_x) + " " + std::to_string(config.Kfv_y) + " " + std::to_string(config.Kfv_z), lspcMutex, lspcObj);
if (config.Kfv_z != reconfigureConfig.Kfv_z) reconfigureModifyParameter("controller", "Kf", std::to_string(config.Kfp_x) + " " + std::to_string(config.Kfp_y) + " " + std::to_string(config.Kfp_z) + " " + std::to_string(config.Kfv_x) + " " + std::to_string(config.Kfv_y) + " " + std::to_string(config.Kfv_z), lspcMutex, lspcObj);

if (config.Kf != reconfigureConfig.Kf) {
reconfigureModifyParameter("controller", "Kf", std::to_string(config.Kf), lspcMutex, lspcObj);
config.Kfp_x = config.Kf;
config.Kfp_y = config.Kf;
config.Kfp_z = config.Kf;
config.Kfv_x = config.Kf;
config.Kfv_y = config.Kf;
config.Kfv_z = config.Kf;
}

if (config.eta_x != reconfigureConfig.eta_x) reconfigureModifyParameter("controller", "eta", std::to_string(config.eta_x) + " " + std::to_string(config.eta_y) + " " + std::to_string(config.eta_z), lspcMutex, lspcObj);
if (config.eta_y != reconfigureConfig.eta_y) reconfigureModifyParameter("controller", "eta", std::to_string(config.eta_x) + " " + std::to_string(config.eta_y) + " " + std::to_string(config.eta_z), lspcMutex, lspcObj);
if (config.eta_z != reconfigureConfig.eta_z) reconfigureModifyParameter("controller", "eta", std::to_string(config.eta_x) + " " + std::to_string(config.eta_y) + " " + std::to_string(config.eta_z), lspcMutex, lspcObj);
Expand Down Expand Up @@ -1712,6 +1759,22 @@ void LoadParamsIntoReconfigure(std::shared_ptr<std::timed_mutex> lspcMutex, std:
}
}

// Load Kf
{
std::istringstream iss(reconfigureRetrieveParameter("controller", "Kf", lspcMutex, lspcObj));
std::vector<std::string> values((std::istream_iterator<std::string>(iss)), // split string by spaces
std::istream_iterator<std::string>());
reconfigureConfig.Kf = 0;
if (values.size() == 6) {
reconfigureConfig.Kfp_x = Parse2RoundedFloat(values.at(0));
reconfigureConfig.Kfp_y = Parse2RoundedFloat(values.at(1));
reconfigureConfig.Kfp_z = Parse2RoundedFloat(values.at(2));
reconfigureConfig.Kfv_x = Parse2RoundedFloat(values.at(3));
reconfigureConfig.Kfv_y = Parse2RoundedFloat(values.at(4));
reconfigureConfig.Kfv_z = Parse2RoundedFloat(values.at(5));
}
}

// Load eta
{
std::istringstream iss(reconfigureRetrieveParameter("controller", "eta", lspcMutex, lspcObj));
Expand Down