diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..578a7d9 --- /dev/null +++ b/.vscode/settings.json @@ -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" + } +} \ No newline at end of file diff --git a/kugle_driver/cfg/Parameters.cfg b/kugle_driver/cfg/Parameters.cfg index e102328..0a6cd9e 100755 --- a/kugle_driver/cfg/Parameters.cfg +++ b/kugle_driver/cfg/Parameters.cfg @@ -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([ @@ -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, "", diff --git a/kugle_driver/libs/lspc/MessageTypes.h b/kugle_driver/libs/lspc/MessageTypes.h index 5ab2ce4..533c5f0 100644 --- a/kugle_driver/libs/lspc/MessageTypes.h +++ b/kugle_driver/libs/lspc/MessageTypes.h @@ -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, @@ -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 { @@ -673,4 +681,4 @@ namespace lspc } // namespace lspc -#endif // LSPC_MESSAGE_TYPES_HPP +#endif // LSPC_MESSAGE_TYPES_HPP \ No newline at end of file diff --git a/kugle_driver/src/kugle_driver_node.cpp b/kugle_driver/src/kugle_driver_node.cpp index 69b89e4..00db1ab 100644 --- a/kugle_driver/src/kugle_driver_node.cpp +++ b/kugle_driver/src/kugle_driver_node.cpp @@ -264,6 +264,7 @@ 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"; } @@ -271,6 +272,7 @@ 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; } @@ -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; @@ -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); @@ -1712,6 +1759,22 @@ void LoadParamsIntoReconfigure(std::shared_ptr lspcMutex, std: } } + // Load Kf + { + std::istringstream iss(reconfigureRetrieveParameter("controller", "Kf", lspcMutex, lspcObj)); + std::vector values((std::istream_iterator(iss)), // split string by spaces + std::istream_iterator()); + 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));