diff --git a/doc/_data/schemas/SpaceVecAlg/ForceVecd.json b/doc/_data/schemas/SpaceVecAlg/ForceVecd.json index db9bb6db71..432eee5460 100644 --- a/doc/_data/schemas/SpaceVecAlg/ForceVecd.json +++ b/doc/_data/schemas/SpaceVecAlg/ForceVecd.json @@ -1,6 +1,7 @@ { "title": "sva::ForceVecd", "type": "object", + "description": "Either couple or force can be ommited if the other is present", "properties": { "couple": { "$ref": "/../../Eigen/Vector3d.json" }, diff --git a/doc/_data/schemas/SpaceVecAlg/ImpedanceVecd.json b/doc/_data/schemas/SpaceVecAlg/ImpedanceVecd.json index 70873c4035..b34430a45c 100644 --- a/doc/_data/schemas/SpaceVecAlg/ImpedanceVecd.json +++ b/doc/_data/schemas/SpaceVecAlg/ImpedanceVecd.json @@ -1,6 +1,7 @@ { "title": "sva::ImpedanceVecd", "type": "object", + "description": "Either angular or linear can be ommited if the other is present", "properties": { "angular": { "$ref": "/../../Eigen/Vector3d.json" }, diff --git a/doc/_data/schemas/SpaceVecAlg/MotionVecd.json b/doc/_data/schemas/SpaceVecAlg/MotionVecd.json index 5c9ac2f869..ca2eac0348 100644 --- a/doc/_data/schemas/SpaceVecAlg/MotionVecd.json +++ b/doc/_data/schemas/SpaceVecAlg/MotionVecd.json @@ -1,6 +1,7 @@ { "title": "sva::MotionVecd", "type": "object", + "description": "Either angular or linear can be ommited if the other is present", "properties": { "angular": { "$ref": "/../../Eigen/Vector3d.json" }, diff --git a/include/mc_rtc/Configuration.h b/include/mc_rtc/Configuration.h index 3f1a44e643..c2ca2667a4 100644 --- a/include/mc_rtc/Configuration.h +++ b/include/mc_rtc/Configuration.h @@ -190,6 +190,8 @@ struct MC_RTC_UTILS_DLLAPI Configuration * \throws If key does not belong in keys() */ Json operator[](const std::string & key) const; + /** Try to find an element at the provided key */ + std::optional find(const std::string & key) const; /** True if the value is a string */ bool isString() const noexcept; /** True if the value is numeric */ diff --git a/include/mc_rtc/Default.h b/include/mc_rtc/Default.h new file mode 100644 index 0000000000..1f50dda11c --- /dev/null +++ b/include/mc_rtc/Default.h @@ -0,0 +1,85 @@ +#pragma once + +#include + +#include +#include +#include + +namespace mc_rtc +{ + +/** Helper to get a default value for a given type + * + * When this is implemented, the template should provide Default::value of type T + * + * Enable is used to SFINAE generic definitions + */ +template +struct Default +{ + static_assert(!std::is_same_v, "Must be specialized"); +}; + +template +struct Default>> +{ + inline static constexpr T value = 0; +}; + +template +struct Default, std::enable_if_t<(N > 0)>> +{ + inline static const Eigen::Matrix value = + Eigen::Matrix::Zero(); +}; + +template +struct Default, std::enable_if_t<(N > 1)>> +{ + inline static const Eigen::Matrix value = + Eigen::Matrix::Identity(); +}; + +template<> +struct Default +{ + inline static const sva::PTransformd value = sva::PTransformd::Identity(); +}; + +template<> +struct Default +{ + inline static const sva::MotionVecd value = sva::MotionVecd::Zero(); +}; + +template<> +struct Default +{ + inline static const sva::ForceVecd value = sva::ForceVecd::Zero(); +}; + +template<> +struct Default +{ + inline static const sva::ImpedanceVecd value = sva::ImpedanceVecd::Zero(); +}; + +template<> +struct Default +{ + inline static const sva::AdmittanceVecd value = sva::AdmittanceVecd::Zero(); +}; + +template<> +struct Default +{ + inline static const std::string value; +}; + +template +struct Default> : public Default +{ +}; + +} // namespace mc_rtc diff --git a/src/mc_rtc/Configuration.cpp b/src/mc_rtc/Configuration.cpp index b61bf3ceb9..2f424c89c9 100644 --- a/src/mc_rtc/Configuration.cpp +++ b/src/mc_rtc/Configuration.cpp @@ -2,10 +2,13 @@ * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL */ -#include #include + +#include #include +#include + #include #include namespace bfs = boost::filesystem; @@ -20,6 +23,14 @@ inline std::string to_lower(const std::string & in) { return boost::algorithm::to_lower_copy(in); } + +template +T cast_or_default(const std::optional & opt) +{ + if(opt) { return opt->operator T(); } + return mc_rtc::Default::value; +} + } // namespace namespace mc_rtc @@ -111,6 +122,22 @@ Configuration::Json Configuration::Json::operator[](size_t idx) const return {static_cast(&(*value)[idx]), doc_}; } +std::optional Configuration::Json::find(const std::string & key) const +{ + assert(value_); + auto * value = static_cast(value_); + if(value->IsObject()) + { + auto it = value->FindMember(key); + if(it != value->MemberEnd()) + { + internal::RapidJSONValue * kvalue = &(it->value); + return Configuration::Json{static_cast(kvalue), doc_}; + } + } + return std::nullopt; +} + Configuration::Json Configuration::Json::operator[](const std::string & key) const { assert(value_); @@ -196,23 +223,15 @@ bool Configuration::has(const std::string & key) const Configuration Configuration::operator()(const std::string & key) const { - if(has(key)) { return Configuration(v[key]); } + auto out = find(key); + if(out) { return *out; } throw Exception("No entry named " + key + " in the configuration", v); } std::optional Configuration::find(const std::string & key) const { - assert(v.value_); - auto * value = static_cast(v.value_); - if(v.isObject()) - { - auto ret = value->FindMember(key); - if(ret != value->MemberEnd()) - { - internal::RapidJSONValue * kvalue = &(ret->value); - return Configuration(Configuration::Json{static_cast(kvalue), v.doc_}); - } - } + auto out = v.find(key); + if(out) { return Configuration(*out); } return std::nullopt; } @@ -507,21 +526,9 @@ Configuration::operator Eigen::MatrixXd() const Configuration::operator sva::PTransformd() const { - if(has("rotation")) - { - Eigen::Matrix3d r = (*this)("rotation"); - if(has("translation")) - { - Eigen::Vector3d t = (*this)("translation"); - return {r, t}; - } - return {r}; - } - if(has("translation")) - { - Eigen::Vector3d t = (*this)("translation"); - return {t}; - } + auto rot = find("rotation"); + auto trans = find("translation"); + if(rot || trans) { return {cast_or_default(rot), cast_or_default(trans)}; } if(size() == 7) { const auto & config = *this; @@ -540,7 +547,9 @@ Configuration::operator sva::PTransformd() const Configuration::operator sva::ForceVecd() const { - if(has("couple") && has("force")) { return {(*this)("couple"), (*this)("force")}; } + auto couple = find("couple"); + auto force = find("force"); + if(couple || force) { return {cast_or_default(couple), cast_or_default(force)}; } if(size() == 6) { const auto & config = *this; @@ -551,7 +560,12 @@ Configuration::operator sva::ForceVecd() const Configuration::operator sva::MotionVecd() const { - if(has("angular") && has("linear")) { return {(*this)("angular"), (*this)("linear")}; } + auto angular = find("angular"); + auto linear = find("linear"); + if(angular || linear) + { + return {cast_or_default(angular), cast_or_default(linear)}; + } if(size() == 6) { const auto & config = *this; @@ -562,10 +576,11 @@ Configuration::operator sva::MotionVecd() const Configuration::operator sva::ImpedanceVecd() const { - if(has("angular") && has("linear")) + auto angular = find("angular"); + auto linear = find("linear"); + if(angular || linear) { - Eigen::Vector3d angular = (*this)("angular"); - return {angular, (*this)("linear")}; + return {cast_or_default(angular), cast_or_default(linear)}; } if(size() == 6) { @@ -855,7 +870,8 @@ void Configuration::add(const std::string & key, const Configuration & value) auto & json = *static_cast(v.value_); internal::RapidJSONValue key_(key.c_str(), allocator); internal::RapidJSONValue value_(*static_cast(value.v.value_), allocator); - if(has(key)) { json.RemoveMember(key.c_str()); } + auto prev = json.FindMember(key_); + if(prev != json.MemberEnd()) { json.RemoveMember(prev); } json.AddMember(key_, value_, allocator); } @@ -867,7 +883,8 @@ Configuration Configuration::add(const std::string & key) auto & json = *static_cast(v.value_); internal::RapidJSONValue key_(key.c_str(), allocator); internal::RapidJSONValue value(rapidjson::kObjectType); - if(has(key)) { json.RemoveMember(key.c_str()); } + auto prev = json.FindMember(key_); + if(prev != json.MemberEnd()) { json.RemoveMember(prev); } json.AddMember(key_, value, allocator); return (*this)(key); } @@ -880,7 +897,8 @@ Configuration Configuration::array(const std::string & key, size_t size) internal::RapidJSONValue key_(key.c_str(), allocator); internal::RapidJSONValue value(rapidjson::kArrayType); if(size) { value.Reserve(size, allocator); } - if(has(key)) { json.RemoveMember(key.c_str()); } + auto prev = json.FindMember(key_); + if(prev != json.MemberEnd()) { json.RemoveMember(prev); } json.AddMember(key_, value, allocator); return (*this)(key); }