diff --git a/.gitignore b/.gitignore index 8aadbe300a507..bf5208975141a 100644 --- a/.gitignore +++ b/.gitignore @@ -20,6 +20,7 @@ TAGS *.manifest *.rsp *~ +.idea/* /nbproject/ /doxygen/html/ docs/readthedocs/site diff --git a/dart/common/Addon.cpp b/dart/common/Addon.cpp new file mode 100644 index 0000000000000..c201ba973029f --- /dev/null +++ b/dart/common/Addon.cpp @@ -0,0 +1,95 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include + +#include "dart/common/Addon.h" +#include "dart/common/Console.h" + +namespace dart { +namespace common { + +//============================================================================== +void Addon::setAddonState(const State& /*otherState*/) +{ + // Do nothing +} + +//============================================================================== +const Addon::State* Addon::getAddonState() const +{ + return nullptr; +} + +//============================================================================== +void Addon::setAddonProperties(const Properties& /*someProperties*/) +{ + // Do nothing +} + +//============================================================================== +const Addon::Properties* Addon::getAddonProperties() const +{ + return nullptr; +} + +//============================================================================== +bool Addon::isOptional(AddonManager* /*oldManager*/) +{ + return true; +} + +//============================================================================== +Addon::Addon(AddonManager* manager) +{ + if(nullptr == manager) + { + dterr << "[Addon::constructor] You are not allowed to construct an Addon " + << "outside of an AddonManager!\n"; + assert(false); + } +} + +//============================================================================== +void Addon::setManager(AddonManager* /*newManager*/, bool /*transfer*/) +{ + // Do nothing +} + +} // namespace common +} // namespace dart diff --git a/dart/common/Addon.h b/dart/common/Addon.h new file mode 100644 index 0000000000000..cf5b8f4750d66 --- /dev/null +++ b/dart/common/Addon.h @@ -0,0 +1,304 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COMMON_ADDON_H_ +#define DART_COMMON_ADDON_H_ + +#include + +#include "dart/common/Extensible.h" +#include "dart/common/detail/NoOp.h" + +namespace dart { +namespace common { + +class AddonManager; + +class Addon +{ +public: + + friend class AddonManager; + + /// If your Addon has a State class, then that State class should inherit this + /// Addon::State class. This allows us to safely serialize, store, and clone + /// the states of arbitrary Addon extensions. If your Addon is stateless, then + /// you do not have to worry about extending this class, because + /// Addon::getState() will simply return a nullptr by default, which is taken + /// to indicate that it is stateless. + /// + /// The distinction between the State class and the Properties class is that + /// State will get stored in AddonManager::State whereas Properties will get + /// stored in AddonManager::Properties. Typically Properties are values that + /// only change rarely if ever, whereas State contains values that might + /// change as often as every time step. + class State : public Extensible { }; + + /// Use the StateMixer class to easily create a State extension from an + /// existing class or struct. + template + using StateMixer = ExtensibleMixer; + + /// If your Addon has a Properties class, then it should inherit this + /// Addon::Properties class. This allows us to safely serialize, store, and + /// clone the properties of arbitrary Addon extensions. If your Addon has no + /// properties, then you do not have to worry about extending this class, + /// because Addon::getProperties() will simply return a nullptr by default, + /// which is taken to indicate that it has no properties. + /// + /// The distinction between the State class and the Properties class is that + /// State will get stored in AddonManager::State whereas Properties will get + /// stored in AddonManager::Properties. Typically Properties are values that + /// only change rarely if ever, whereas State contains values that might + /// change as often as every time step. + class Properties : public Extensible { }; + + /// Use the PropertiesMixer class to easily create a Properties extension + /// from an existing class or struct. + template + using PropertiesMixer = ExtensibleMixer; + + /// Virtual destructor + virtual ~Addon() = default; + + /// Clone this Addon into a new manager + virtual std::unique_ptr cloneAddon(AddonManager* newManager) const = 0; + + /// Set the State of this Addon. By default, this does nothing. + virtual void setAddonState(const State& otherState); + + /// Get the State of this Addon. By default, this returns a nullptr which + /// implies that the Addon is stateless. + virtual const State* getAddonState() const; + + /// Set the Properties of this Addon. By default, this does nothing. + virtual void setAddonProperties(const Properties& someProperties); + + /// Get the Properties of this Addon. By default, this returns a nullptr + /// which implies that the Addon has no properties. + virtual const Properties* getAddonProperties() const; + + /// This function will be called if the user is attempting to delete the Addon + /// but not immediately replacing it with another Addon of the same type. The + /// incoming argument will point to the AddonManager that had been holding + /// this Addon. + /// + /// If your Addon is mandatory for the AddonManager type that is passed in + /// here, then you should perform error handling in this function, and you + /// should return false to indicate that the operation is not permitted. If + /// you return false, then the Addon will NOT be removed from its Manager. + /// + /// By default, this simply returns true. + virtual bool isOptional(AddonManager* oldManager); + +protected: + + /// Constructor + /// + /// We require the AddonManager argument in this constructor to make it clear + /// to extensions that they must have an AddonManager argument in their + /// constructors. + Addon(AddonManager* manager); + + /// This function will be triggered (1) after the Addon has been created + /// [transfer will be false] and (2) after the Addon has been transferred + /// to a new AddonManager [transfer will be true]. You should override this + /// function if your Addon requires special handling in either of those cases. + /// By default, this function does nothing. + virtual void setManager(AddonManager* newManager, bool transfer); + +}; + +//============================================================================== +/// AddonWithProtectedState generates implementations of the State managing +/// functions for an Addon class. +template > +class AddonWithProtectedState : public Addon +{ +public: + + using State = Addon::StateMixer; + constexpr static void (*UpdateState)(Base*) = updateState; + + AddonWithProtectedState() = delete; + AddonWithProtectedState(const AddonWithProtectedState&) = delete; + + /// Construct using a StateData instance + AddonWithProtectedState( + AddonManager* mgr, const StateData& state = StateData()); + + // Documentation inherited + void setAddonState(const Addon::State& otherState) override final; + + // Documentation inherited + const Addon::State* getAddonState() const override final; + + /// Set the State of this Addon + void setState(const StateData& state); + + /// Get the State of this Addon + const State& getState() const; + + // Documentation inherited + std::unique_ptr cloneAddon( + AddonManager* newManager) const override final; + +protected: + + /// State of this Addon + State mState; +}; + +//============================================================================== +/// AddonWithProtectedProperties generates implementations of the Property +/// managing functions for an Addon class. +template > +class AddonWithProtectedProperties : public Addon +{ +public: + + using Properties = Addon::PropertiesMixer; + constexpr static void (*UpdateProperties)(Base*) = updateProperties; + + AddonWithProtectedProperties() = delete; + AddonWithProtectedProperties(const AddonWithProtectedProperties&) = delete; + + /// Construct using a PropertiesData instance + AddonWithProtectedProperties( + AddonManager* mgr, const PropertiesData& properties = PropertiesData()); + + // Documentation inherited + void setAddonProperties(const Addon::Properties& someProperties) override final; + + // Documentation inherited + const Addon::Properties* getAddonProperties() const override final; + + /// Set the Properties of this Addon + void setProperties(const PropertiesData& properties); + + /// Get the Properties of this Addon + const Properties& getProperties() const; + + // Documentation inherited + std::unique_ptr cloneAddon( + AddonManager* newManager) const override final; + +protected: + + /// Properties of this Addon + Properties mProperties; +}; + +//============================================================================== +/// AddonWithProtectedStateAndProperties combines the +/// AddonWithProtectedState and AddonWithProtectedProperties classes into a +/// single templated class +template , + void (*updateProperties)(Base*) = updateState> +class AddonWithProtectedStateAndProperties : public Addon +{ +public: + + using State = Addon::StateMixer; + using Properties = Addon::PropertiesMixer; + constexpr static void (*UpdateState)(Base*) = updateState; + constexpr static void (*UpdateProperties)(Base*) = updateProperties; + + AddonWithProtectedStateAndProperties() = delete; + AddonWithProtectedStateAndProperties( + const AddonWithProtectedStateAndProperties&) = delete; + + /// Construct using a StateData and a PropertiesData instance + AddonWithProtectedStateAndProperties( + AddonManager* mgr, + const StateData& state = StateData(), + const PropertiesData& properties = PropertiesData()); + + /// Construct using a StateData and a PropertiesData instance, flipped + AddonWithProtectedStateAndProperties( + AddonManager* mgr, + const PropertiesData& properties, + const StateData& state = StateData()); + + // Documentation inherited + void setAddonState(const Addon::State& otherState) override final; + + // Documentation inherited + const Addon::State* getAddonState() const override final; + + /// Set the State of this Addon + void setState(const StateData& state); + + /// Get the State of this Addon + const State& getState() const; + + // Documentation inherited + void setAddonProperties(const Addon::Properties& otherProperties) override final; + + // Documentation inherited + const Addon::Properties* getAddonProperties() const override final; + + /// Set the Properties of this Addon + void setProperties(const PropertiesData& properties); + + /// Get the Properties of this Addon + const Properties& getProperties() const; + + // Documentation inherited + std::unique_ptr cloneAddon( + AddonManager* newManager) const override final; + +protected: + + /// State of this Addon + State mState; + + /// Properties of this Addon + Properties mProperties; +}; + +} // namespace common +} // namespace dart + +#include "dart/common/detail/Addon.h" + +#endif // DART_COMMON_ADDON_H_ diff --git a/dart/common/AddonManager.cpp b/dart/common/AddonManager.cpp new file mode 100644 index 0000000000000..d7320f6203def --- /dev/null +++ b/dart/common/AddonManager.cpp @@ -0,0 +1,264 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +#include "dart/common/Console.h" +#include "dart/common/AddonManager.h" + +namespace dart { +namespace common { + +//============================================================================== +void AddonManager::setAddonStates(const State& newStates) +{ + const StateMap& stateMap = newStates.getMap(); + + AddonMap::iterator addons = mAddonMap.begin(); + StateMap::const_iterator states = stateMap.begin(); + + while( mAddonMap.end() != addons && stateMap.end() != states ) + { + if( addons->first == states->first ) + { + Addon* addon = addons->second.get(); + if(addon && states->second) + addon->setAddonState(*states->second); + + ++addons; + ++states; + } + else if( addons->first < states->first ) + { + ++addons; + } + else + { + ++states; + } + } +} + +//============================================================================== +template +void extractMapData(MapType& outgoingMap, const AddonManager::AddonMap& addonMap) +{ + // TODO(MXG): Consider placing this function in a header so it can be utilized + // by anything that needs to transfer data between maps of extensibles + + // This method allows us to avoid dynamic allocation (cloning) whenever possible. + for(const auto& addon : addonMap) + { + if(nullptr == addon.second) + continue; + + const DataType* data = (addon.second.get()->*getData)(); + if(data) + { + // Attempt to insert a nullptr to see whether this entry exists while also + // creating an itertor to it if it did not already exist. This allows us + // to search for a spot in the map once, instead of searching the map to + // see if the entry already exists and then searching the map again in + // order to insert the entry if it didn't already exist. + std::pair insertion = + outgoingMap.insert(typename MapType::value_type(addon.first, nullptr)); + + typename MapType::iterator& it = insertion.first; + bool existed = !insertion.second; + + if(existed) + { + // The entry already existed + if(it->second) + { + // The entry was not a nullptr, so we can do an efficient copy + it->second->copy(*data); + } + else + { + // The entry was a nullptr, so we need to clone + it->second = data->clone(); + } + } + else + { + // The entry did not already exist, so we need to clone + it->second = data->clone(); + } + } + } +} + +//============================================================================== +AddonManager::State AddonManager::getAddonStates() const +{ + State states; + copyAddonStatesTo(states); + + return states; +} + +//============================================================================== +void AddonManager::copyAddonStatesTo(State& outgoingStates) const +{ + StateMap& states = outgoingStates.getMap(); + extractMapData(states, mAddonMap); +} + +//============================================================================== +void AddonManager::setAddonProperties(const Properties& newProperties) +{ + const PropertiesMap& propertiesMap = newProperties.getMap(); + + AddonMap::iterator addons = mAddonMap.begin(); + PropertiesMap::const_iterator props = propertiesMap.begin(); + + while( mAddonMap.end() != addons && propertiesMap.end() != props ) + { + if( addons->first == props->first ) + { + Addon* addon = addons->second.get(); + if(addon) + addon->setAddonProperties(*props->second); + + ++addons; + ++props; + } + else if( addons->first < props->first ) + { + ++addons; + } + else + { + ++props; + } + } +} + +//============================================================================== +AddonManager::Properties AddonManager::getAddonProperties() const +{ + Properties properties; + copyAddonPropertiesTo(properties); + + return properties; +} + +//============================================================================== +void AddonManager::copyAddonPropertiesTo( + Properties& outgoingProperties) const +{ + PropertiesMap& properties = outgoingProperties.getMap(); + extractMapData( + properties, mAddonMap); +} + +//============================================================================== +void AddonManager::duplicateAddons(const AddonManager* otherManager) +{ + if(nullptr == otherManager) + { + dterr << "[AddonManager::duplicateAddons] You have asked to duplicate the " + << "Addons of a nullptr, which is not allowed!\n"; + assert(false); + return; + } + + if(this == otherManager) + return; + + const AddonMap& otherMap = otherManager->mAddonMap; + + AddonMap::iterator receiving = mAddonMap.begin(); + AddonMap::const_iterator incoming = otherMap.begin(); + + while( otherMap.end() != incoming ) + { + if( mAddonMap.end() == receiving ) + { + // If we've reached the end of this Manager's AddonMap, then we should + // just add each entry + mAddonMap[incoming->first] = incoming->second->cloneAddon(this); + } + else if( receiving->first == incoming->first ) + { + if(incoming->second) + receiving->second = incoming->second->cloneAddon(this); + + ++receiving; + ++incoming; + } + else if( receiving->first < incoming->first) + { + ++receiving; + } + else + { + // If this Manager does not have an entry corresponding to the incoming + // Addon, then we must create it + mAddonMap[incoming->first] = incoming->second->cloneAddon(this); + ++incoming; + } + } +} + +//============================================================================== +void AddonManager::matchAddons(const AddonManager* otherManager) +{ + if(nullptr == otherManager) + { + dterr << "[AddonManager::matchAddons] You have asked to match the Addons " + << "of a nullptr, which is not allowed!\n"; + assert(false); + return; + } + + for(auto& addon : mAddonMap) + addon.second = nullptr; + + duplicateAddons(otherManager); +} + +//============================================================================== +void AddonManager::becomeManager(Addon* addon, bool transfer) +{ + if(addon) + addon->setManager(this, transfer); +} + +} // namespace common +} // namespace dart diff --git a/dart/common/AddonManager.h b/dart/common/AddonManager.h new file mode 100644 index 0000000000000..9ebe37b80ed92 --- /dev/null +++ b/dart/common/AddonManager.h @@ -0,0 +1,185 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COMMON_ADDONMANAGER_H_ +#define DART_COMMON_ADDONMANAGER_H_ + +#include +#include +#include + +#include "dart/common/Addon.h" + +namespace dart { +namespace common { + +/// AddonManager is a base class that should be virtually inherited by any class +/// that wants to be able to manage Addons. +/// +/// The base AddonManager class is completely agnostic to what kind of Addons it +/// is given. Addons are stored in a std::map, so access to its Addons happens +/// on average in log(N) time. Most often, a class that accepts Addons will have +/// certain Addon types that it will need to access frequently, and it would be +/// beneficial to have constant-time access to those Addon types. To get +/// constant-time access to specific Addon types, there are FOUR macros that you +/// should use in your derived class: +/// +/// DART_ENABLE_ADDON_SPECIALIZATION() must be declared once in the derived +/// class's definition, under a 'public:' declaration range. +/// +/// DART_SPECIALIZE_ADDON_INTERNAL( AddonType ) must be declared once for each +/// AddonType that you want to specialize. It should be placed in the derived +/// class's definition, under a 'public:' declaration range. +/// +/// DART_SPECIALIZE_ADDON_EXTERNAL( Derived, AddonType ) must be declared once +/// for each AddonType that you want to specialize. It should be placed +/// immediately after the class's definition in the same header file as the +/// derived class, inside of the same namespace as the derived class. This macro +/// defines a series of templated functions, so it should go in a header, and +/// not in a source file. +/// +/// DART_INSTANTIATE_SPECIALIZED_ADDON( AddonType ) must be declared once for +/// each AddonType that you want to specialize. It should be placed inside the +/// constructor of the derived class, preferably before anything else is done +/// inside the body of the constructor. +class AddonManager +{ +public: + + using StateMap = std::map< std::type_index, std::unique_ptr >; + using State = ExtensibleMapHolder; + + using PropertiesMap = std::map< std::type_index, std::unique_ptr >; + using Properties = ExtensibleMapHolder; + + using AddonMap = std::map< std::type_index, std::unique_ptr >; + + /// Virtual destructor + virtual ~AddonManager() = default; + + /// Check if this AddonManager currently has a certain type of Addon + template + bool has() const; + + /// Get a certain type of Addon from this AddonManager + template + T* get(); + + /// Get a certain type of Addon from this AddonManager + template + const T* get() const; + + /// Make a clone of the addon and place the clone into this AddonManager. If + /// an Addon of the same type already exists in this AddonManager, the + /// existing Addon will be destroyed. + template + void set(const T* addon); + + /// Use move semantics to place addon into this AddonManager. If an Addon of + /// the same type already exists in this AddonManager, the existing Addon will + /// be destroyed. + template + void set(std::unique_ptr&& addon); + + /// Construct an Addon inside of this AddonManager + template + T* create(Args&&... args); + + /// Remove an Addon from this AddonManager. + template + void erase(); + + /// Remove an Addon from this AddonManager, but return its unique_ptr instead + /// of letting it be deleted. This allows you to safely use move semantics to + /// transfer an Addon between two AddonManagers. + template + std::unique_ptr release(); + + /// Check if this Manager is specialized for a specific type of Addon. + /// + /// By default, this simply returns false. + template + static constexpr bool isSpecializedFor(); + + /// Set the states of the addons in this AddonManager based on the given + /// AddonManager::State. The states of any Addon types that do not exist + /// within this manager will be ignored. + void setAddonStates(const State& newStates); + + /// Get the states of the addons inside of this AddonManager + State getAddonStates() const; + + /// Fill outgoingStates with the states of the addons inside this AddonManager + void copyAddonStatesTo(State& outgoingStates) const; + + /// Set the properties of the addons in this AddonManager based on the given + /// AddonManager::Properties. The properties of any Addon types that do not + /// exist within this manager will be ignored. + void setAddonProperties(const Properties& newProperties); + + /// Get the properties of the addons inside of this AddonManager + Properties getAddonProperties() const; + + /// Fill outgoingProperties with the properties of the addons inside this + /// AddonManager + void copyAddonPropertiesTo(Properties& outgoingProperties) const; + + /// Give this AddonManager a copy of each Addon from otherManager + void duplicateAddons(const AddonManager* otherManager); + + /// Make the Addons of this AddonManager match the Addons of otherManager. Any + /// Addons in this AddonManager which do not exist in otherManager will be + /// erased. + void matchAddons(const AddonManager* otherManager); + +protected: + + template struct type { }; + + /// Become the AddonManager of the given Addon. This allows derived + /// AddonManager types to call the protected Addon::setManager function. + void becomeManager(Addon* addon, bool transfer); + + /// A map that relates the type of Addon to its pointer + AddonMap mAddonMap; +}; + +} // namespace common +} // namespace dart + +#include "dart/common/detail/AddonManager.h" + +#endif // DART_COMMON_ADDONMANAGER_H_ diff --git a/dart/common/AddonManagerJoiner.h b/dart/common/AddonManagerJoiner.h new file mode 100644 index 0000000000000..b01aa4ca548e0 --- /dev/null +++ b/dart/common/AddonManagerJoiner.h @@ -0,0 +1,157 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COMMON_ADDONMANAGERJOINER_H_ +#define DART_COMMON_ADDONMANAGERJOINER_H_ + +#include "dart/common/AddonManager.h" +#include "dart/common/Empty.h" + +namespace dart { +namespace common { + +/// Terminator for the variadic template +template +class AddonManagerJoiner { }; + +/// Special case of only having 1 class: we do nothing but inherit it. +template +class AddonManagerJoiner : public Base1 { }; + +/// AddonManagerJoiner allows classes that inherit from various +/// SpecializedAddonManager types to be inherited by a single derived class. +/// This class solves the diamond-of-death problem for multiple +/// SpecializedAddonManager inheritance. +template +class AddonManagerJoiner : public Base1, public Base2 +{ +public: + + /// Default constructor + AddonManagerJoiner() = default; + + /// This constructor allows one argument to be passed to the Base1 constructor + /// and arbitrarily many arguments to be passed to the Base2 constructor. + // + // TODO(MXG): When we migrate to using C++14, we can use std::tuple and + // std::index_sequence (which is only available in C++14) to dispatch + // arbitrarily many arguments to the constructors of each base class. Until + // then, this is the best we can offer due to fundamental limitations of + // variadic templates in C++11. + template + AddonManagerJoiner(Base1Arg&& arg1, Base2Args&&... args2); + + /// This constructor passes one argument to the Base1 constructor and no + /// arguments to the Base2 constructor. + template + AddonManagerJoiner(Base1Arg&& arg1, NoArg_t); + + /// This constructor passes no arguments to the Base1 constructor and + /// arbitrarily many arguments to the Base2 constructor. + template + AddonManagerJoiner(NoArg_t, Base2Args&&... args2); + + // Documentation inherited + template + bool has() const; + + // Documentation inherited + template + T* get(); + + // Documentation inherited + template + const T* get() const; + + // Documentation inherited + template + void set(const T* addon); + + // Documentation inherited + template + void set(std::unique_ptr&& addon); + + // Documentation inherited + template + T* create(Args&&... args); + + // Documentation inherited + template + void erase(); + + // Documentation inherited + template + std::unique_ptr release(); + + // Documentation inherited + template + static constexpr bool isSpecializedFor(); + +}; + +/// This is the variadic version of the AddonManagerJoiner class which allows +/// you to include arbitrarily many base classes in the joining. +template +class AddonManagerJoiner : + public AddonManagerJoiner< Base1, AddonManagerJoiner > +{ +public: + + /// Default constructor + AddonManagerJoiner() = default; + + /// This constructor allows one argument to be passed to each Base class's + /// constructor (except the final Base class, which accepts arbitrarily many + /// arguments). Pass in dart::common::NoArgs for classes whose constructors + /// do not take any arguments. + // + // TODO(MXG): When we migrate to using C++14, we can use std::tuple and + // std::index_sequence (which is only available in C++14) to dispatch + // arbitrarily many arguments to the constructors of each base class. Until + // then, this is the best we can offer due to fundamental limitations of + // variadic templates in C++11. + template + AddonManagerJoiner(Args&&... args); + +}; + +} // namespace common +} // namespace dart + +#include "dart/common/detail/AddonManagerJoiner.h" + +#endif // DART_COMMON_ADDONMANAGERJOINER_H_ + diff --git a/dart/common/Console.h b/dart/common/Console.h index ce67953838169..bb9e4271a0a48 100644 --- a/dart/common/Console.h +++ b/dart/common/Console.h @@ -39,6 +39,7 @@ #define DART_COMMON_CONSOLE_H_ #include +#include /// \brief Output a message #define dtmsg (dart::common::colorMsg("Msg", 32)) diff --git a/dart/common/Empty.h b/dart/common/Empty.h new file mode 100644 index 0000000000000..c0d3eff3839e3 --- /dev/null +++ b/dart/common/Empty.h @@ -0,0 +1,54 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COMMON_EMPTY_H_ +#define DART_COMMON_EMPTY_H_ + +namespace dart { +namespace common { + +/// This is an empty structure which can be used as a template argument when a +/// zero-cost placeholder is needed. +struct Empty { }; + +/// Used to tag arguments as blank for in variadic joiner classes such as +/// common::AddonManagerJoiner and dynamics::NodeManagerJoiner +enum NoArg_t { NoArg }; + +} // namespace common +} // namespace dart + +#endif // DART_COMMON_EMPTY_H_ diff --git a/dart/common/Extensible.h b/dart/common/Extensible.h new file mode 100644 index 0000000000000..5918b945b7272 --- /dev/null +++ b/dart/common/Extensible.h @@ -0,0 +1,217 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COMMON_EXTENSIBLE_H_ +#define DART_COMMON_EXTENSIBLE_H_ + +#include +#include + +namespace dart { +namespace common { + +/// Extensible is a CRTP base class that provides an interface for easily +/// creating data structures that are meant to be extended. Ordinary copying +/// does not work with extended data structures, because information from the +/// derived classes will typically be lost during a copy. The Extensible class +/// provides an interface for creating a new copy of an extended data structure, +/// as well as copying information between two extended data structures of the +/// same type. +template +class Extensible +{ +public: + + /// Default constructor + Extensible() = default; + + /// Virtual destructor + virtual ~Extensible() = default; + + /// Do not copy this class directly, use clone() or copy() instead + Extensible(const Extensible& doNotCopy) = delete; + + /// Do not copy this class directly, use clone() or copy() instead + Extensible& operator=(const Extensible& doNotCopy) = delete; + + /// Implement this function to allow your Extensible type to be copied safely. + virtual std::unique_ptr clone() const = 0; + + /// Copy the contents of anotherExtensible into this one. + virtual void copy(const T& anotherExtensible) = 0; +}; + +/// The ExtensibleMixer class is used to easily create an Extensible (such as +/// Node::State) which simply takes an existing class (Mixin) and creates an +/// Extensible that wraps it. This creates all the appropriate copy, move, and +/// clone members, allowing you to follow the Rule Of Zero. You can also +/// construct an instance in the exact same way that you would construct a Mixin +/// instance. +template +class ExtensibleMixer : public T, public Mixin +{ +public: + + /// Default constructor. Uses the default constructor of Mixin + ExtensibleMixer(); + + /// Templated constructor. Uses whichever Mixin constructor is able to match + /// the arguments. + template + ExtensibleMixer(Args&&... args); + + /// Constructs using a Mixin instance + ExtensibleMixer(const Mixin& mixin); + + /// Constructs using a Mixin rvalue + ExtensibleMixer(Mixin&& mixin); + + /// Copy constructor + ExtensibleMixer(const ExtensibleMixer& other); + + /// Move constructor + ExtensibleMixer(ExtensibleMixer&& other); + + /// Copy assignment operator that uses a Mixin instance + ExtensibleMixer& operator=(const Mixin& mixin); + + /// Move assignment operator that uses a Mixin rvalue + ExtensibleMixer& operator=(Mixin&& mixin); + + /// Copy assignment operator + ExtensibleMixer& operator=(const ExtensibleMixer& other); + + /// Move assignment operator + ExtensibleMixer& operator=(ExtensibleMixer&& other); + + // Documentation inherited + std::unique_ptr clone() const override final; + + // Documentation inherited + void copy(const T& other) override final; + +}; + +/// MapHolder is a templated wrapper class that is used to allow maps of +/// Addon::State and Addon::Properties to be handled in a semantically +/// palatable way. +template +class ExtensibleMapHolder final +{ +public: + + /// Default constructor + ExtensibleMapHolder() = default; + + /// Copy constructor + ExtensibleMapHolder(const ExtensibleMapHolder& otherStates); + + /// Move constructor + ExtensibleMapHolder(ExtensibleMapHolder&& otherStates); + + /// Map-based constructor + ExtensibleMapHolder(const MapType& otherMap); + + /// Map-based move constructor + ExtensibleMapHolder(MapType&& otherMap); + + /// Assignment operator + ExtensibleMapHolder& operator=(const ExtensibleMapHolder& otherStates); + + /// Move assignment operator + ExtensibleMapHolder& operator=(ExtensibleMapHolder&& otherStates); + + /// Map-based assignment operator + ExtensibleMapHolder& operator=(const MapType& otherMap); + + /// Map-based move assignment operator + ExtensibleMapHolder& operator=(MapType&& otherMap); + + /// Get the map that is being held + MapType& getMap(); + + /// Get the map that is being held + const MapType& getMap() const; + +private: + + /// A map containing the collection of States for the Addon + MapType mMap; +}; + +/// The ExtensibleVector type wraps a std::vector of an Extensible type allowing +/// it to be handled by an ExtensibleMapHolder +template +class ExtensibleVector final +{ +public: + + /// Default constructor + ExtensibleVector() = default; + + /// Construct from a regular vector + ExtensibleVector(const std::vector& regularVector); + + /// Construct from a regular vector using move semantics + ExtensibleVector(std::vector&& regularVector); + + /// Do not copy this class directly, use clone() or copy() instead + ExtensibleVector(const ExtensibleVector& doNotCopy) = delete; + + /// Do not copy this class directly, use clone() or copy() instead + ExtensibleVector& operator=(const ExtensibleVector& doNotCopy) = delete; + + /// Create a copy of this ExtensibleVector's contents + std::unique_ptr< ExtensibleVector > clone() const; + + /// Copy the contents of another extensible vector into this one. + void copy(const ExtensibleVector& anotherVector); + + /// Get a reference to the std::vector that this class is wrapping + const std::vector& getVector() const; + +private: + + /// The std::vector that this class is wrapping + std::vector mVector; +}; + +} // namespace common +} // namespace dart + +#include "dart/common/detail/Extensible.h" + +#endif // DART_COMMON_EXTENSIBLE_H_ diff --git a/dart/common/NameManager.h b/dart/common/NameManager.h index de7cea302c995..c2c12ae079b34 100644 --- a/dart/common/NameManager.h +++ b/dart/common/NameManager.h @@ -71,7 +71,7 @@ class NameManager const std::string& _defaultName = "default"); /// Destructor - virtual ~NameManager(); + virtual ~NameManager() = default; /// Set a new pattern for name generation. /// diff --git a/dart/common/SpecializedAddonManager.h b/dart/common/SpecializedAddonManager.h new file mode 100644 index 0000000000000..62dd5acb8a4e5 --- /dev/null +++ b/dart/common/SpecializedAddonManager.h @@ -0,0 +1,195 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COMMON_SPECIALIZEDADDONMANAGER_H_ +#define DART_COMMON_SPECIALIZEDADDONMANAGER_H_ + +#include "dart/common/AddonManager.h" +#include "dart/common/AddonManagerJoiner.h" +#include "dart/common/Virtual.h" + +namespace dart { +namespace common { + +/// Declaration of the variadic template +template +class SpecializedAddonManager { }; + +//============================================================================== +/// SpecializedAddonManager allows classes that inherit AddonManager to have +/// constant-time access to a specific type of Addon +template +class SpecializedAddonManager : public virtual AddonManager +{ +public: + + /// Default Constructor + SpecializedAddonManager(); + + /// Check if this AddonManager currently has a certain type of Addon + template + bool has() const; + + /// Get a certain type of Addon from this AddonManager + template + T* get(); + + /// Get a certain type of Addon from this AddonManager + template + const T* get() const; + + /// Make a clone of the addon and place the clone into this AddonManager. If + /// an Addon of the same type already exists in this AddonManager, the + /// existing Addon will be destroyed. + template + void set(const T* addon); + + /// Use move semantics to place an addon into this AddonManager. If an Addon + /// of the same type already exists in this AddonManager, the existing Addon + /// will be destroyed. + template + void set(std::unique_ptr&& addon); + + /// Construct an Addon inside of this AddonManager + template + T* create(Args&&... args); + + /// Remove an Addon from this AddonManager + template + void erase(); + + /// Remove an Addon from this AddonManager, but return its unique_ptr instead + /// of letting it be deleted. This allows you to safely use move semantics to + /// transfer an Addon between two AddonManagers. + template + std::unique_ptr release(); + + /// Check if this Manager is specialized for a specific type of Addon + template + static constexpr bool isSpecializedFor(); + +protected: + + /// Redirect to AddonManager::has() + template + bool _has(type) const; + + /// Specialized implementation of has() + bool _has(type) const; + + /// Redirect to AddonManager::get() + template + T* _get(type); + + /// Specialized implementation of get() + SpecAddon* _get(type); + + /// Redirect to AddonManager::get() + template + const T* _get(type) const; + + /// Specialized implementation of get() + const SpecAddon* _get(type) const; + + /// Redirect to AddonManager::set() + /// + /// Using the type tag for this is not be necessary, but it helps to avoid + /// confusion between the set() versus _set() function. + template + void _set(type, const T* addon); + + /// Specialized implementation of set() + void _set(type, const SpecAddon* addon); + + /// Redirect to AddonManager::set() + /// + /// Using the type tag for this is not be necessary, but it helps to avoid + /// confusion between the set() versus _set() function. + template + void _set(type, std::unique_ptr&& addon); + + /// Specialized implementation of set() + void _set(type, std::unique_ptr&& addon); + + /// Redirect to AddonManager::create() + template + T* _create(type, Args&&... args); + + /// Specialized implementation of create() + template + SpecAddon* _create(type, Args&&... args); + + /// Redirect to AddonManager::erase() + template + void _erase(type); + + /// Specialized implementation of erase() + void _erase(type); + + /// Redirect to AddonManager::release() + template + std::unique_ptr _release(type); + + /// Specialized implementation of release() + std::unique_ptr _release(type); + + /// Return false + template + static constexpr bool _isSpecializedFor(type); + + /// Return true + static constexpr bool _isSpecializedFor(type); + + /// Iterator that points to the Addon of this SpecializedAddonManager + AddonManager::AddonMap::iterator mSpecAddonIterator; + +}; + +//============================================================================== +/// This is the variadic version of the SpecializedAddonManager class which +/// allows you to include arbitrarily many specialized types in the +/// specialization. +template +class SpecializedAddonManager : + public AddonManagerJoiner< Virtual< SpecializedAddonManager >, + Virtual< SpecializedAddonManager > > { }; + +} // namespace common +} // namespace dart + +#include "dart/common/detail/SpecializedAddonManager.h" + +#endif // DART_COMMON_SPECIALIZEDADDONMANAGER_H_ diff --git a/dart/common/Virtual.h b/dart/common/Virtual.h new file mode 100644 index 0000000000000..3872989f875fe --- /dev/null +++ b/dart/common/Virtual.h @@ -0,0 +1,51 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COMMON_VIRTUAL_H_ +#define DART_COMMON_VIRTUAL_H_ + +namespace dart { +namespace common { + +/// This class is used to have CRTP functions inherit their template parameters +/// virtually instead of directly. +template +class Virtual : public virtual T { }; + +} // namespace common +} // namespace dart + +#endif // DART_COMMON_VIRTUAL_H_ diff --git a/dart/common/detail/Addon.h b/dart/common/detail/Addon.h new file mode 100644 index 0000000000000..89ac9c0bfd639 --- /dev/null +++ b/dart/common/detail/Addon.h @@ -0,0 +1,338 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COMMON_DETAIL_ADDON_H_ +#define DART_COMMON_DETAIL_ADDON_H_ + +#include + +#include "dart/common/Console.h" +#include "dart/common/Addon.h" + +namespace dart { +namespace common { + +#define DART_COMMON_CAST_NEW_MANAGER_TYPE(Base, ManagerType, newManager,\ + castedManager, func)\ + ManagerType* castedManager = dynamic_cast(newManager);\ + if(nullptr == castedManager)\ + {\ + dterr << "[" << typeid(Base).name() << "::" << #func << "] Attempting to "\ + << "use a [" << typeid(newManager).name() << "] type manager, but "\ + << "this Addon is only designed to be attached to a ["\ + << typeid(ManagerType).name() << "] type manager. This may cause "\ + << "undefined behavior!\n";\ + assert(false);\ + } + +#define DART_COMMON_CAST_NEW_MANAGER_TYPE_AND_RETURN_NULL_IF_BAD(\ + Base, ManagerType, newManager, castedManager, func)\ + DART_COMMON_CAST_NEW_MANAGER_TYPE(Base, ManagerType, newManager, castedManager, func)\ + if(nullptr == castedManager) return nullptr; + +//============================================================================== +template +AddonWithProtectedState:: +AddonWithProtectedState( + AddonManager* mgr, const StateData& state) + : Addon(mgr), + mState(state) +{ + // Do nothing +} + +//============================================================================== +template +void AddonWithProtectedState:: +setAddonState(const Addon::State& otherState) +{ + setState(static_cast(otherState)); +} + +//============================================================================== +template +const Addon::State* +AddonWithProtectedState:: +getAddonState() const +{ + return &mState; +} + +//============================================================================== +template +void AddonWithProtectedState:: +setState(const StateData& state) +{ + static_cast(mState) = state; + UpdateState(static_cast(this)); +} + +//============================================================================== +template +auto AddonWithProtectedState:: +getState() const -> const State& +{ + return mState; +} + +//============================================================================== +template +std::unique_ptr +AddonWithProtectedState:: + cloneAddon(AddonManager* newManager) const +{ + DART_COMMON_CAST_NEW_MANAGER_TYPE_AND_RETURN_NULL_IF_BAD( + Base, ManagerType, newManager, castedManager, clone); + return std::unique_ptr(new Base(newManager, mState)); +} + +//============================================================================== +template +AddonWithProtectedProperties:: +AddonWithProtectedProperties( + AddonManager* mgr, const PropertiesData& properties) + : Addon(mgr), + mProperties(properties) +{ + // Do nothing +} + +//============================================================================== +template +void AddonWithProtectedProperties:: +setAddonProperties(const Addon::Properties& someProperties) +{ + setProperties(static_cast(someProperties)); +} + +//============================================================================== +template +const Addon::Properties* +AddonWithProtectedProperties:: +getAddonProperties() const +{ + return &mProperties; +} + +//============================================================================== +template +void AddonWithProtectedProperties:: +setProperties(const PropertiesData& properties) +{ + static_cast(mProperties) = properties; + UpdateProperties(static_cast(this)); +} + +//============================================================================== +template +auto AddonWithProtectedProperties:: +getProperties() const -> const Properties& +{ + return mProperties; +} + +//============================================================================== +template +std::unique_ptr +AddonWithProtectedProperties:: +cloneAddon(AddonManager* newManager) const +{ + DART_COMMON_CAST_NEW_MANAGER_TYPE_AND_RETURN_NULL_IF_BAD( + Base, ManagerType, newManager, castedManager, clone); + return std::unique_ptr(new Base(newManager, mProperties)); +} + +//============================================================================== +template +AddonWithProtectedStateAndProperties:: +AddonWithProtectedStateAndProperties( + AddonManager* mgr, const StateData& state, const PropertiesData& properties) + : Addon(mgr), + mState(state), + mProperties(properties) +{ + // Do nothing +} + +//============================================================================== +template +AddonWithProtectedStateAndProperties:: +AddonWithProtectedStateAndProperties( + AddonManager* mgr, const PropertiesData& properties, const StateData& state) + : Addon(mgr), + mState(state), + mProperties(properties) +{ + // Do nothing +} + +//============================================================================== +template +void AddonWithProtectedStateAndProperties:: +setAddonState(const Addon::State& otherState) +{ + setState(static_cast(otherState)); +} + +//============================================================================== +template +const Addon::State* AddonWithProtectedStateAndProperties< + Base, StateData, PropertiesData, ManagerType, + updateState, updateProperties>::getAddonState() const +{ + return &mState; +} + +//============================================================================== +template +void AddonWithProtectedStateAndProperties< + Base, StateData, PropertiesData, ManagerType, + updateState, updateProperties>::setState(const StateData& state) +{ + static_cast(mState) = state; + UpdateState(static_cast(this)); +} + +//============================================================================== +template +auto AddonWithProtectedStateAndProperties< + Base, StateData, PropertiesData, ManagerType, + updateState, updateProperties>::getState() const -> const State& +{ + return mState; +} + +//============================================================================== +template +void AddonWithProtectedStateAndProperties:: +setAddonProperties(const Addon::Properties& otherProperties) +{ + setProperties(static_cast(otherProperties)); +} + +//============================================================================== +template +const Addon::Properties* AddonWithProtectedStateAndProperties< + Base, StateData, PropertiesData, ManagerType, + updateState, updateProperties>::getAddonProperties() const +{ + return &mProperties; +} + +//============================================================================== +template +void AddonWithProtectedStateAndProperties< + Base, StateData, PropertiesData, ManagerType, + updateState, updateProperties>:: +setProperties(const PropertiesData& properties) +{ + static_cast(mProperties) = properties; + UpdateProperties(static_cast(this)); +} + +//============================================================================== +template +auto AddonWithProtectedStateAndProperties< + Base, StateData, PropertiesData, ManagerType, + updateState, updateProperties>:: +getProperties() const -> const Properties& +{ + return mProperties; +} + +//============================================================================== +template +std::unique_ptr +AddonWithProtectedStateAndProperties:: +cloneAddon(AddonManager* newManager) const +{ + DART_COMMON_CAST_NEW_MANAGER_TYPE_AND_RETURN_NULL_IF_BAD( + Base, ManagerType, newManager, castedManager, clone); + return std::unique_ptr(new Base(castedManager, mState, mProperties)); +} + +} // namespace common +} // namespace dart + +#endif // DART_COMMON_DETAIL_ADDON_H_ diff --git a/dart/common/detail/AddonManager.h b/dart/common/detail/AddonManager.h new file mode 100644 index 0000000000000..4e185babbf79a --- /dev/null +++ b/dart/common/detail/AddonManager.h @@ -0,0 +1,170 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COMMON_DETAIL_ADDONMANAGER_H_ +#define DART_COMMON_DETAIL_ADDONMANAGER_H_ + +#include "dart/common/AddonManager.h" + +#define DART_COMMON_CHECK_ILLEGAL_ADDON_ERASE( Func, it, ReturnType )\ + if(it ->second && !it ->second->isOptional(this))\ + {\ + dterr << "[AddonManager::" #Func << "] Illegal request to remove Addon!\n";\ + assert(false);\ + return ReturnType ;\ + } + +namespace dart { +namespace common { + +//============================================================================== +template +bool AddonManager::has() const +{ + return (get() != nullptr); +} + +//============================================================================== +template +T* AddonManager::get() +{ + AddonMap::iterator it = mAddonMap.find( typeid(T) ); + if(mAddonMap.end() == it) + return nullptr; + + return static_cast(it->second.get()); +} + +//============================================================================== +template +const T* AddonManager::get() const +{ + return const_cast(this)->get(); +} + +//============================================================================== +template +void AddonManager::set(const T* addon) +{ + if(addon) + { + mAddonMap[typeid(T)] = addon->cloneAddon(this); + becomeManager(mAddonMap[typeid(T)].get(), false); + } + else + { + mAddonMap[typeid(T)] = nullptr; + } +} + +//============================================================================== +template +void AddonManager::set(std::unique_ptr&& addon) +{ + mAddonMap[typeid(T)] = std::move(addon); + becomeManager(mAddonMap[typeid(T)].get(), true); +} + +//============================================================================== +template +T* AddonManager::create(Args&&... args) +{ + T* addon = new T(this, std::forward(args)...); + mAddonMap[typeid(T)] = std::unique_ptr(addon); + becomeManager(addon, false); + + return addon; +} + +//============================================================================== +template +void AddonManager::erase() +{ + AddonMap::iterator it = mAddonMap.find( typeid(T) ); + DART_COMMON_CHECK_ILLEGAL_ADDON_ERASE(erase, it, DART_BLANK) + if(mAddonMap.end() != it) + it->second = nullptr; +} + +//============================================================================== +template +std::unique_ptr AddonManager::release() +{ + std::unique_ptr extraction = nullptr; + AddonMap::iterator it = mAddonMap.find( typeid(T) ); + DART_COMMON_CHECK_ILLEGAL_ADDON_ERASE(release, it, nullptr) + if(mAddonMap.end() != it) + extraction = std::unique_ptr(static_cast(it->second.release())); + + return extraction; +} + +//============================================================================== +template +constexpr bool AddonManager::isSpecializedFor() +{ + return false; +} + +} // namespace common +} // namespace dart + +//============================================================================== +// Create non-template alternatives to AddonManager functions +#define DART_BAKE_SPECIALIZED_ADDON_IRREGULAR( TypeName, AddonName )\ + inline bool has ## AddonName () const\ + { return has(); }\ + inline TypeName * get ## AddonName ()\ + { return get(); }\ + inline const TypeName* get ## AddonName () const\ + { return get(); }\ + inline void set ## AddonName (const TypeName * addon)\ + { set(addon); }\ + inline void set ## AddonName (std::unique_ptr< TypeName >&& addon)\ + { set(std::move(addon)); }\ + template \ + inline TypeName * create ## AddonName (Args&&... args)\ + { return create(std::forward(args)...); }\ + inline void erase ## AddonName ()\ + { erase(); }\ + inline std::unique_ptr< TypeName > release ## AddonName ()\ + { return release(); } + +//============================================================================== +#define DART_BAKE_SPECIALIZED_ADDON(AddonName)\ + DART_BAKE_SPECIALIZED_ADDON_IRREGULAR(AddonName, AddonName); + +#endif // DART_COMMON_DETAIL_ADDONMANAGER_H_ diff --git a/dart/common/detail/AddonManagerJoiner.h b/dart/common/detail/AddonManagerJoiner.h new file mode 100644 index 0000000000000..7e0e3b9418ddb --- /dev/null +++ b/dart/common/detail/AddonManagerJoiner.h @@ -0,0 +1,126 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COMMON_DETAIL_ADDONMANAGERJOINER_H_ +#define DART_COMMON_DETAIL_ADDONMANAGERJOINER_H_ + +#include "dart/common/AddonManagerJoiner.h" +#include "dart/common/detail/TemplateJoinerDispatchMacro.h" + +namespace dart { +namespace common { + +//============================================================================== +template +template +AddonManagerJoiner::AddonManagerJoiner( + Base1Arg&& arg1, Base2Args&&... args2) + : Base1(std::forward(arg1)), + Base2(std::forward(args2)...) +{ + // Do nothing +} + +//============================================================================== +template +template +AddonManagerJoiner::AddonManagerJoiner( + Base1Arg&& arg1, NoArg_t) + : Base1(std::forward(arg1)), + Base2() +{ + // Do nothing +} + +//============================================================================== +template +template +AddonManagerJoiner::AddonManagerJoiner( + NoArg_t, Base2Args&&... args2) + : Base1(), + Base2(std::forward(args2)...) +{ + // Do nothing +} + +//============================================================================== +DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(bool, AddonManagerJoiner, has, () const, ()) +DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(T*, AddonManagerJoiner, get, (), ()) +DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(const T*, AddonManagerJoiner, get, () const, ()) +DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(void, AddonManagerJoiner, set, (const T* addon), (addon)) +DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(void, AddonManagerJoiner, set, (std::unique_ptr&& addon), (std::move(addon))) +DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(void, AddonManagerJoiner, erase, (), ()) +DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(std::unique_ptr, AddonManagerJoiner, release, (), ()) + +//============================================================================== +// Because this function requires a comma inside of its template argument list, +// it is not easily fit into a macro like the other functions, so we just +// implement it explicitly. +template +template +T* AddonManagerJoiner::create(Args&&... args) +{ + if(Base1::template isSpecializedFor()) + return Base1::template create(std::forward(args)...); + + return Base2::template create(std::forward(args)...); +} + +//============================================================================== +template +template +constexpr bool AddonManagerJoiner::isSpecializedFor() +{ + return (Base1::template isSpecializedFor() + || Base2::template isSpecializedFor()); +} + +//============================================================================== +template +template +AddonManagerJoiner::AddonManagerJoiner( + Args&&... args) + : AddonManagerJoiner>( + std::forward(args)...) +{ + // Do nothing +} + +} // namespace common +} // namespace dart + +#endif // DART_COMMON_DETAIL_ADDONMANAGERJOINER_H_ + diff --git a/dart/common/detail/Extensible.h b/dart/common/detail/Extensible.h new file mode 100644 index 0000000000000..f61120a0caba7 --- /dev/null +++ b/dart/common/detail/Extensible.h @@ -0,0 +1,324 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COMMON_DETAIL_EXTENSIBLE_H_ +#define DART_COMMON_DETAIL_EXTENSIBLE_H_ + +#include "dart/common/Extensible.h" + +namespace dart { +namespace common { + +//============================================================================== +template +ExtensibleMixer::ExtensibleMixer() +{ + // Do nothing +} + +//============================================================================== +template +template +ExtensibleMixer::ExtensibleMixer(Args&&... args) + : Mixin(std::forward(args)...) +{ + // Do nothing +} + +//============================================================================== +template +ExtensibleMixer::ExtensibleMixer(const Mixin& mixin) + : Mixin(mixin) +{ + // Do nothing +} + +//============================================================================== +template +ExtensibleMixer::ExtensibleMixer(Mixin&& mixin) + : Mixin(std::move(mixin)) +{ + // Do nothing +} + +//============================================================================== +template +ExtensibleMixer::ExtensibleMixer( + const ExtensibleMixer& other) + : Mixin(other) +{ + // Do nothing +} + +//============================================================================== +template +ExtensibleMixer::ExtensibleMixer(ExtensibleMixer&& other) + : Mixin(other) +{ + // Do nothing +} + +//============================================================================== +template +ExtensibleMixer& ExtensibleMixer::operator=( + const Mixin& mixin) +{ + static_cast(*this) = mixin; + return *this; +} + +//============================================================================== +template +ExtensibleMixer& ExtensibleMixer::operator=(Mixin&& mixin) +{ + static_cast(*this) = std::move(mixin); + return *this; +} + +//============================================================================== +template +ExtensibleMixer& ExtensibleMixer::operator=( + const ExtensibleMixer& other) +{ + static_cast(*this) = static_cast(other); + return *this; +} + +//============================================================================== +template +ExtensibleMixer& ExtensibleMixer::operator=( + ExtensibleMixer&& other) +{ + static_cast(*this) = std::move(static_cast(other)); +} + +//============================================================================== +template +std::unique_ptr ExtensibleMixer::clone() const +{ + return std::unique_ptr(new ExtensibleMixer(*this)); +} + +//============================================================================== +template +void ExtensibleMixer::copy(const T& other) +{ + *this = static_cast&>(other); +} + +//============================================================================== +template +ExtensibleMapHolder::ExtensibleMapHolder( + const ExtensibleMapHolder& otherHolder) +{ + *this = otherHolder; +} + +//============================================================================== +template +ExtensibleMapHolder::ExtensibleMapHolder( + ExtensibleMapHolder&& otherHolder) +{ + *this = std::move(otherHolder); +} + +//============================================================================== +template +ExtensibleMapHolder::ExtensibleMapHolder(const MapType& otherMap) +{ + *this = otherMap; +} + +//============================================================================== +template +ExtensibleMapHolder::ExtensibleMapHolder(MapType&& otherMap) +{ + *this = std::move(otherMap); +} + +//============================================================================== +template +ExtensibleMapHolder& ExtensibleMapHolder::operator=( + const ExtensibleMapHolder& otherHolder) +{ + *this = otherHolder.getMap(); + + return *this; +} + +//============================================================================== +template +ExtensibleMapHolder& ExtensibleMapHolder::operator=( + ExtensibleMapHolder&& otherHolder) +{ + mMap = std::move(otherHolder.mMap); + + return *this; +} + +//============================================================================== +template +ExtensibleMapHolder& ExtensibleMapHolder::operator=( + const MapType& otherMap) +{ + typename MapType::iterator receiver = mMap.begin(); + typename MapType::const_iterator sender = otherMap.begin(); + + while( otherMap.end() != sender ) + { + if( mMap.end() == receiver ) + { + // If we've reached the end of this ExtensibleMapHolder's map, then we + // should just add each entry + mMap[sender->first] = sender->second->clone(); + ++sender; + } + else if( receiver->first == sender->first ) + { + // We should copy the incoming object when possible so we can avoid the + // memory allocation overhead of cloning. + if(receiver->second) + receiver->second->copy(*sender->second); + else + receiver->second = sender->second->clone(); + + ++receiver; + ++sender; + } + else if( receiver->first < sender->first ) + { + // Clear this entry in the map, because it does not have an analog in the + // map that we are copying + receiver->second = nullptr; + ++receiver; + } + else + { + mMap[sender->first] = sender->second->clone(); + ++sender; + } + } + + while( mMap.end() != receiver ) + { + mMap.erase(receiver++); + } + + return *this; +} + +//============================================================================== +template +ExtensibleMapHolder& ExtensibleMapHolder::operator=( + MapType&& otherHolder) +{ + mMap = std::move(otherHolder); + + return *this; +} + +//============================================================================== +template +MapType& ExtensibleMapHolder::getMap() +{ + return mMap; +} + +//============================================================================== +template +const MapType& ExtensibleMapHolder::getMap() const +{ + return mMap; +} + +//============================================================================== +template +ExtensibleVector::ExtensibleVector(const std::vector& regularVector) + : mVector(regularVector) +{ + // Do nothing +} + +//============================================================================== +template +ExtensibleVector::ExtensibleVector(std::vector&& regularVector) +{ + mVector = std::move(regularVector); +} + +//============================================================================== +template +std::unique_ptr< ExtensibleVector > ExtensibleVector::clone() const +{ + std::vector clonedVector; + clonedVector.reserve(mVector.size()); + + for(const T& entry : mVector) + clonedVector.push_back(entry->clone()); + + return std::unique_ptr< ExtensibleVector >( + new ExtensibleVector(std::move(clonedVector)) ); +} + +//============================================================================== +template +void ExtensibleVector::copy(const ExtensibleVector& anotherVector) +{ + const std::vector& other = anotherVector.getVector(); + mVector.resize(other.size()); + + for(size_t i=0; i < other.size(); ++i) + { + if(mVector[i] && other[i]) + mVector[i]->copy(*other[i]); + else if(other[i]) + mVector[i] = other[i]->clone(); + else + mVector[i] = nullptr; + } +} + +//============================================================================== +template +const std::vector& ExtensibleVector::getVector() const +{ + return mVector; +} + +} // namespace common +} // namespace dart + +#endif // DART_COMMON_DETAIL_EXTENSIBLE_H_ diff --git a/dart/common/detail/NameManager.h b/dart/common/detail/NameManager.h index 224f2d0fe6ad7..0e55febebb23b 100644 --- a/dart/common/detail/NameManager.h +++ b/dart/common/detail/NameManager.h @@ -41,6 +41,7 @@ #include #include #include "dart/common/Console.h" +#include "dart/common/NameManager.h" namespace dart { namespace common { @@ -57,13 +58,6 @@ NameManager::NameManager(const std::string& _managerName, // Do nothing } -//============================================================================== -template -NameManager::~NameManager() -{ - // Do nothing -} - //============================================================================== template bool NameManager::setPattern(const std::string& _newPattern) diff --git a/dart/common/detail/NoOp.h b/dart/common/detail/NoOp.h new file mode 100644 index 0000000000000..b71d48e836a98 --- /dev/null +++ b/dart/common/detail/NoOp.h @@ -0,0 +1,62 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COMMON_DETAIL_NOOP_H_ +#define DART_COMMON_DETAIL_NOOP_H_ + +namespace dart { +namespace common { +namespace detail { + +/// NoOp is short for "no operation". This is a placeholder function that can be +/// used in places where a function needs to be provided but you do not actually +/// want the function to do anything. +template +void NoOp(Args... /*args*/) +{ + // Do nothing +} + +} // namespace detail +} // namespace common +} // namespace dart + +// The DART_BLANK macro can be passed into macro arguments where you want the +// argument to expand into blank space. +#define DART_BLANK // leave blank + + +#endif // DART_COMMON_DETAIL_NOOP_H_ diff --git a/dart/common/detail/SpecializedAddonManager.h b/dart/common/detail/SpecializedAddonManager.h new file mode 100644 index 0000000000000..e125a6ec1aa2c --- /dev/null +++ b/dart/common/detail/SpecializedAddonManager.h @@ -0,0 +1,329 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COMMON_DETAIL_SPECIALIZEDADDONMANAGER_H_ +#define DART_COMMON_DETAIL_SPECIALIZEDADDONMANAGER_H_ + +#include "dart/common/SpecializedAddonManager.h" + +// This preprocessor token should only be used by the unittest that is +// responsible for checking that the specialized routines are being used to +// access specialized Addons +#ifdef DART_UNITTEST_SPECIALIZED_ADDON_ACCESS +bool usedSpecializedAddonAccess; +#endif // DART_UNITTEST_SPECIALIZED_ADDON_ACCESS + +namespace dart { +namespace common { + +//============================================================================== +template +SpecializedAddonManager::SpecializedAddonManager() +{ + mAddonMap[typeid( SpecAddon )] = nullptr; + mSpecAddonIterator = mAddonMap.find(typeid( SpecAddon )); +} + +//============================================================================== +template +template +bool SpecializedAddonManager::has() const +{ + return _has(type()); +} + +//============================================================================== +template +template +T* SpecializedAddonManager::get() +{ + return _get(type()); +} + +//============================================================================== +template +template +const T* SpecializedAddonManager::get() const +{ + return _get(type()); +} + +//============================================================================== +template +template +void SpecializedAddonManager::set(const T* addon) +{ + _set(type(), addon); +} + +//============================================================================== +template +template +void SpecializedAddonManager::set(std::unique_ptr&& addon) +{ + _set(type(), std::move(addon)); +} + +//============================================================================== +template +template +T* SpecializedAddonManager::create(Args&&... args) +{ + return _create(type(), std::forward(args)...); +} + +//============================================================================== +template +template +void SpecializedAddonManager::erase() +{ + _erase(type()); +} + +//============================================================================== +template +template +std::unique_ptr SpecializedAddonManager::release() +{ + return _release(type()); +} + +//============================================================================== +template +template +constexpr bool SpecializedAddonManager::isSpecializedFor() +{ + return _isSpecializedFor(type()); +} + +//============================================================================== +template +template +bool SpecializedAddonManager::_has(type) const +{ +#ifdef DART_UNITTEST_SPECIALIZED_ADDON_ACCESS + usedSpecializedAddonAccess = true; +#endif // DART_UNITTEST_SPECIALIZED_ADDON_ACCESS + + return AddonManager::has(); +} + +//============================================================================== +template +bool SpecializedAddonManager::_has(type) const +{ +#ifdef DART_UNITTEST_SPECIALIZED_ADDON_ACCESS + usedSpecializedAddonAccess = true; +#endif // DART_UNITTEST_SPECIALIZED_ADDON_ACCESS + + return (mSpecAddonIterator->second.get() != nullptr); +} + +//============================================================================== +template +template +T* SpecializedAddonManager::_get(type) +{ + return AddonManager::get(); +} + +//============================================================================== +template +SpecAddon* SpecializedAddonManager::_get(type) +{ +#ifdef DART_UNITTEST_SPECIALIZED_ADDON_ACCESS + usedSpecializedAddonAccess = true; +#endif // DART_UNITTEST_SPECIALIZED_ADDON_ACCESS + + return static_cast(mSpecAddonIterator->second.get()); +} + +//============================================================================== +template +template +const T* SpecializedAddonManager::_get(type) const +{ + return AddonManager::get(); +} + +//============================================================================== +template +const SpecAddon* SpecializedAddonManager::_get(type) const +{ +#ifdef DART_UNITTEST_SPECIALIZED_ADDON_ACCESS + usedSpecializedAddonAccess = true; +#endif // DART_UNITTEST_SPECIALIZED_ADDON_ACCESS + + return static_cast(mSpecAddonIterator->second.get()); +} + +//============================================================================== +template +template +void SpecializedAddonManager::_set(type, const T* addon) +{ + AddonManager::set(addon); +} + +//============================================================================== +template +void SpecializedAddonManager::_set( + type, const SpecAddon* addon) +{ +#ifdef DART_UNITTEST_SPECIALIZED_ADDON_ACCESS + usedSpecializedAddonAccess = true; +#endif // DART_UNITTEST_SPECIALIZED_ADDON_ACCESS + + if(addon) + { + mSpecAddonIterator->second = addon->cloneAddon(this); + becomeManager(mSpecAddonIterator->second.get(), false); + } + else + { + mSpecAddonIterator->second = nullptr; + } +} + +//============================================================================== +template +template +void SpecializedAddonManager::_set(type, std::unique_ptr&& addon) +{ + AddonManager::set(std::move(addon)); +} + +//============================================================================== +template +void SpecializedAddonManager::_set( + type, std::unique_ptr&& addon) +{ +#ifdef DART_UNITTEST_SPECIALIZED_ADDON_ACCESS + usedSpecializedAddonAccess = true; +#endif // DART_UNITTEST_SPECIALIZED_ADDON_ACCESS + + mSpecAddonIterator->second = std::move(addon); + becomeManager(mSpecAddonIterator->second.get(), true); +} + +//============================================================================== +template +template +T* SpecializedAddonManager::_create(type, Args&&... args) +{ + return AddonManager::create(std::forward(args)...); +} + +//============================================================================== +template +template +SpecAddon* SpecializedAddonManager::_create( + type, Args&&... args) +{ +#ifdef DART_UNITTEST_SPECIALIZED_ADDON_ACCESS + usedSpecializedAddonAccess = true; +#endif // DART_UNITTEST_SPECIALIZED_ADDON_ACCESS + + SpecAddon* addon = new SpecAddon(this, std::forward(args)...); + mSpecAddonIterator->second = std::unique_ptr(addon); + becomeManager(addon, false); + + return addon; +} + +//============================================================================== +template +template +void SpecializedAddonManager::_erase(type) +{ + AddonManager::erase(); +} + +//============================================================================== +template +void SpecializedAddonManager::_erase(type) +{ +#ifdef DART_UNITTEST_SPECIALIZED_ADDON_ACCESS + usedSpecializedAddonAccess = true; +#endif // DART_UNITTEST_SPECIALIZED_ADDON_ACCESS + + DART_COMMON_CHECK_ILLEGAL_ADDON_ERASE(erase, mSpecAddonIterator, DART_BLANK); + mSpecAddonIterator->second = nullptr; +} + +//============================================================================== +template +template +std::unique_ptr SpecializedAddonManager::_release(type) +{ + return AddonManager::release(); +} + +//============================================================================== +template +std::unique_ptr SpecializedAddonManager::_release( + type) +{ +#ifdef DART_UNITTEST_SPECIALIZED_ADDON_ACCESS + usedSpecializedAddonAccess = true; +#endif // DART_UNITTEST_SPECIALIZED_ADDON_ACCESS + + DART_COMMON_CHECK_ILLEGAL_ADDON_ERASE(release, mSpecAddonIterator, nullptr); + std::unique_ptr extraction( + static_cast(mSpecAddonIterator->second.release())); + + return extraction; +} + +//============================================================================== +template +template +constexpr bool SpecializedAddonManager::_isSpecializedFor(type) +{ + return false; +} + +//============================================================================== +template +constexpr bool SpecializedAddonManager::_isSpecializedFor(type) +{ + return true; +} + +} // namespace common +} // namespace dart + +#endif // DART_COMMON_DETAIL_SPECIALIZEDADDONMANAGER_H_ diff --git a/dart/common/detail/TemplateJoinerDispatchMacro.h b/dart/common/detail/TemplateJoinerDispatchMacro.h new file mode 100644 index 0000000000000..6f894c87f8980 --- /dev/null +++ b/dart/common/detail/TemplateJoinerDispatchMacro.h @@ -0,0 +1,57 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COMMON_DETAIL_TEMPLATEJOINERDISPATCHMACRO_H_ +#define DART_COMMON_DETAIL_TEMPLATEJOINERDISPATCHMACRO_H_ + +//============================================================================== +/// This macro provides the implementation for most of the member functions in +/// common::AddonManagerJoiner, dynamics::NodeManagerJoinerForBodyNode, and +/// NodeManagerJoinerForSkeleton. The member functions of those classes share +/// essentially the same logic, so it makes sense to have a single macro that +/// provides the implementation for all of them. +#define DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(ReturnType, ClassName, Function, Suffix, Args)\ + template \ + template \ + ReturnType ClassName :: Function Suffix\ + {\ + if(Base1::template isSpecializedFor())\ + return Base1::template Function Args ;\ + \ + return Base2::template Function Args ;\ + } + +#endif // DART_COMMON_DETAIL_TEMPLATEJOINERDISPATCHMACRO_H_ diff --git a/dart/dynamics/Addon.h b/dart/dynamics/Addon.h new file mode 100644 index 0000000000000..4c4b84aa3950a --- /dev/null +++ b/dart/dynamics/Addon.h @@ -0,0 +1,372 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_DYNAMICS_ADDON_H_ +#define DART_DYNAMICS_ADDON_H_ + +#include "dart/common/Addon.h" +#include "dart/dynamics/SmartPointer.h" + +namespace dart { +namespace dynamics { + +//============================================================================== +/// AddonWithProtectedPropertiesInSkeleton generates implementations of the +/// Property managing functions for an Addon class which is embedded within a +/// Skeleton or a component of a Skeleton (such as a BodyNode, Joint, or custom +/// Node class). This will increment the version count any time the +/// Addon::setProperties function is called. +template , + bool OptionalT = true> +class AddonWithProtectedPropertiesInSkeleton : public common::Addon +{ +public: + + using Base = BaseT; + using PropertiesData = PropertiesDataT; + using ManagerType = ManagerT; + using Properties = common::Addon::PropertiesMixer; + constexpr static void (*UpdateProperties)(Base*) = updateProperties; + constexpr static bool Optional = OptionalT; + + using Implementation = AddonWithProtectedPropertiesInSkeleton< + BaseT, PropertiesDataT, ManagerT, updateProperties, OptionalT>; + + AddonWithProtectedPropertiesInSkeleton() = delete; + AddonWithProtectedPropertiesInSkeleton( + const AddonWithProtectedPropertiesInSkeleton&) = delete; + + /// Construct using a PropertiesData instance + AddonWithProtectedPropertiesInSkeleton( + common::AddonManager* mgr, + const PropertiesData& properties = PropertiesData()); + + // Documentation inherited + std::unique_ptr cloneAddon( + common::AddonManager* newManager) const override final; + + // Documentation inherited + void setAddonProperties(const Addon::Properties& someProperties) override final; + + // Documentation inherited + const Addon::Properties* getAddonProperties() const override final; + + /// Set the Properties of this Addon + void setProperties(const PropertiesData& properties); + + /// Get the Properties of this Addon + const Properties& getProperties() const; + + // Documentation inherited + bool isOptional(common::AddonManager* oldManager) override final; + + /// Get the Skeleton that this Addon is embedded in + SkeletonPtr getSkeleton(); + + /// Get the Skeleton that this Addon is embedded in + ConstSkeletonPtr getSkeleton() const; + + /// Get the AddonManager that this Addon is embedded in + ManagerType* getManager(); + + /// Get the AddonManager that this Addon is embedded in + const ManagerType* getManager() const; + + /// Increment the version number of the Skeleton this Addon is attached to + void incrementSkeletonVersion(); + +protected: + + // Documentation inherited + void setManager(common::AddonManager* newManager, bool transfer) override; + + /// Properties of this Addon + Properties mProperties; + + /// Manager that this Addon is embedded in + ManagerType* mManager; + +}; + +//============================================================================== +/// AddonWithProtectedStateAndPropertiesInSkeleton combines the +/// AddonWithProtectedState and AddonWithProtectedPropertiesInSkeleton classes +/// into a single templated class. This should be the base class of any Addon +/// with both a State and Properties that will be embedded in a Skeleton or a +/// component of a Skeleton (such as a BodyNode, Joint, or custom Node). +template , + void (*updateProperties)(BaseT*) = updateState, + bool OptionalT = true> +class AddonWithProtectedStateAndPropertiesInSkeleton : public common::Addon +{ +public: + + using Base = BaseT; + using StateData = StateDataT; + using PropertiesData = PropertiesDataT; + using ManagerType = ManagerT; + using State = common::Addon::StateMixer; + using Properties = common::Addon::PropertiesMixer; + constexpr static void (*UpdateState)(Base*) = updateState; + constexpr static void (*UpdateProperties)(Base*) = updateProperties; + constexpr static bool Optional = OptionalT; + + AddonWithProtectedStateAndPropertiesInSkeleton() = delete; + AddonWithProtectedStateAndPropertiesInSkeleton( + const AddonWithProtectedStateAndPropertiesInSkeleton&) = delete; + + /// Construct using a StateData and a PropertiesData instance + AddonWithProtectedStateAndPropertiesInSkeleton( + common::AddonManager* mgr, + const StateDataT& state = StateData(), + const PropertiesDataT& properties = PropertiesData()); + + /// Construct using a StateData and a PropertiesData instance, flipped + AddonWithProtectedStateAndPropertiesInSkeleton( + common::AddonManager* mgr, + const PropertiesData& properties, + const StateData& state = StateData()); + + // Documentation inherited + std::unique_ptr cloneAddon( + common::AddonManager* newManager) const override final; + + // Documentation inherited + void setAddonState(const Addon::State& otherState) override final; + + // Documentation inherited + const Addon::State* getAddonState() const override final; + + /// Set the State of this Addon + void setState(const StateData& state); + + /// Get the State of this Addon + const State& getState() const; + + // Documentation inherited + void setAddonProperties(const Addon::Properties& properties) override final; + + // Documentation inherited + const Addon::Properties* getAddonProperties() const override final; + + /// Set the Properties of this Addon + void setProperties(const PropertiesData& properties); + + /// Get the Properties of this Addon + const Properties& getProperties() const; + + // Documentation inherited + bool isOptional(common::AddonManager* oldManager) override final; + + /// Get the Skeleton that this Addon is embedded in + SkeletonPtr getSkeleton(); + + /// Get the Skeleton that this Addon is embedded in + ConstSkeletonPtr getSkeleton() const; + + /// Get the AddonManager that this Addon is embedded in + ManagerType* getManager(); + + /// Get the AddonManager that this Addon is embedded in + const ManagerType* getManager() const; + + /// Increment the version number of the Skeleton this Addon is attached to + void incrementSkeletonVersion(); + +protected: + + // Documentation inherited + void setManager(common::AddonManager* newManager, bool transfer) override; + + /// State of this Addon + State mState; + + /// Properties of this Addon + Properties mProperties; + + /// Manager that this Addon is embedded in + ManagerType* mManager; + +}; + +} // namespace dynamics +} // namespace dart + +//============================================================================== +#define DART_DYNAMICS_ADDON_PROPERTY_CONSTRUCTOR( ClassName, UpdatePropertiesMacro )\ + ClassName (const ClassName &) = delete;\ + inline ClassName (dart::common::AddonManager* mgr, const PropertiesData& properties)\ + : AddonWithProtectedPropertiesInSkeleton< Base, PropertiesData, ManagerType, UpdatePropertiesMacro, Optional>(mgr, properties) { } + +//============================================================================== +#define DART_DYNAMICS_JOINT_ADDON_CONSTRUCTOR( ClassName )\ + DART_DYNAMICS_ADDON_PROPERTY_CONSTRUCTOR( ClassName, &detail::JointPropertyUpdate ) + +//============================================================================== +#define DART_DYNAMICS_ADDON_STATE_PROPERTY_CONSTRUCTORS( ClassName, UpdateStateMacro, UpdatePropertiesMacro )\ + ClassName (const ClassName &) = delete;\ + inline ClassName (dart::common::AddonManager* mgr, const StateData& state = StateData(), const PropertiesData& properties = PropertiesData())\ + : AddonWithProtectedStateAndPropertiesInSkeleton< Base, StateData, PropertiesData, ManagerType, UpdateStateMacro, UpdatePropertiesMacro, Optional >(mgr, state, properties) { }\ + inline ClassName (dart::common::AddonManager* mgr, const PropertiesData& properties, const StateData state = StateData())\ + : AddonWithProtectedStateAndPropertiesInSkeleton< Base, StateData, PropertiesData, ManagerType, UpdateStateMacro, UpdatePropertiesMacro, Optional >(mgr, properties, state) { } + +//============================================================================== +#define DART_DYNAMICS_SET_ADDON_PROPERTY_CUSTOM( Type, Name, Update )\ + inline void set ## Name (const Type & value)\ + { mProperties.m ## Name = value; Update(this); incrementSkeletonVersion(); } + +//============================================================================== +#define DART_DYNAMICS_SET_ADDON_PROPERTY( Type, Name )\ + DART_DYNAMICS_SET_ADDON_PROPERTY_CUSTOM( Type, Name, UpdateProperties ) + +//============================================================================== +#define DART_DYNAMICS_GET_ADDON_PROPERTY( Type, Name )\ + inline const Type& get ## Name () const\ + { return mProperties.m ## Name; } + +//============================================================================== +#define DART_DYNAMICS_SET_GET_ADDON_PROPERTY( Type, Name )\ + DART_DYNAMICS_SET_ADDON_PROPERTY( Type, Name )\ + DART_DYNAMICS_GET_ADDON_PROPERTY( Type, Name ) + +//============================================================================== +#define DART_DYNAMICS_SET_ADDON_PROPERTY_ARRAY( Class, SingleType, VectorType, SingleName, PluralName, Size, UpdatePrefix )\ + void set ## SingleName (size_t index, const SingleType & value)\ + {\ + if( index >= Size )\ + {\ + dterr << "[" #Class << "::set" #SingleName << "] Invalid index (" << index << "). "\ + << "The specified index must be less than " #Size << "!\n";\ + assert(false); return;\ + }\ + this->mProperties.m ## PluralName [index] = value;\ + UpdatePrefix :: UpdateProperties(this);\ + this->incrementSkeletonVersion();\ + }\ + void set ## PluralName (const VectorType & vec)\ + {\ + this->mProperties.m ## PluralName = vec;\ + UpdatePrefix :: UpdateProperties(this);\ + this->incrementSkeletonVersion();\ + } + +//============================================================================== +#define DART_DYNAMICS_GET_ADDON_PROPERTY_ARRAY(Class, SingleType, VectorType, SingleName, PluralName, Size)\ + inline const SingleType& get ## SingleName (size_t index) const\ + {\ + if(index >= Size)\ + {\ + dterr << "[" #Class << "::get" #SingleName << "] Invalid index (" << index << "). "\ + << "The specified index must be less than " #Size << "!\n";\ + assert(false); index = 0;\ + }\ + return this->mProperties.m ## PluralName [index];\ + }\ + inline const VectorType& get ## PluralName () const\ + {\ + return this->mProperties.m ## PluralName;\ + } + +//============================================================================== +#define DART_DYNAMICS_IRREGULAR_SET_GET_ADDON_PROPERTY_ARRAY( Class, SingleType, VectorType, SingleName, PluralName, Size, UpdatePrefix )\ + DART_DYNAMICS_SET_ADDON_PROPERTY_ARRAY( Class, SingleType, VectorType, SingleName, PluralName, Size, UpdatePrefix )\ + DART_DYNAMICS_GET_ADDON_PROPERTY_ARRAY( Class, SingleType, VectorType, SingleName, PluralName, Size ) + +//============================================================================== +#define DART_DYNAMICS_SET_GET_ADDON_PROPERTY_ARRAY( Class, SingleType, VectorType, SingleName, Size, UpdatePrefix )\ + DART_DYNAMICS_IRREGULAR_SET_GET_ADDON_PROPERTY_ARRAY( Class, SingleType, VectorType, SingleName, SingleName ## s, Size, UpdatePrefix ) + +//============================================================================== +#define DART_DYNAMICS_IRREGULAR_SET_GET_MULTIDOF_ADDON( SingleType, VectorType, SingleName, PluralName )\ + DART_DYNAMICS_IRREGULAR_SET_GET_ADDON_PROPERTY_ARRAY( MultiDofJointAddon, SingleType, VectorType, SingleName, PluralName, DOF, MultiDofJoint::Addon ) + +//============================================================================== +#define DART_DYNAMICS_SET_GET_MULTIDOF_ADDON( SingleType, VectorType, SingleName )\ + DART_DYNAMICS_IRREGULAR_SET_GET_MULTIDOF_ADDON( SingleType, VectorType, SingleName, SingleName ## s ) + +//============================================================================== +#define DETAIL_DART_ADDON_PROPERTIES_UPDATE( AddonName, GetAddon )\ + AddonName :: UpdateProperties( GetAddon () );\ + GetAddon ()->incrementSkeletonVersion(); + +//============================================================================== +#define DETAIL_DART_ADDON_STATE_PROPERTIES_UPDATE( AddonName, GetAddon )\ + AddonName :: UpdateState( GetAddon () );\ + DETAIL_DART_ADDON_PROPERTIES_UPDATE( AddonName, GetAddon ); + +//============================================================================== +// Used for Addons that have Properties (but no State) inside of a Skeleton +#define DART_DYNAMICS_SKEL_PROPERTIES_ADDON_INLINE( AddonName )\ + DETAIL_DART_SPECIALIZED_ADDON_INLINE( AddonName,\ + DETAIL_DART_ADDON_PROPERTIES_UPDATE( AddonName, get ## AddonName ) ) + +//============================================================================== +// Used for Addons that have both State and Properties inside of a Skeleton +#define DART_DYNAMICS_SKEL_ADDON_INLINE( AddonName )\ + DETAIL_DART_SPECIALIZED_ADDON_INLINE( AddonName,\ + DETAIL_DART_ADDON_STATE_PROPERTIES_UPDATE( AddonName, get ## AddonName ) ) + +//============================================================================== +// Used for edge cases, such as nested template classes, that have Properties +// (but no State) inside of a Skeleton +#define DART_DYNAMICS_IRREGULAR_SKEL_PROPERTIES_ADDON_INLINE( TypeName, HomogenizedName )\ + DETAIL_DART_IRREGULAR_SPECIALIZED_ADDON_INLINE( TypeName, HomogenizedName,\ + DETAIL_DART_ADDON_PROPERTIES_UPDATE( TypeName, get ## HomogenizedName ) ) + +//============================================================================== +// Used for edge cases, such as nested template classes, that have both State +// and Properties inside of a Skeleton +#define DART_DYNAMICS_IRREGULAR_SKEL_ADDON_INLINE( TypeName, HomogenizedName )\ + DETAIL_DART_IRREGULAR_SPECIALIZED_ADDON_INLINE( TypeName, HomogenizedName,\ + DETAIL_DART_ADDON_STATE_PROPERTIES_UPDATE( TypeName, get ## HomogenizedName ) ) + +//============================================================================== +// Used for nested-class Addons that have Properties (but no State) inside of a Skeleton +#define DART_DYNAMICS_NESTED_SKEL_PROPERTIES_ADDON_INLINE( ParentName, AddonName )\ + DART_DYNAMICS_IRREGULAR_SKEL_PROPERTIES_ADDON_INLINE( ParentName :: AddonName, ParentName ## AddonName ) + +//============================================================================== +// Used for nested-class Addons that have both State and Properties inside of a Skeleton +#define DART_DYNAMICS_NESTED_SKEL_ADDON_INLINE( ParentName, AddonName )\ + DART_DYNAMICS_IRREGULAR_SKEL_ADDON_INLINE( ParentName :: AddonName, ParentName ## AddonName ) + +#include "dart/dynamics/Skeleton.h" +#include "dart/dynamics/detail/Addon.h" + +#endif // DART_DYNAMICS_ADDON_H_ diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index acbce6ec78901..70ea9b4e45dd8 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -50,6 +50,7 @@ #include "dart/dynamics/Chain.h" #include "dart/dynamics/Marker.h" #include "dart/dynamics/SoftBodyNode.h" +#include "dart/dynamics/EndEffector.h" namespace dart { namespace dynamics { @@ -109,17 +110,6 @@ ConstSkeletonPtr SkeletonRefCountingBase::getSkeleton() const //============================================================================== typedef std::set EntityPtrSet; -//============================================================================== -template -static T getVectorObjectIfAvailable(size_t _index, const std::vector& _vec) -{ - assert(_index < _vec.size()); - if(_index < _vec.size()) - return _vec[_index]; - - return nullptr; -} - //============================================================================== size_t BodyNode::msBodyNodeCount = 0; @@ -148,6 +138,29 @@ BodyNode::Properties::Properties(const Entity::Properties& _entityProperties, // Do nothing } +//============================================================================== +BodyNode::ExtendedProperties::ExtendedProperties( + const Properties& standardProperties, + const NodeProperties& nodeProperties, + const AddonProperties& addonProperties) + : Properties(standardProperties), + mNodeProperties(nodeProperties), + mAddonProperties(addonProperties) +{ + // Do nothing +} + +//============================================================================== +BodyNode::ExtendedProperties::ExtendedProperties( + Properties&& standardProperties, + NodeProperties&& nodeProperties, + AddonProperties&& addonProperties) + : Properties(std::move(standardProperties)) +{ + mNodeProperties = std::move(nodeProperties); + mAddonProperties = std::move(addonProperties); +} + //============================================================================== BodyNode::~BodyNode() { @@ -164,6 +177,61 @@ BodyNode::~BodyNode() delete mParentJoint; } +//============================================================================== +void BodyNode::setProperties(const ExtendedProperties& _properties) +{ + setProperties(static_cast(_properties)); + + setProperties(_properties.mNodeProperties); + + setProperties(_properties.mAddonProperties); +} + +//============================================================================== +void BodyNode::setProperties(const NodeProperties& _properties) +{ + const NodePropertiesMap& propertiesMap = _properties.getMap(); + + NodeMap::iterator node_it = mNodeMap.begin(); + NodePropertiesMap::const_iterator prop_it = propertiesMap.begin(); + + while( mNodeMap.end() != node_it && propertiesMap.end() != prop_it ) + { + if( node_it->first == prop_it->first ) + { + if( prop_it->second ) + { + const std::vector& node_vec = node_it->second; + const std::vector< std::unique_ptr >& prop_vec = + prop_it->second->getVector(); + size_t stop = std::min(node_vec.size(), prop_vec.size()); + for(size_t i=0; i < stop; ++i) + { + if(prop_vec[i]) + node_vec[i]->setNodeProperties(*prop_vec[i]); + } + } + + ++node_it; + ++prop_it; + } + else if( node_it->first < prop_it->first ) + { + ++node_it; + } + else + { + ++prop_it; + } + } +} + +//============================================================================== +void BodyNode::setProperties(const AddonProperties& _properties) +{ + setAddonProperties(_properties); +} + //============================================================================== void BodyNode::setProperties(const Properties& _properties) { @@ -200,13 +268,46 @@ BodyNode::Properties BodyNode::getBodyNodeProperties() const return BodyNode::Properties(mEntityP, mBodyP); } +//============================================================================== +BodyNode::NodeProperties BodyNode::getAttachedNodeProperties() const +{ + // TODO(MXG): Make a version of this function that will fill in a + // NodeProperties instance instead of creating a new one + NodePropertiesMap nodeProperties; + + for(const auto& entry : mNodeMap) + { + const std::vector& nodes = entry.second; + std::vector< std::unique_ptr > vec; + for(size_t i=0; i < nodes.size(); ++i) + { + std::unique_ptr prop = nodes[i]->getNodeProperties(); + if(prop) + vec.push_back(std::move(prop)); + } + + nodeProperties[entry.first] = std::unique_ptr( + new NodePropertiesVector(std::move(vec))); + } + + return nodeProperties; +} + +//============================================================================== +BodyNode::ExtendedProperties BodyNode::getExtendedProperties() const +{ + return ExtendedProperties(getBodyNodeProperties(), + getAttachedNodeProperties(), + getAddonProperties()); +} + //============================================================================== void BodyNode::copy(const BodyNode& _otherBodyNode) { if(this == &_otherBodyNode) return; - setProperties(_otherBodyNode.getBodyNodeProperties()); + setProperties(_otherBodyNode.getExtendedProperties()); } //============================================================================== @@ -225,6 +326,42 @@ BodyNode& BodyNode::operator=(const BodyNode& _otherBodyNode) return *this; } +//============================================================================== +void BodyNode::duplicateNodes(const BodyNode* otherBodyNode) +{ + if(nullptr == otherBodyNode) + { + dterr << "[BodyNode::duplicateNodes] You have asked to duplicate the Nodes " + << "of a nullptr, which is not allowed!\n"; + assert(false); + return; + } + + const NodeMap& otherMap = otherBodyNode->mNodeMap; + for(const auto& vec : otherMap) + { + for(const auto& node : vec.second) + node->cloneNode(this)->attach(); + } +} + +//============================================================================== +void BodyNode::matchNodes(const BodyNode* otherBodyNode) +{ + if(nullptr == otherBodyNode) + { + dterr << "[BodyNode::matchNodes] You have asked to match the Nodes of a " + << "nullptr, which is not allowed!\n"; + assert(false); + return; + } + + for(auto& cleaner : mNodeDestructors) + cleaner->getNode()->stageForRemoval(); + + duplicateNodes(otherBodyNode); +} + //============================================================================== const std::string& BodyNode::setName(const std::string& _name) { @@ -736,31 +873,26 @@ const BodyNode* BodyNode::getChildBodyNode(size_t _index) const } //============================================================================== -size_t BodyNode::getNumEndEffectors() const +size_t BodyNode::getNumChildJoints() const { - return mEndEffectors.size(); + return mChildBodyNodes.size(); } //============================================================================== -EndEffector* BodyNode::getEndEffector(size_t _index) +Joint* BodyNode::getChildJoint(size_t _index) { - return getVectorObjectIfAvailable(_index, mEndEffectors); -} + BodyNode* childBodyNode = getChildBodyNode(_index); -//============================================================================== -const EndEffector* BodyNode::getEndEffector(size_t _index) const -{ - return getVectorObjectIfAvailable(_index, mEndEffectors); + if(childBodyNode) + return childBodyNode->getParentJoint(); + else + return nullptr; } //============================================================================== -EndEffector* BodyNode::createEndEffector( - const EndEffector::Properties& _properties) +const Joint* BodyNode::getChildJoint(size_t _index) const { - EndEffector* ee = new EndEffector(this, _properties); - getSkeleton()->registerEndEffector(ee); - - return ee; + return const_cast(this)->getChildJoint(_index); } //============================================================================== @@ -769,30 +901,13 @@ EndEffector* BodyNode::createEndEffector(const std::string& _name) EndEffector::Properties properties; properties.mName = _name; - return createEndEffector(properties); -} - -//============================================================================== -size_t BodyNode::getNumChildJoints() const -{ - return mChildBodyNodes.size(); -} - -//============================================================================== -Joint* BodyNode::getChildJoint(size_t _index) -{ - BodyNode* childBodyNode = getChildBodyNode(_index); - - if(childBodyNode) - return childBodyNode->getParentJoint(); - else - return nullptr; + return createNode(properties); } //============================================================================== -const Joint* BodyNode::getChildJoint(size_t _index) const +EndEffector* BodyNode::createEndEffector(const char* _name) { - return const_cast(this)->getChildJoint(_index); + return createEndEffector(std::string(_name)); } //============================================================================== @@ -1065,7 +1180,7 @@ BodyNode::BodyNode(BodyNode* _parentBodyNode, Joint* _parentJoint, const Properties& _properties) : Entity(ConstructFrame), Frame(Frame::World(), ""), // Name gets set later by setProperties - Node(ConstructBodyNode), + TemplatedJacobianNode(this), mID(BodyNode::msBodyNodeCount++), mIsColliding(false), mParentJoint(_parentJoint), @@ -1099,6 +1214,7 @@ BodyNode::BodyNode(BodyNode* _parentBodyNode, Joint* _parentJoint, // double-delete this BodyNode when it gets destroyed. mSelfDestructor = std::shared_ptr(new NodeDestructor(nullptr)); mDestructor = mSelfDestructor; + mAmAttached = true; mParentJoint->mChildBodyNode = this; setProperties(_properties); @@ -1108,9 +1224,27 @@ BodyNode::BodyNode(BodyNode* _parentBodyNode, Joint* _parentJoint, } //============================================================================== -BodyNode* BodyNode::clone(BodyNode* _parentBodyNode, Joint* _parentJoint) const +BodyNode* BodyNode::clone(BodyNode* _parentBodyNode, Joint* _parentJoint, + bool cloneNodes) const +{ + BodyNode* clonedBn = + new BodyNode(_parentBodyNode, _parentJoint, getBodyNodeProperties()); + + clonedBn->matchAddons(this); + + if(cloneNodes) + clonedBn->matchNodes(this); + + return clonedBn; +} + +//============================================================================== +Node* BodyNode::cloneNode(BodyNode* /*bn*/) const { - return new BodyNode(_parentBodyNode, _parentJoint, getBodyNodeProperties()); + dterr << "[BodyNode::cloneNode] This function should never be called! Please " + << "report this as an error!\n"; + assert(false); + return nullptr; } //============================================================================== diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h index 7aea341b40aee..4b7d8df8f340d 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -40,7 +40,6 @@ #include #include -#include #include #include @@ -51,11 +50,10 @@ #include "dart/dynamics/Node.h" #include "dart/dynamics/Frame.h" #include "dart/dynamics/Inertia.h" -#include "dart/dynamics/Skeleton.h" #include "dart/dynamics/Marker.h" #include "dart/dynamics/SmartPointer.h" #include "dart/dynamics/TemplatedJacobianNode.h" -#include "dart/dynamics/EndEffector.h" +#include "dart/dynamics/SpecializedNodeManager.h" const double DART_DEFAULT_FRICTION_COEFF = 1.0; const double DART_DEFAULT_RESTITUTION_COEFF = 0.0; @@ -73,6 +71,7 @@ class GenCoord; class Skeleton; class Joint; class DegreeOfFreedom; +class EndEffector; class Shape; class Marker; @@ -84,6 +83,8 @@ class Marker; /// BodyNode inherits Frame, and a parent Frame of a BodyNode is the parent /// BodyNode of the BodyNode. class BodyNode : + public virtual common::AddonManager, + public virtual SpecializedNodeManagerForBodyNode, public SkeletonRefCountingBase, public TemplatedJacobianNode { @@ -146,20 +147,62 @@ class BodyNode : virtual ~Properties() = default; }; + using NodePropertiesVector = common::ExtensibleVector< std::unique_ptr >; + using NodePropertiesMap = std::map< std::type_index, std::unique_ptr >; + using NodeProperties = common::ExtensibleMapHolder; + using AddonProperties = common::AddonManager::Properties; + + struct ExtendedProperties : Properties + { + /// Composed constructor + ExtendedProperties( + const Properties& standardProperties = Properties(), + const NodeProperties& nodeProperties = NodeProperties(), + const AddonProperties& addonProperties = AddonProperties()); + + /// Composed move constructor + ExtendedProperties( + Properties&& standardProperties, + NodeProperties&& nodeProperties, + AddonProperties&& addonProperties); + + /// Properties of all the Nodes attached to this BodyNode + NodeProperties mNodeProperties; + + /// Properties of all the Addons attached to this BodyNode + AddonProperties mAddonProperties; + }; + BodyNode(const BodyNode&) = delete; /// Destructor virtual ~BodyNode(); + /// Set the ExtendedProperties of this BodyNode + void setProperties(const ExtendedProperties& _properties); + + /// Set the Properties of the attached Nodes + void setProperties(const NodeProperties& _properties); + + /// Same as setAddonProperties() + void setProperties(const AddonProperties& _properties); + /// Set the Properties of this BodyNode void setProperties(const Properties& _properties); - /// Set the Properties of this BodyNode + /// Set the UniqueProperties of this BodyNode void setProperties(const UniqueProperties& _properties); /// Get the Properties of this BodyNode Properties getBodyNodeProperties() const; + /// Get the the Properties of the Nodes attached to this BodyNode + NodeProperties getAttachedNodeProperties() const; + + /// The the full extended Properties of this BodyNode, including the + /// Properties of its Addons, its attached Nodes, and the BodyNode itself. + ExtendedProperties getExtendedProperties() const; + /// Copy the Properties of another BodyNode void copy(const BodyNode& _otherBodyNode); @@ -169,6 +212,13 @@ class BodyNode : /// Same as copy(const BodyNode&) BodyNode& operator=(const BodyNode& _otherBodyNode); + /// Give this BodyNode a copy of each Node from otherBodyNode + void duplicateNodes(const BodyNode* otherBodyNode); + + /// Make the Nodes of this BodyNode match the Nodes of otherBodyNode. All + /// existing Nodes in this BodyNode will be removed. + void matchNodes(const BodyNode* otherBodyNode); + /// Set name. If the name is already taken, this will return an altered /// version which will be used by the Skeleton const std::string& setName(const std::string& _name) override; @@ -559,21 +609,21 @@ class BodyNode : /// Return the (const) _index-th child Joint of this BodyNode const Joint* getChildJoint(size_t _index) const; - /// Return the number of EndEffectors attached to this BodyNode - size_t getNumEndEffectors() const; - - /// Return an EndEffector attached to this BodyNode - EndEffector* getEndEffector(size_t _index); + /// Create some Node type and attach it to this BodyNode. + template + NodeType* createNode(Args&&... args); - /// Return an EndEffector attached to this BodyNode - const EndEffector* getEndEffector(size_t _index) const; - - /// Create an EndEffector attached to this BodyNode - EndEffector* createEndEffector(const EndEffector::Properties& _properties); + /// Create an EndEffector attached to this BodyNode. Pass an + /// EndEffector::Properties argument into this function. + template + EndEffector* createEndEffector(const EndEffectorProperties& _properties); /// Create an EndEffector with the specified name EndEffector* createEndEffector(const std::string& _name = "EndEffector"); + /// Create an EndEffector with the specified name + EndEffector* createEndEffector(const char* _name); + /// Add a marker into the bodynode void addMarker(Marker* _marker); @@ -839,7 +889,11 @@ class BodyNode : /// Create a clone of this BodyNode. This may only be called by the Skeleton /// class. - virtual BodyNode* clone(BodyNode* _parentBodyNode, Joint* _parentJoint) const; + virtual BodyNode* clone(BodyNode* _parentBodyNode, Joint* _parentJoint, + bool cloneNodes) const; + + /// This is needed in order to inherit the Node class, but it does nothing + Node* cloneNode(BodyNode* bn) const override final; /// Initialize the vector members with proper sizes. virtual void init(const SkeletonPtr& _skeleton); @@ -1036,15 +1090,9 @@ class BodyNode : /// allows some performance optimizations. std::set mNonBodyNodeEntities; - /// List of EndEffectors that are attached to this BodyNode - std::vector mEndEffectors; - /// List of markers associated std::vector mMarkers; - /// Map that retrieves the destructors for a given Node - std::unordered_map> mNodeMap; - /// A increasingly sorted list of dependent dof indices. std::vector mDependentGenCoordIndices; @@ -1185,9 +1233,11 @@ class BodyNode : }; -#include "dart/dynamics/detail/BodyNode.h" - } // namespace dynamics } // namespace dart +#include "dart/dynamics/Skeleton.h" +// These headers need to be included after the BodyNode class is defined in +// order for the header dependencies to work out correctly. + #endif // DART_DYNAMICS_BODYNODE_H_ diff --git a/dart/dynamics/CMakeLists.txt b/dart/dynamics/CMakeLists.txt index 097b804be9b75..a4b9c174ba4a2 100644 --- a/dart/dynamics/CMakeLists.txt +++ b/dart/dynamics/CMakeLists.txt @@ -1,5 +1,5 @@ # Search all header and source files -file(GLOB srcs "*.cpp") +file(GLOB srcs "*.cpp" "detail/*.cpp") file(GLOB hdrs "*.h") file(GLOB detail "detail/*.h") diff --git a/dart/dynamics/EndEffector.cpp b/dart/dynamics/EndEffector.cpp index 371dc44fc69f0..f51713ab7cd6f 100644 --- a/dart/dynamics/EndEffector.cpp +++ b/dart/dynamics/EndEffector.cpp @@ -37,96 +37,92 @@ #include "dart/common/Console.h" #include "dart/dynamics/EndEffector.h" #include "dart/dynamics/BodyNode.h" -#include "dart/dynamics/Skeleton.h" namespace dart { namespace dynamics { -//============================================================================== -Support::Support(EndEffector* _ee) - : mActive(false), - mEndEffector(_ee) -{ - if(nullptr == mEndEffector) - { - dterr << "[Support::Support] It is not permissible to construct a Support " - << "with a nullptr EndEffector!\n"; - assert(false); - } -} +namespace detail { -//============================================================================== -void Support::setGeometry(const math::SupportGeometry& _newSupport) +void SupportUpdate(Support* support) { - mGeometry = _newSupport; - mEndEffector->getSkeleton()->notifySupportUpdate( - mEndEffector->getTreeIndex()); + if(EndEffector* ee = support->getManager()) + ee->getSkeleton()->notifySupportUpdate(ee->getTreeIndex()); } -//============================================================================== -const math::SupportGeometry& Support::getGeometry() const -{ - return mGeometry; -} +} // namespace detail //============================================================================== void Support::setActive(bool _supporting) { - if(mActive == _supporting) + if(mState.mActive == _supporting) return; - mActive = _supporting; - mEndEffector->getSkeleton()->notifySupportUpdate( - mEndEffector->getTreeIndex()); + mState.mActive = _supporting; + UpdateState(this); } //============================================================================== bool Support::isActive() const { - return mActive; + return mState.mActive; } //============================================================================== -EndEffector::UniqueProperties::UniqueProperties(const Eigen::Isometry3d& _defaultTransform, - const math::SupportGeometry& _supportGeometry, bool _supporting) +EndEffector::StateData::StateData( + const Eigen::Isometry3d& relativeTransform, + const common::AddonManager::State& addonStates) + : mRelativeTransform(relativeTransform), + mAddonStates(addonStates) +{ + // Do nothing +} + +//============================================================================== +EndEffector::UniqueProperties::UniqueProperties( + const Eigen::Isometry3d& _defaultTransform) : mDefaultTransform(_defaultTransform) { // Do nothing } //============================================================================== -EndEffector::Properties::Properties( +EndEffector::PropertiesData::PropertiesData( const Entity::Properties& _entityProperties, - const UniqueProperties& _effectorProperties) + const UniqueProperties& _effectorProperties, + const common::AddonManager::Properties& _addonProperties) : Entity::Properties(_entityProperties), - UniqueProperties(_effectorProperties) + UniqueProperties(_effectorProperties), + mAddonProperties(_addonProperties) { // Do nothing } //============================================================================== -EndEffector::~EndEffector() +void EndEffector::setState(const StateData& _state) { - size_t index = mIndexInBodyNode; - assert(mBodyNode->mEndEffectors[index] == this); - mBodyNode->mEndEffectors.erase(mBodyNode->mEndEffectors.begin() + index); + setRelativeTransform(_state.mRelativeTransform); + setAddonStates(_state.mAddonStates); +} - for(size_t i=index; imEndEffectors.size(); ++i) - { - EndEffector* ee = mBodyNode->mEndEffectors[i]; - ee->mIndexInBodyNode = i; - } +//============================================================================== +void EndEffector::setState(StateData&& _state) +{ + setRelativeTransform(_state.mRelativeTransform); + setAddonStates(std::move(_state.mAddonStates)); +} - SkeletonPtr skel = getSkeleton(); - if(skel) - skel->unregisterEndEffector(this); +//============================================================================== +EndEffector::StateData EndEffector::getEndEffectorState() const +{ + return StateData(mRelativeTf, getAddonStates()); } //============================================================================== -void EndEffector::setProperties(const Properties& _properties, bool _useNow) +void EndEffector::setProperties(const PropertiesData& _properties, bool _useNow) { Entity::setProperties(_properties); setProperties(static_cast(_properties), _useNow); + setAddonProperties(_properties.mAddonProperties); } //============================================================================== @@ -137,9 +133,10 @@ void EndEffector::setProperties(const UniqueProperties& _properties, } //============================================================================== -EndEffector::Properties EndEffector::getEndEffectorProperties() const +EndEffector::PropertiesData EndEffector::getEndEffectorProperties() const { - return Properties(getEntityProperties(), mEndEffectorP); + return PropertiesData(getEntityProperties(), mEndEffectorP, + getAddonProperties()); } //============================================================================== @@ -148,11 +145,8 @@ void EndEffector::copy(const EndEffector& _otherEndEffector) if(this == &_otherEndEffector) return; + setState(_otherEndEffector.getEndEffectorState()); setProperties(_otherEndEffector.getEndEffectorProperties()); - - // We should also copy the relative transform, because it could be different - // than the default relative transform - setRelativeTransform(_otherEndEffector.getRelativeTransform()); } //============================================================================== @@ -178,15 +172,74 @@ const std::string& EndEffector::setName(const std::string& _name) if(mEntityP.mName == _name && !_name.empty()) return mEntityP.mName; - // Remove the current name entry and add a new name entry - getSkeleton()->mNameMgrForEndEffectors.removeName(mEntityP.mName); - mEntityP.mName = _name; - getSkeleton()->addEntryToEndEffectorNameMgr(this); + mEntityP.mName = registerNameChange(_name); // Return the resulting name, after it has been checked for uniqueness return mEntityP.mName; } +//============================================================================== +void EndEffector::setNodeState(const Node::State& otherState) +{ + setState(static_cast(otherState)); +} + +//============================================================================== +std::unique_ptr EndEffector::getNodeState() const +{ + return std::unique_ptr( + new EndEffector::State(getEndEffectorState())); +} + +//============================================================================== +void EndEffector::copyNodeStateTo(std::unique_ptr& outputState) const +{ + if(outputState) + { + EndEffector::State* state = + static_cast(outputState.get()); + + state->mRelativeTransform = mRelativeTf; + copyAddonStatesTo(state->mAddonStates); + } + else + { + outputState = getNodeState(); + } +} + +//============================================================================== +void EndEffector::setNodeProperties(const Node::Properties& otherProperties) +{ + setProperties(static_cast(otherProperties)); +} + +//============================================================================== +std::unique_ptr EndEffector::getNodeProperties() const +{ + return std::unique_ptr( + new EndEffector::Properties(getEndEffectorProperties())); +} + +//============================================================================== +void EndEffector::copyNodePropertiesTo( + std::unique_ptr& outputProperties) const +{ + if(outputProperties) + { + EndEffector::Properties* properties = + static_cast(outputProperties.get()); + + static_cast(*properties) = getEntityProperties(); + static_cast(*properties) = mEndEffectorP; + copyAddonPropertiesTo(properties->mAddonProperties); + } + else + { + outputProperties = getNodeProperties(); + } +} + //============================================================================== void EndEffector::setRelativeTransform(const Eigen::Isometry3d& _newRelativeTf) { @@ -215,29 +268,10 @@ void EndEffector::resetRelativeTransform() //============================================================================== Support* EndEffector::getSupport(bool _createIfNull) { - if(nullptr == mSupport && _createIfNull) + if(nullptr == getSupport() && _createIfNull) createSupport(); - return mSupport.get(); -} - -//============================================================================== -const Support* EndEffector::getSupport() const -{ - return mSupport.get(); -} - -//============================================================================== -Support* EndEffector::createSupport() -{ - mSupport = std::unique_ptr(new Support(this)); - return mSupport.get(); -} - -//============================================================================== -void EndEffector::eraseSupport() -{ - mSupport = nullptr; + return getSupport(); } //============================================================================== @@ -312,30 +346,6 @@ const std::vector EndEffector::getChainDofs() const return mBodyNode->getChainDofs(); } -//============================================================================== -BodyNode* EndEffector::getParentBodyNode() -{ - return mBodyNode; -} - -//============================================================================== -const BodyNode* EndEffector::getParentBodyNode() const -{ - return mBodyNode; -} - -//============================================================================== -size_t EndEffector::getIndexInSkeleton() const -{ - return mIndexInSkeleton; -} - -//============================================================================== -size_t EndEffector::getTreeIndex() const -{ - return mBodyNode->getTreeIndex(); -} - //============================================================================== const math::Jacobian& EndEffector::getJacobian() const { @@ -392,27 +402,26 @@ void EndEffector::notifyVelocityUpdate() } //============================================================================== -EndEffector::EndEffector(BodyNode* _parent, const Properties& _properties) +EndEffector::EndEffector(BodyNode* _parent, const PropertiesData& _properties) : Entity(ConstructFrame), Frame(_parent, ""), - Node(ConstructNode, _parent), FixedFrame(_parent, "", _properties.mDefaultTransform), - mIndexInSkeleton(0), - mIndexInBodyNode(0) - + TemplatedJacobianNode(_parent) { setProperties(_properties); - - _parent->mEndEffectors.push_back(this); - mIndexInBodyNode = _parent->mEndEffectors.size()-1; } //============================================================================== -EndEffector* EndEffector::clone(BodyNode* _parent) const +Node* EndEffector::cloneNode(BodyNode* _parent) const { - EndEffector* ee = new EndEffector(_parent, Properties()); + EndEffector* ee = new EndEffector(_parent, PropertiesData()); + ee->duplicateAddons(this); + ee->copy(this); + if(mIK) + ee->mIK = mIK->clone(ee); + return ee; } diff --git a/dart/dynamics/EndEffector.h b/dart/dynamics/EndEffector.h index 1309a2b3b0d2e..5c0e5ace58893 100644 --- a/dart/dynamics/EndEffector.h +++ b/dart/dynamics/EndEffector.h @@ -39,7 +39,8 @@ #include "dart/dynamics/FixedFrame.h" #include "dart/dynamics/TemplatedJacobianNode.h" -//#include "dart/math/Geometry.h" +#include "dart/dynamics/Addon.h" +#include "dart/common/SpecializedAddonManager.h" namespace dart { namespace dynamics { @@ -48,20 +49,44 @@ class BodyNode; class Skeleton; class EndEffector; -class Support // Inherit the Addon class once it is implemented +class Support; +namespace detail +{ + +struct SupportStateData +{ + inline SupportStateData(bool active = false) : mActive(active) { } + + bool mActive; +}; + +struct SupportPropertiesData +{ + math::SupportGeometry mGeometry; +}; + +void SupportUpdate(Support* support); + +} // namespace detail + +class Support final : + public AddonWithProtectedStateAndPropertiesInSkeleton< + Support, + detail::SupportStateData, + detail::SupportPropertiesData, + EndEffector, + &detail::SupportUpdate> { public: - /// Constructor - Support(EndEffector* _ee); + DART_DYNAMICS_ADDON_STATE_PROPERTY_CONSTRUCTORS( Support, &detail::SupportUpdate, &detail::SupportUpdate ) - /// Set the support geometry for this EndEffector. The SupportGeometry + /// Set/Get the support geometry for this EndEffector. The SupportGeometry /// represents points in the EndEffector frame that can be used for contact /// when solving balancing or manipulation constraints. - void setGeometry(const math::SupportGeometry& _newSupport); - - /// Get a const-reference to the SupportGeometry for this EndEffector - const math::SupportGeometry& getGeometry() const; + DART_DYNAMICS_SET_GET_ADDON_PROPERTY(math::SupportGeometry, Geometry) + // void setGeometry(const math::SupportGeometry&); + // const math::SupportGeometry& getGeometry() const; /// Pass in true if this EndEffector should be used to support the robot, like /// a foot @@ -70,79 +95,122 @@ class Support // Inherit the Addon class once it is implemented /// Get whether this EndEffector is currently being used for support bool isActive() const; -protected: - - /// The support geometry that this EndEffector is designed to use - math::SupportGeometry mGeometry; - - /// True if this EndEffector is currently usable for support - bool mActive; - - /// EndEffector that this support is associated with - EndEffector* mEndEffector; - }; -class EndEffector : public FixedFrame, - public AccessoryNode, - public TemplatedJacobianNode +class EndEffector final : + public virtual common::SpecializedAddonManager, + public FixedFrame, + public AccessoryNode, + public TemplatedJacobianNode { public: friend class Skeleton; friend class BodyNode; + struct StateData + { + StateData(const Eigen::Isometry3d& relativeTransform = + Eigen::Isometry3d::Identity(), + const common::AddonManager::State& addonStates = + common::AddonManager::State()); + + /// The current relative transform of the EndEffector + // TODO(MXG): Consider moving this to a FixedFrame::State struct and then + // inheriting that struct + Eigen::Isometry3d mRelativeTransform; + + /// The current states of the EndEffector's Addons + common::AddonManager::State mAddonStates; + }; + + using State = Node::StateMixer; + struct UniqueProperties { /// The relative transform will be set to this whenever /// resetRelativeTransform() is called Eigen::Isometry3d mDefaultTransform; + /// Default constructor UniqueProperties( const Eigen::Isometry3d& _defaultTransform = - Eigen::Isometry3d::Identity(), - const math::SupportGeometry& _supportGeometry = - math::SupportGeometry(), - bool _supporting = false); + Eigen::Isometry3d::Identity()); }; - struct Properties : Entity::Properties, UniqueProperties + struct PropertiesData : Entity::Properties, UniqueProperties { - Properties( + PropertiesData( const Entity::Properties& _entityProperties = Entity::Properties(), - const UniqueProperties& _effectorProperties = UniqueProperties() ); + const UniqueProperties& _effectorProperties = UniqueProperties(), + const common::AddonManager::Properties& _addonProperties = + common::AddonManager::Properties()); + + /// The properties of the EndEffector's Addons + common::AddonManager::Properties mAddonProperties; }; + using Properties = Node::PropertiesMixer; + /// Destructor - virtual ~EndEffector(); + virtual ~EndEffector() = default; //---------------------------------------------------------------------------- /// \{ \name Structural Properties //---------------------------------------------------------------------------- + /// Set the State of this EndEffector. + void setState(const StateData& _state); + + /// Set the State of this EndEffector using move semantics + void setState(StateData&& _state); + + /// Get the State of this EndEffector + StateData getEndEffectorState() const; + /// Set the Properties of this EndEffector. If _useNow is true, the current /// Transform will be set to the new default transform. - void setProperties(const Properties& _properties, bool _useNow=true); + void setProperties(const PropertiesData& _properties, bool _useNow=false); /// Set the Properties of this EndEffector. If _useNow is true, the current /// Transform will be set to the new default transform. - void setProperties(const UniqueProperties& _properties, bool _useNow=true); + void setProperties(const UniqueProperties& _properties, bool _useNow=false); - Properties getEndEffectorProperties() const; + /// Get the Properties of this EndEffector + PropertiesData getEndEffectorProperties() const; - /// Copy the Properties of another EndEffector + /// Copy the State and Properties of another EndEffector void copy(const EndEffector& _otherEndEffector); - /// Copy the Properties of another EndEffector + /// Copy the State and Properties of another EndEffector void copy(const EndEffector* _otherEndEffector); - /// Copy the Properties of another EndEffector + /// Copy the State and Properties of another EndEffector EndEffector& operator=(const EndEffector& _otherEndEffector); /// Set name. If the name is already taken, this will return an altered /// version which will be used by the Skeleton const std::string& setName(const std::string& _name) override; + // Documentation inherited + void setNodeState(const Node::State& otherState) override final; + + // Documentation inherited + std::unique_ptr getNodeState() const override final; + + // Documentation inherited + void copyNodeStateTo(std::unique_ptr& outputState) const override final; + + // Documentation inherited + void setNodeProperties(const Node::Properties& otherProperties) override final; + + // Documentation inherited + std::unique_ptr getNodeProperties() const override final; + + // Documentation inherited + void copyNodePropertiesTo( + std::unique_ptr& outputProperties) const override final; + /// Set the current relative transform of this EndEffector void setRelativeTransform(const Eigen::Isometry3d& _newRelativeTf); @@ -158,19 +226,11 @@ class EndEffector : public FixedFrame, /// be set with setDefaultRelativeTransform() void resetRelativeTransform(); + DART_BAKE_SPECIALIZED_ADDON(Support) + /// Get a pointer to the Support Addon for this EndEffector. If _createIfNull /// is true, then the Support will be generated if one does not already exist. - Support* getSupport(bool _createIfNull = false); - - /// Get a pointer to the Support Addon for this EndEffector. - const Support* getSupport() const; - - /// Create a new Support Addon for this EndEffector. If a Support Addon - /// already exists for this EndEffector, it will be deleted and replaced. - Support* createSupport(); - - /// Erase the Support Addon from this EndEffector - void eraseSupport(); + Support* getSupport(bool _createIfNull); // Documentation inherited std::shared_ptr getSkeleton() override; @@ -208,18 +268,6 @@ class EndEffector : public FixedFrame, // Documentation inherited const std::vector getChainDofs() const override; - /// Get the BodyNode that this EndEffector is rigidly attached to - BodyNode* getParentBodyNode(); - - /// Get the BodyNode that this EndEffector is rigidly attached to - const BodyNode* getParentBodyNode() const; - - /// Get the index of this EndEffector within the Skeleton - size_t getIndexInSkeleton() const; - - /// Get the tree index of the BodyNode that this EndEffector is attached to - size_t getTreeIndex() const; - /// \} //---------------------------------------------------------------------------- @@ -265,11 +313,11 @@ class EndEffector : public FixedFrame, protected: /// Constructor used by the Skeleton class - explicit EndEffector(BodyNode* _parent, const Properties& _properties); + explicit EndEffector(BodyNode* _parent, const PropertiesData& _properties); /// Create a clone of this BodyNode. This may only be called by the Skeleton /// class. - virtual EndEffector* clone(BodyNode* _parent) const; + virtual Node* cloneNode(BodyNode* _parent) const override; /// Update the Jacobian of this EndEffector. getJacobian() calls this function /// if mIsEffectorJacobianDirty is true. @@ -291,15 +339,6 @@ class EndEffector : public FixedFrame, /// Properties of this EndEffector UniqueProperties mEndEffectorP; - /// The index of this EndEffector within its Skeleton - size_t mIndexInSkeleton; - - /// The index of this EndEffector within its BodyNode - size_t mIndexInBodyNode; - - /// TODO(MXG): When Addons are implemented, this should be changed - std::unique_ptr mSupport; - /// Cached Jacobian of this EndEffector /// /// Do not use directly! Use getJacobian() to access this quantity diff --git a/dart/dynamics/Entity.h b/dart/dynamics/Entity.h index 32ccd6d21d6be..f585d9bc758b3 100644 --- a/dart/dynamics/Entity.h +++ b/dart/dynamics/Entity.h @@ -43,6 +43,7 @@ #include "dart/common/Subject.h" #include "dart/common/Signal.h" +#include "dart/common/AddonManager.h" #include "dart/dynamics/Shape.h" #include "dart/dynamics/SmartPointer.h" @@ -85,6 +86,8 @@ class Entity : public virtual common::Subject using VizShapeRemovedSignal = VizShapeAddedSignal; + // TODO(MXG): Deprecate this with class the next major version-up, and move + // mName into the properties of inheriting classes. struct Properties { /// Name of the Entity diff --git a/dart/dynamics/EulerJoint.cpp b/dart/dynamics/EulerJoint.cpp index 0458c8cc1014b..563bd6dc7fa97 100644 --- a/dart/dynamics/EulerJoint.cpp +++ b/dart/dynamics/EulerJoint.cpp @@ -46,26 +46,10 @@ namespace dart { namespace dynamics { -//============================================================================== -EulerJoint::UniqueProperties::UniqueProperties(AxisOrder _axisOrder) - : mAxisOrder(_axisOrder) -{ - // Do nothing -} - -//============================================================================== -EulerJoint::Properties::Properties( - const MultiDofJoint<3>::Properties& _multiDofProperties, - const EulerJoint::UniqueProperties& _eulerJointProperties) - : MultiDofJoint<3>::Properties(_multiDofProperties), - EulerJoint::UniqueProperties(_eulerJointProperties) -{ - // Do nothing -} - //============================================================================== EulerJoint::~EulerJoint() { + // Do nothing } //============================================================================== @@ -79,13 +63,13 @@ void EulerJoint::setProperties(const Properties& _properties) //============================================================================== void EulerJoint::setProperties(const UniqueProperties& _properties) { - setAxisOrder(_properties.mAxisOrder); + getEulerJointAddon()->setProperties(_properties); } //============================================================================== EulerJoint::Properties EulerJoint::getEulerJointProperties() const { - return EulerJoint::Properties(getMultiDofJointProperties(), mEulerP); + return EulerJoint::Properties(getMultiDofJointProperties(), getEulerJointAddon()->getProperties()); } //============================================================================== @@ -135,16 +119,16 @@ bool EulerJoint::isCyclic(size_t _index) const //============================================================================== void EulerJoint::setAxisOrder(EulerJoint::AxisOrder _order, bool _renameDofs) { - mEulerP.mAxisOrder = _order; + getEulerJointAddon()->setAxisOrder(_order); if (_renameDofs) updateDegreeOfFreedomNames(); - notifyPositionUpdate(); + // The EulerJoint::Addon will take care of notifying a position update } //============================================================================== EulerJoint::AxisOrder EulerJoint::getAxisOrder() const { - return mEulerP.mAxisOrder; + return getEulerJointAddon()->getAxisOrder(); } //============================================================================== @@ -158,7 +142,7 @@ Eigen::Isometry3d EulerJoint::convertToTransform( Eigen::Isometry3d EulerJoint::convertToTransform( const Eigen::Vector3d &_positions) const { - return convertToTransform(_positions, mEulerP.mAxisOrder); + return convertToTransform(_positions, getAxisOrder()); } //============================================================================== @@ -167,14 +151,14 @@ Eigen::Matrix3d EulerJoint::convertToRotation( { switch (_ordering) { - case AO_XYZ: + case AxisOrder::XYZ: return math::eulerXYZToMatrix(_positions); - case AO_ZYX: + case AxisOrder::ZYX: return math::eulerZYXToMatrix(_positions); default: { dterr << "[EulerJoint::convertToRotation] Invalid AxisOrder specified (" - << _ordering << ")\n"; + << static_cast(_ordering) << ")\n"; return Eigen::Matrix3d::Identity(); } } @@ -184,7 +168,7 @@ Eigen::Matrix3d EulerJoint::convertToRotation( Eigen::Matrix3d EulerJoint::convertToRotation(const Eigen::Vector3d& _positions) const { - return convertToRotation(_positions, mEulerP.mAxisOrder); + return convertToRotation(_positions, getAxisOrder()); } //============================================================================== @@ -209,9 +193,9 @@ Eigen::Matrix EulerJoint::getLocalJacobianStatic( Eigen::Vector6d J1 = Eigen::Vector6d::Zero(); Eigen::Vector6d J2 = Eigen::Vector6d::Zero(); - switch (mEulerP.mAxisOrder) + switch (getAxisOrder()) { - case AO_XYZ: + case AxisOrder::XYZ: { //------------------------------------------------------------------------ // S = [ c1*c2, s2, 0 @@ -237,7 +221,7 @@ Eigen::Matrix EulerJoint::getLocalJacobianStatic( break; } - case AO_ZYX: + case AxisOrder::ZYX: { //------------------------------------------------------------------------ // S = [ -s1, 0, 1 @@ -297,10 +281,13 @@ Eigen::Matrix EulerJoint::getLocalJacobianStatic( //============================================================================== EulerJoint::EulerJoint(const Properties& _properties) - : MultiDofJoint<3>(_properties) + : detail::EulerJointBase(_properties, common::NoArg) { - setProperties(_properties); - updateDegreeOfFreedomNames(); + createEulerJointAddon(_properties); + + // Inherited Joint Properties must be set in the final joint class or else we + // get pure virtual function calls + MultiDofJoint<3>::setProperties(_properties); } //============================================================================== @@ -313,21 +300,21 @@ Joint* EulerJoint::clone() const void EulerJoint::updateDegreeOfFreedomNames() { std::vector affixes; - switch (mEulerP.mAxisOrder) + switch (getAxisOrder()) { - case AO_ZYX: + case AxisOrder::ZYX: affixes.push_back("_z"); affixes.push_back("_y"); affixes.push_back("_x"); break; - case AO_XYZ: + case AxisOrder::XYZ: affixes.push_back("_x"); affixes.push_back("_y"); affixes.push_back("_z"); break; default: dterr << "Unsupported axis order in EulerJoint named '" << mJointP.mName - << "' (" << mEulerP.mAxisOrder << ")\n"; + << "' (" << static_cast(getAxisOrder()) << ")\n"; } if (affixes.size() == 3) @@ -380,9 +367,9 @@ void EulerJoint::updateLocalJacobianTimeDeriv() const Eigen::Vector6d dJ1 = Eigen::Vector6d::Zero(); Eigen::Vector6d dJ2 = Eigen::Vector6d::Zero(); - switch (mEulerP.mAxisOrder) + switch (getAxisOrder()) { - case AO_XYZ: + case AxisOrder::XYZ: { //------------------------------------------------------------------------ // dS = [ -(dq1*c2*s1) - dq2*c1*s2, dq2*c2, 0 @@ -399,7 +386,7 @@ void EulerJoint::updateLocalJacobianTimeDeriv() const break; } - case AO_ZYX: + case AxisOrder::ZYX: { //------------------------------------------------------------------------ // dS = [ -c1*dq1, 0, 0 diff --git a/dart/dynamics/EulerJoint.h b/dart/dynamics/EulerJoint.h index e2686359f394d..391c403d39f33 100644 --- a/dart/dynamics/EulerJoint.h +++ b/dart/dynamics/EulerJoint.h @@ -37,49 +37,24 @@ #ifndef DART_DYNAMICS_EULERJOINT_H_ #define DART_DYNAMICS_EULERJOINT_H_ -#include - -#include "dart/dynamics/MultiDofJoint.h" +#include "dart/dynamics/detail/EulerJointProperties.h" namespace dart { namespace dynamics { /// class EulerJoint -class EulerJoint : public MultiDofJoint<3> +class EulerJoint : public detail::EulerJointBase { public: friend class Skeleton; + using AxisOrder = detail::AxisOrder; + using UniqueProperties = detail::EulerJointUniqueProperties; + using Properties = detail::EulerJointProperties; + using Addon = detail::EulerJointAddon; + using Base = detail::EulerJointBase; - /// Axis order - enum AxisOrder - { - AO_ZYX = 0, - AO_XYZ = 1 - }; - - struct UniqueProperties - { - /// Euler angle order - AxisOrder mAxisOrder; - - /// Constructor - UniqueProperties(AxisOrder _axisOrder = AO_XYZ); - - virtual ~UniqueProperties() = default; - }; - - struct Properties : MultiDofJoint<3>::Properties, EulerJoint::UniqueProperties - { - /// Composed constructor - Properties( - const MultiDofJoint<3>::Properties& _multiDofProperties = - MultiDofJoint<3>::Properties(), - const EulerJoint::UniqueProperties& _eulerJointProperties = - EulerJoint::UniqueProperties()); - - virtual ~Properties() = default; - }; + DART_BAKE_SPECIALIZED_ADDON_IRREGULAR(Addon, EulerJointAddon) EulerJoint(const EulerJoint&) = delete; @@ -134,13 +109,13 @@ class EulerJoint : public MultiDofJoint<3> { switch(_ordering) { - case AO_XYZ: + case AxisOrder::XYZ: return math::matrixToEulerXYZ(_rotation); - case AO_ZYX: + case AxisOrder::ZYX: return math::matrixToEulerZYX(_rotation); default: dtwarn << "[EulerJoint::convertToPositions] Unsupported AxisOrder (" - << _ordering << "), returning a zero vector\n"; + << static_cast(_ordering) << "), returning a zero vector\n"; return Eigen::Vector3d::Zero(); } } @@ -151,7 +126,7 @@ class EulerJoint : public MultiDofJoint<3> template Eigen::Vector3d convertToPositions(const RotationType& _rotation) const { - return convertToPositions(_rotation, mEulerP.mAxisOrder); + return convertToPositions(_rotation, getAxisOrder()); } /// Convert a set of Euler angle positions into a transform @@ -173,6 +148,8 @@ class EulerJoint : public MultiDofJoint<3> Eigen::Matrix getLocalJacobianStatic( const Eigen::Vector3d& _positions) const override; + template friend void detail::JointPropertyUpdate(AddonType*); + protected: /// Constructor called by Skeleton class @@ -196,11 +173,6 @@ class EulerJoint : public MultiDofJoint<3> // Documentation inherited virtual void updateLocalJacobianTimeDeriv() const override; -protected: - - /// EulerJoint Properties - UniqueProperties mEulerP; - public: // To get byte-aligned Eigen vectors EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/dart/dynamics/Frame.cpp b/dart/dynamics/Frame.cpp index 1e50a5c1cf576..c3dcf70bcf64d 100644 --- a/dart/dynamics/Frame.cpp +++ b/dart/dynamics/Frame.cpp @@ -532,6 +532,16 @@ Frame::Frame(Frame* _refFrame, const std::string& _name) changeParentFrame(_refFrame); } +//============================================================================== +Frame::Frame(ConstructAbstract_t) + : Entity(Entity::ConstructAbstract), + mAmWorld(false) +{ + dterr << "[Frame::constructor] You are calling a constructor for the Frame " + << "class which is only meant to be used by pure abstract classes. If " + << "you are seeing this, then there is a bug!\n"; + assert(false); +} //============================================================================== void Frame::changeParentFrame(Frame* _newParentFrame) diff --git a/dart/dynamics/FreeJoint.cpp b/dart/dynamics/FreeJoint.cpp index 46533bfb5d39f..049741f826777 100644 --- a/dart/dynamics/FreeJoint.cpp +++ b/dart/dynamics/FreeJoint.cpp @@ -532,7 +532,6 @@ FreeJoint::FreeJoint(const Properties& _properties) mJacobianDeriv = Eigen::Matrix6d::Zero(); setProperties(_properties); - updateDegreeOfFreedomNames(); } //============================================================================== diff --git a/dart/dynamics/InvalidIndex.h b/dart/dynamics/InvalidIndex.h index 9040a563ef8f3..d5b42b86f2358 100644 --- a/dart/dynamics/InvalidIndex.h +++ b/dart/dynamics/InvalidIndex.h @@ -42,12 +42,7 @@ namespace dart { namespace dynamics { -#if defined(_MSC_VER) -// TODO: Change to constexpr once Visual Studio supports it -const size_t INVALID_INDEX = static_cast(-1); -#else constexpr size_t INVALID_INDEX = static_cast(-1); -#endif } // namespace dynamics } // namespace dart diff --git a/dart/dynamics/JacobianNode.cpp b/dart/dynamics/JacobianNode.cpp index c668ea3e4cc4f..80c907661d017 100644 --- a/dart/dynamics/JacobianNode.cpp +++ b/dart/dynamics/JacobianNode.cpp @@ -41,19 +41,6 @@ namespace dart { namespace dynamics { -//============================================================================== -JacobianNode::JacobianNode() - : Entity(nullptr, "", false), - Frame(nullptr, ""), - Node(Node::ConstructAbstract), - mIsBodyJacobianDirty(true), - mIsWorldJacobianDirty(true), - mIsBodyJacobianSpatialDerivDirty(true), - mIsWorldJacobianClassicDerivDirty(true) -{ - // Do nothing -} - //============================================================================== // This destructor needs to be defined somewhere that the definition of // InverseKinematics is visible, because it's needed by the @@ -95,6 +82,25 @@ void JacobianNode::clearIK() mIK = nullptr; } +//============================================================================== +const std::string& JacobianNode::getName() const +{ + return mEntityP.mName; +} + +//============================================================================== +JacobianNode::JacobianNode(BodyNode* bn) + : Entity(Entity::ConstructAbstract), + Frame(Frame::ConstructAbstract), + Node(bn), + mIsBodyJacobianDirty(true), + mIsWorldJacobianDirty(true), + mIsBodyJacobianSpatialDerivDirty(true), + mIsWorldJacobianClassicDerivDirty(true) +{ + // Do nothing +} + //============================================================================== void JacobianNode::notifyJacobianUpdate() { diff --git a/dart/dynamics/JacobianNode.h b/dart/dynamics/JacobianNode.h index e9f0f059e500e..31a3d7941bff5 100644 --- a/dart/dynamics/JacobianNode.h +++ b/dart/dynamics/JacobianNode.h @@ -54,16 +54,10 @@ class InverseKinematics; /// The JacobianNode class serves as a common interface for BodyNodes and /// EndEffectors to both be used as references for IK modules. This is a pure /// abstract class. -class JacobianNode : public virtual Frame, public virtual Node +class JacobianNode : public virtual Frame, public Node { public: - /// This constructor only exists as a formality, because Entity and Frame - /// require it. Since this class is pure abstract and virutally inherits - /// Frame, the most derived class needs to call the constructor of Entity and - /// Frame anyway. - JacobianNode(); - /// Virtual destructor virtual ~JacobianNode(); @@ -93,6 +87,12 @@ class JacobianNode : public virtual Frame, public virtual Node /// \{ \name Structural Properties //---------------------------------------------------------------------------- + // Documentation inherited + const std::string& setName(const std::string& _name) override = 0; + + // Documentation inherited + const std::string& getName() const override final; + /// Return the Skeleton this JacobianNode is attached to virtual std::shared_ptr getSkeleton() = 0; @@ -285,6 +285,9 @@ class JacobianNode : public virtual Frame, public virtual Node protected: + /// Constructor + JacobianNode(BodyNode* bn); + /// Dirty flag for body Jacobian. mutable bool mIsBodyJacobianDirty; diff --git a/dart/dynamics/Joint.cpp b/dart/dynamics/Joint.cpp index a5fd58b0cd99a..cd4c7b4a22608 100644 --- a/dart/dynamics/Joint.cpp +++ b/dart/dynamics/Joint.cpp @@ -67,6 +67,26 @@ Joint::Properties::Properties(const std::string& _name, // Do nothing } +//============================================================================== +Joint::ExtendedProperties::ExtendedProperties( + const Properties& standardProperties, + const AddonProperties& addonProperties) + : Properties(standardProperties), + mAddonProperties(addonProperties) +{ + // Do nothing +} + +//============================================================================== +Joint::ExtendedProperties::ExtendedProperties( + Properties&& standardProperties, + AddonProperties&& addonProperties) + : Properties(std::move(standardProperties)), + mAddonProperties(std::move(addonProperties)) +{ + // Do nothing +} + //============================================================================== Joint::~Joint() { diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index 82639c3a47bea..69e41e1ed1b62 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -44,6 +44,7 @@ #include "dart/common/Deprecated.h" #include "dart/common/Subject.h" +#include "dart/common/AddonManager.h" #include "dart/math/MathTypes.h" #include "dart/dynamics/SmartPointer.h" @@ -61,9 +62,11 @@ class Skeleton; class DegreeOfFreedom; /// class Joint -class Joint : public virtual common::Subject +class Joint : public virtual common::Subject, + public virtual common::AddonManager { public: + /// Actuator type /// /// The command is taken by setCommand() or setCommands(), and the meaning of @@ -155,6 +158,24 @@ class Joint : public virtual common::Subject EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; + using AddonProperties = common::AddonManager::Properties; + + struct ExtendedProperties : Properties + { + /// Composed constructor + ExtendedProperties( + const Properties& standardProperties = Properties(), + const AddonProperties& addonProperties = AddonProperties()); + + /// Composed move constructor + ExtendedProperties( + Properties&& standardProperties, + AddonProperties&& addonProperties); + + /// Properties of all the Addons attached to this Joint + AddonProperties mAddonProperties; + }; + /// Default actuator type static const ActuatorType DefaultActuatorType; @@ -674,6 +695,22 @@ class Joint : public virtual common::Subject /// \sa BodyNode::updateArticulatedInertia(double). // Eigen::VectorXd getDampingForces() const; + + //---------------------------------------------------------------------------- + /// \{ \name Update Notifiers + //---------------------------------------------------------------------------- + + /// Notify that a position update is needed + void notifyPositionUpdate(); + + /// Notify that a velocity update is needed + void notifyVelocityUpdate(); + + /// Notify that an acceleration update is needed + void notifyAccelerationUpdate(); + + /// \} + //---------------------------------------------------------------------------- // Rendering //---------------------------------------------------------------------------- @@ -891,15 +928,6 @@ class Joint : public virtual common::Subject /// \} - /// Notify that a position update is needed - void notifyPositionUpdate(); - - /// Notify that a velocity update is needed - void notifyVelocityUpdate(); - - /// Notify that an acceleration update is needed - void notifyAccelerationUpdate(); - protected: /// Properties of this Joint @@ -955,10 +983,22 @@ class Joint : public virtual common::Subject mutable bool mIsLocalJacobianTimeDerivDirty; public: + // To get byte-aligned Eigen vectors EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; +namespace detail { + +template +void JointPropertyUpdate(AddonType* addon) +{ + addon->getManager()->notifyPositionUpdate(); + addon->getManager()->updateLocalJacobian(); +} + +} // namespace detail + } // namespace dynamics } // namespace dart diff --git a/dart/dynamics/MultiDofJoint.h b/dart/dynamics/MultiDofJoint.h index 5b1ff7dc8113e..20a001a964697 100644 --- a/dart/dynamics/MultiDofJoint.h +++ b/dart/dynamics/MultiDofJoint.h @@ -47,6 +47,8 @@ #include "dart/dynamics/Joint.h" #include "dart/dynamics/Skeleton.h" #include "dart/dynamics/DegreeOfFreedom.h" +#include "dart/common/SpecializedAddonManager.h" +#include "dart/dynamics/detail/MultiDofJointProperties.h" namespace dart { namespace dynamics { @@ -56,107 +58,22 @@ class Skeleton; /// class MultiDofJoint template -class MultiDofJoint : public Joint +class MultiDofJoint : + public Joint, + public virtual common::SpecializedAddonManager< detail::MultiDofJointAddon > { public: - typedef Eigen::Matrix Vector; + constexpr static size_t NumDofs = DOF; + using Vector = Eigen::Matrix; - MultiDofJoint(const MultiDofJoint&) = delete; - - struct UniqueProperties - { - /// Lower limit of position - Vector mPositionLowerLimits; - - /// Upper limit of position - Vector mPositionUpperLimits; - - /// Initial positions - Vector mInitialPositions; - - /// Min value allowed. - Vector mVelocityLowerLimits; - - /// Max value allowed. - Vector mVelocityUpperLimits; - - /// Initial velocities - Vector mInitialVelocities; - - /// Min value allowed. - Vector mAccelerationLowerLimits; - - /// upper limit of generalized acceleration - Vector mAccelerationUpperLimits; - - /// Min value allowed. - Vector mForceLowerLimits; - - /// Max value allowed. - Vector mForceUpperLimits; - - /// Joint spring stiffness - Vector mSpringStiffnesses; - - /// Rest joint position for joint spring - Vector mRestPositions; - - /// Joint damping coefficient - Vector mDampingCoefficients; + using UniqueProperties = detail::MultiDofJointUniqueProperties; + using Properties = detail::MultiDofJointProperties; + using Addon = detail::MultiDofJointAddon; - /// Joint Coulomb friction - Vector mFrictions; + DART_BAKE_SPECIALIZED_ADDON_IRREGULAR( typename MultiDofJoint::Addon, MultiDofJointAddon ) - /// True if the name of the corresponding DOF is not allowed to be - /// overwritten - std::array mPreserveDofNames; - - /// The name of the DegreesOfFreedom for this Joint - std::array mDofNames; - - /// Default constructor - UniqueProperties( - const Vector& _positionLowerLimits = Vector::Constant(-DART_DBL_INF), - const Vector& _positionUpperLimits = Vector::Constant( DART_DBL_INF), - const Vector& _velocityLowerLimits = Vector::Constant(-DART_DBL_INF), - const Vector& _velocityUpperLimits = Vector::Constant( DART_DBL_INF), - const Vector& _accelerationLowerLimits = Vector::Constant(-DART_DBL_INF), - const Vector& _accelerationUpperLimits = Vector::Constant( DART_DBL_INF), - const Vector& _forceLowerLimits = Vector::Constant(-DART_DBL_INF), - const Vector& _forceUpperLimits = Vector::Constant( DART_DBL_INF), - const Vector& _springStiffness = Vector::Constant(0.0), - const Vector& _restPosition = Vector::Constant(0.0), - const Vector& _dampingCoefficient = Vector::Constant(0.0), - const Vector& _coulombFrictions = Vector::Constant(0.0)); - // TODO(MXG): In version 6.0, we should add mInitialPositions and - // mInitialVelocities to the constructor arguments. For now we must wait in - // order to avoid breaking the API. - - /// Copy constructor - // Note: we only need this because VS2013 lacks full support for std::array - // Once std::array is properly supported, this should be removed. - UniqueProperties(const UniqueProperties& _other); - - virtual ~UniqueProperties() = default; - - public: - // To get byte-aligned Eigen vectors - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; - - struct Properties : Joint::Properties, UniqueProperties - { - Properties( - const Joint::Properties& _jointProperties = Joint::Properties(), - const UniqueProperties& _multiDofProperties = UniqueProperties()); - - virtual ~Properties(); - - public: - // To get byte-aligned Eigen vectors - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; + MultiDofJoint(const MultiDofJoint&) = delete; /// Destructor virtual ~MultiDofJoint(); @@ -678,9 +595,6 @@ class MultiDofJoint : public Joint protected: - /// Properties of this MultiDofJoint - typename MultiDofJoint::UniqueProperties mMultiDofP; - /// Array of DegreeOfFreedom objects std::array mDofs; @@ -873,9 +787,9 @@ class MultiDofJoint : public Joint /// \} }; -#include "dart/dynamics/detail/MultiDofJoint.h" - } // namespace dynamics } // namespace dart +#include "dart/dynamics/detail/MultiDofJoint.h" + #endif // DART_DYNAMICS_MULTIDOFJOINT_H_ diff --git a/dart/dynamics/Node.cpp b/dart/dynamics/Node.cpp index d6bbc2d3c74fe..7729763eed51f 100644 --- a/dart/dynamics/Node.cpp +++ b/dart/dynamics/Node.cpp @@ -59,6 +59,49 @@ NodeDestructor::~NodeDestructor() delete mNode; } +//============================================================================== +Node* NodeDestructor::getNode() const +{ + return mNode; +} + +//============================================================================== +void Node::setNodeState(const State& /*otherState*/) +{ + // Do nothing +} + +//============================================================================== +std::unique_ptr Node::getNodeState() const +{ + return nullptr; +} + +//============================================================================== +void Node::copyNodeStateTo(std::unique_ptr& outputState) const +{ + outputState = getNodeState(); +} + +//============================================================================== +void Node::setNodeProperties(const Properties& /*properties*/) +{ + // Do nothing +} + +//============================================================================== +std::unique_ptr Node::getNodeProperties() const +{ + return nullptr; +} + +//============================================================================== +void Node::copyNodePropertiesTo( + std::unique_ptr& outputProperties) const +{ + outputProperties = getNodeProperties(); +} + //============================================================================== BodyNodePtr Node::getBodyNodePtr() { @@ -80,7 +123,7 @@ bool Node::isRemoved() const return true; } - return (mBodyNode->mNodeMap.find(this) == mBodyNode->mNodeMap.end()); + return !mAmAttached; } //============================================================================== @@ -97,31 +140,35 @@ std::shared_ptr Node::getOrCreateDestructor() } //============================================================================== -Node::Node(ConstructNode_t, BodyNode* _bn) - : mBodyNode(_bn) +Node::Node(BodyNode* _bn) + : mBodyNode(_bn), + mAmAttached(false), + mIndexInBodyNode(INVALID_INDEX), + mIndexInSkeleton(INVALID_INDEX), + mIndexInTree(INVALID_INDEX) { if(nullptr == mBodyNode) { REPORT_INVALID_NODE(Node); return; } - - attach(); } //============================================================================== -Node::Node(ConstructBodyNode_t) - : mBodyNode(nullptr) +std::string Node::registerNameChange(const std::string& newName) { - // BodyNodes do not get "attached" to themselves, so we do nothing here -} + const SkeletonPtr& skel = mBodyNode->getSkeleton(); + if(nullptr == skel) + return newName; -//============================================================================== -Node::Node(ConstructAbstract_t) -{ - dterr << "[Node::Node] Your class implementation is calling the Node " - << "constructor that is meant to be reserved for abstract classes!\n"; - assert(false); + Skeleton::NodeNameMgrMap& nodeNameMgrMap = skel->mNodeNameMgrMap; + Skeleton::NodeNameMgrMap::iterator it = nodeNameMgrMap.find(typeid(*this)); + + if(nodeNameMgrMap.end() == it) + return newName; + + common::NameManager& mgr = it->second; + return mgr.changeObjectName(this, newName); } //============================================================================== @@ -133,8 +180,48 @@ void Node::attach() return; } - if(mBodyNode->mNodeMap.end() == mBodyNode->mNodeMap.find(this)) - mBodyNode->mNodeMap[this] = getOrCreateDestructor(); + // If we are in release mode, and the Node believes it is attached, then we + // can shortcut this procedure +#ifdef NDEBUG + if(mAmAttached) + return; +#endif + + BodyNode::NodeMap::iterator it = mBodyNode->mNodeMap.find(typeid(*this)); + + if(mBodyNode->mNodeMap.end() == it) + { + mBodyNode->mNodeMap[typeid(*this)] = std::vector(); + it = mBodyNode->mNodeMap.find(typeid(*this)); + } + + std::vector& nodes = it->second; + BodyNode::NodeDestructorSet& destructors = mBodyNode->mNodeDestructors; + + NodeDestructorPtr destructor = getOrCreateDestructor(); + if(INVALID_INDEX == mIndexInBodyNode) + { + // If the Node was not in the map, then its destructor should not be in the set + assert(destructors.find(destructor) == destructors.end()); + + // If this Node believes its index is invalid, then it should not exist + // anywhere in the vector + assert(std::find(nodes.begin(), nodes.end(), this) == nodes.end()); + + nodes.push_back(this); + mIndexInBodyNode = nodes.size()-1; + + destructors.insert(destructor); + } + + assert(std::find(nodes.begin(), nodes.end(), this) != nodes.end()); + assert(destructors.find(destructor) != destructors.end()); + + const SkeletonPtr& skel = mBodyNode->getSkeleton(); + if(skel) + skel->registerNode(this); + + mAmAttached = true; } //============================================================================== @@ -146,31 +233,51 @@ void Node::stageForRemoval() return; } - const auto it = mBodyNode->mNodeMap.find(this); + // If we are in release mode, and the Node believes it is detached, then we + // can shortcut this procedure. +#ifdef NDEBUG + if(!mAmAttached) + return; +#endif + + BodyNode::NodeMap::iterator it = mBodyNode->mNodeMap.find(typeid(*this)); + NodeDestructorPtr destructor = getOrCreateDestructor(); + + BodyNode::NodeDestructorSet& destructors = mBodyNode->mNodeDestructors; if(mBodyNode->mNodeMap.end() == it) + { + // If the Node was not in the map, then its index should be invalid + assert(INVALID_INDEX == mIndexInBodyNode); + + // If the Node was not in the map, then its destructor should not be in the set + assert(destructors.find(destructor) == destructors.end()); return; + } - mBodyNode->mNodeMap.erase(it); -} + BodyNode::NodeDestructorSet::iterator destructor_iter = destructors.find(destructor); + // This Node's destructor should be in the set of destructors + assert(destructors.end() != destructor_iter); -//============================================================================== -void AccessoryNode::remove() -{ - stageForRemoval(); -} + std::vector& nodes = it->second; -//============================================================================== -void AccessoryNode::reattach() -{ - attach(); -} + // This Node's index in the vector should be referring to this Node + assert(nodes[mIndexInBodyNode] == this); + nodes.erase(nodes.begin() + mIndexInBodyNode); + destructors.erase(destructor_iter); -//============================================================================== -AccessoryNode::AccessoryNode() - : Node(ConstructAbstract) -{ - // Do nothing + // Reset all the Node indices that have been altered + for(size_t i=mIndexInBodyNode; i < nodes.size(); ++i) + nodes[i]->mIndexInBodyNode = i; + + assert(std::find(nodes.begin(), nodes.end(), this) == nodes.end()); + + const SkeletonPtr& skel = mBodyNode->getSkeleton(); + if(skel) + skel->unregisterNode(this); + + mIndexInBodyNode = INVALID_INDEX; + mAmAttached = false; } } // namespace dynamics diff --git a/dart/dynamics/Node.h b/dart/dynamics/Node.h index f4422a5ab7168..5445db45086fc 100644 --- a/dart/dynamics/Node.h +++ b/dart/dynamics/Node.h @@ -40,6 +40,7 @@ #include #include "dart/common/Subject.h" +#include "dart/common/Extensible.h" #include "dart/dynamics/SmartPointer.h" namespace dart { @@ -61,6 +62,8 @@ class NodeDestructor final /// Non-virtual destructor (this class cannot be inherited) ~NodeDestructor(); + Node* getNode() const; + private: /// Node that this Destructor is responsible for @@ -71,17 +74,92 @@ class NodeDestructor final /// The Node class is a base class for BodyNode and any object that attaches to /// a BodyNode. This base class handles ownership and reference counting for the /// classes that inherit it. +/// +/// In most cases, when creating your own custom Node class, you will also want +/// to inherit from AccessoryNode using CRTP. class Node : public virtual common::Subject { public: friend class BodyNode; + friend class Skeleton; + template friend class AccessoryNode; template friend class TemplateNodePtr; template friend class TemplateWeakNodePtr; + /// If your Node has a State class, then that State class should inherit this + /// Node::State class. This allows us to safely serialize, store, and clone + /// the states of arbitrary Node extensions. If your Node is stateless, then + /// you do not have to worry about extending this class, because + /// Node::getNodeState() will simply return a nullptr by default, which is + /// taken to indicate that it is stateless. + /// + /// The distinction between the State class and the Properties class is that + /// State will get stored in BodyNode::ExtendedState whereas Properties will + /// get stored in BodyNode::ExtendedProperties. Typically Properties are + /// values that only change rarely if ever, whereas State contains values that + /// might change as often as every time step. + class State : public common::Extensible { }; + + /// Use the StateMixer class to easily create a State extension from an + /// existing class or struct + template + using StateMixer = common::ExtensibleMixer; + + /// If your Node has a Properties class, then that Properties class should + /// inherit this Node::Properties class. This allows us to safely serialize, + /// store, and clone the properties of arbitrary Node extensions. If your + /// Node has no properties, then you do not have to worry about extending + /// this class, because Node::getNodeProperties() will simply return a nullptr + /// by default, which is taken to indicate that it has no properties. + /// + /// The distinction between the State class and the Properties class is that + /// State will get stored in BodyNode::ExtendedState whereas Properties will + /// get stored in BodyNode::ExtendedProperties. Typically Properties are + /// values that only change rarely if ever, whereas State contains values that + /// might change as often as every time step. + class Properties : public common::Extensible { }; + + /// Use the PropertiesMixer class to easily create a Properties extension + /// from an existing class or struct. + template + using PropertiesMixer = common::ExtensibleMixer; + /// Virtual destructor virtual ~Node() = default; + /// Set the name of this Node + virtual const std::string& setName(const std::string& newName) = 0; + + /// Get the name of this Node + virtual const std::string& getName() const = 0; + + /// Set the State of this Node. By default, this does nothing. + virtual void setNodeState(const State& otherState); + + /// Get the State of this Node. By default, this returns a nullptr which + /// implies that the Node is stateless. + virtual std::unique_ptr getNodeState() const; + + /// Copy the State of this Node into a unique_ptr. By default, this simply + /// calls getNodeState() and passes the result into the outputState, but this + /// function can be overriden to be more performant by avoiding allocations. + virtual void copyNodeStateTo(std::unique_ptr& outputState) const; + + /// Set the Properties of this Node. By default, this does nothing. + virtual void setNodeProperties(const Properties& properties); + + /// Get the Properties of this Node. By default, this returns a nullptr which + /// implies that the Node has no properties. + virtual std::unique_ptr getNodeProperties() const; + + /// Copy the Properties of this Node into a unique_ptr. By default, this + /// simply calls getNodeProperties() and passes the result into the + /// outputProperties, but this function can be overriden to be more + /// performant. + virtual void copyNodePropertiesTo( + std::unique_ptr& outputProperties) const; + /// Get a pointer to the BodyNode that this Node is associated with BodyNodePtr getBodyNodePtr(); @@ -99,24 +177,17 @@ class Node : public virtual common::Subject protected: - /// Construct a typical Node that will be attached to a BodyNode - enum ConstructNode_t { ConstructNode }; - - /// Construct the Node base of a BodyNode - enum ConstructBodyNode_t { ConstructBodyNode }; - - /// Used when constructing a pure abstract class, because calling the Node - /// constructor is just a formality - enum ConstructAbstract_t { ConstructAbstract }; + /// Allow your Node implementation to be cloned into a new BodyNode + virtual Node* cloneNode(BodyNode* bn) const = 0; - /// Used when constructing a Node type that does NOT inherit from BodyNode - Node(ConstructNode_t, BodyNode* _bn); - - /// Used when constructing a BodyNode - Node(ConstructBodyNode_t); + /// Constructor + Node(BodyNode* _bn); - /// Used when constructing a pure abstract type - Node(ConstructAbstract_t); + /// Inform the Skeleton that the name of this Node has changed. If the name is + /// already taken by another Node of the same type, then this function will + /// return a modified version which is unique. If the name is not already + /// taken, then it will just return the same name that the function was given. + std::string registerNameChange(const std::string& newName); /// Attach the Node to its BodyNode void attach(); @@ -133,15 +204,43 @@ class Node : public virtual common::Subject /// Pointer to the BodyNode that this Node is attached to BodyNode* mBodyNode; + + /// bool that tracks whether this Node is attached to its BodyNode + bool mAmAttached; + + /// The index of this Node within its vector in its BodyNode's NodeMap + size_t mIndexInBodyNode; + + /// The index of this Node within its vector in its Skeleton's NodeMap + size_t mIndexInSkeleton; + + /// Index of this Node within its tree + size_t mIndexInTree; }; -class AccessoryNode : public virtual Node +/// AccessoryNode provides an interface for Nodes to get their index within the +/// list of Nodes, as well as detach and reattach. This uses CRTP to get around +/// the diamond of death problem. +template +class AccessoryNode { public: /// Virtual destructor virtual ~AccessoryNode() = default; + /// Get the index of this Node within its BodyNode. + size_t getIndexInBodyNode() const; + + /// Get the index of this Node within its Skeleton. + size_t getIndexInSkeleton() const; + + /// Get the index of this Node within its tree. + size_t getIndexInTree() const; + + /// Get the index of this Node's tree within its Skeleton + size_t getTreeIndex() const; + /// Stage the Node for removal. When all strong references to the Node expire, /// the Node will be removed from its BodyNode and deleted. void remove(); @@ -153,11 +252,13 @@ class AccessoryNode : public virtual Node protected: /// Prevent a non-inheriting class from constructing one - AccessoryNode(); + AccessoryNode() = default; }; } // namespace dynamics } // namespace dart +#include "dart/dynamics/detail/Node.h" + #endif // DART_DYNAMICS_NODE_H_ diff --git a/dart/dynamics/NodeManagerJoiner.h b/dart/dynamics/NodeManagerJoiner.h new file mode 100644 index 0000000000000..2d079b88dd0dc --- /dev/null +++ b/dart/dynamics/NodeManagerJoiner.h @@ -0,0 +1,190 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_DYNAMICS_NODEMANAGERJOINER_H_ +#define DART_DYNAMICS_NODEMANAGERJOINER_H_ + +#include + +#include "dart/common/Empty.h" + +namespace dart { +namespace dynamics { + +//============================================================================== +/// Declaration of the variadic template +template +class NodeManagerJoinerForBodyNode { }; + +//============================================================================== +/// Special case of only having 1 class: we do nothing but inherit it. +template +class NodeManagerJoinerForBodyNode : public Base1 { }; + +//============================================================================== +/// NodeManagerJoiner allows classes that inherit from various +/// SpecializedNodeManager types to be inherited by a single derived class. This +/// class solved the diamond-of-death problem for multiple +/// SpecializedNodeManager inheritance. +template +class NodeManagerJoinerForBodyNode : public Base1, public Base2 +{ +public: + + /// Default constructor + NodeManagerJoinerForBodyNode() = default; + + /// This constructor allows one argument to be passed to the Base1 constructor + /// and arbitrarily many arguments to be passed to the Base2 constructor. + template + NodeManagerJoinerForBodyNode(Base1Arg&& arg1, Base2Args&&... args2); + + /// This constructor passes one argument to the Base1 constructor and no + /// arguments to the Base2 constructor. + template + NodeManagerJoinerForBodyNode(Base1Arg&& arg1, common::NoArg_t); + + /// This constructor passes no arguments to the Base1 constructor and + /// arbitrarily many arguments to the Base2 constructor. + template + NodeManagerJoinerForBodyNode(common::NoArg_t, Base2Args&&... args2); + + template + size_t getNumNodes() const; + + template + NodeType* getNode(size_t index); + + template + const NodeType* getNode(size_t index) const; + + template + static constexpr bool isSpecializedForNode(); + +}; + +//============================================================================== +/// This is the variadic version of the NodeManagerJoinerForBodyNode class which +/// allows you to include arbitrarily many base classes in the joining. +template +class NodeManagerJoinerForBodyNode : + public NodeManagerJoinerForBodyNode< Base1, NodeManagerJoinerForBodyNode > +{ +public: + + NodeManagerJoinerForBodyNode() = default; + + template + NodeManagerJoinerForBodyNode(Args&&... args); + +}; + +//============================================================================== +/// Declaration of variadic template +template +class NodeManagerJoinerForSkeleton { }; + +//============================================================================== +/// Special case of only having 1 class: we do nothing but inherit it +template +class NodeManagerJoinerForSkeleton : public Base1 { }; + +//============================================================================== +template +class NodeManagerJoinerForSkeleton : + NodeManagerJoinerForBodyNode +{ +public: + + using Base = NodeManagerJoinerForBodyNode; + using Base::getNumNodes; + using Base::getNode; + using Base::isSpecializedForNode; + + /// Default constructor + NodeManagerJoinerForSkeleton() = default; + + /// Forwards construction to NodeManagerJoinerForBodyNode + template + NodeManagerJoinerForSkeleton(Args&&... args); + + /// Get the number of Nodes of the specified type that are in the treeIndexth + /// tree of this Skeleton + template + size_t getNumNodes(size_t treeIndex) const; + + /// Get the nodeIndexth Node of the specified type within the tree of + /// treeIndex. + template + NodeType* getNode(size_t treeIndex, size_t nodeIndex); + + /// Get the nodeIndexth Node of the specified type within the tree of + /// treeIndex. + template + const NodeType* getNode(size_t treeIndex, size_t nodeIndex) const; + + /// Get the Node of the specified type with the given name. + template + NodeType* getNode(const std::string& name); + + /// Get the Node of the specified type with the given name. + template + const NodeType* getNode(const std::string& name) const; + +}; + +//============================================================================== +/// This is the variadic version of the NodeManagerJoinerForSkeleton class which +/// allows you to include arbitrarily many base classes in the joining. +template +class NodeManagerJoinerForSkeleton : + public NodeManagerJoinerForSkeleton< Base1, NodeManagerJoinerForSkeleton > +{ +public: + + NodeManagerJoinerForSkeleton() = default; + + template + NodeManagerJoinerForSkeleton(Args&&... args); + +}; + +} // namespace dynamics +} // namespace dart + +#include "dart/dynamics/detail/NodeManagerJoiner.h" + +#endif // DART_DYNAMICS_NODEMANAGERJOINER_H_ diff --git a/dart/dynamics/PlanarJoint.cpp b/dart/dynamics/PlanarJoint.cpp index 6192f0d2d31be..f01e5e496a8b7 100644 --- a/dart/dynamics/PlanarJoint.cpp +++ b/dart/dynamics/PlanarJoint.cpp @@ -46,94 +46,6 @@ namespace dart { namespace dynamics { -//============================================================================== -PlanarJoint::UniqueProperties::UniqueProperties(PlaneType _planeType) -{ - switch(_planeType) - { - case PT_ARBITRARY: - case PT_XY: - setXYPlane(); - mPlaneType = _planeType; // In case the PlaneType was supposed to be arbitrary - break; - case PT_YZ: - setYZPlane(); - break; - case PT_ZX: - setZXPlane(); - break; - } -} - -//============================================================================== -PlanarJoint::UniqueProperties::UniqueProperties( - const Eigen::Vector3d& _transAxis1, - const Eigen::Vector3d& _transAxis2) -{ - setArbitraryPlane(_transAxis1, _transAxis2); -} - -//============================================================================== -void PlanarJoint::UniqueProperties::setXYPlane() -{ - mPlaneType = PT_XY; - mRotAxis = Eigen::Vector3d::UnitZ(); - mTransAxis1 = Eigen::Vector3d::UnitX(); - mTransAxis2 = Eigen::Vector3d::UnitY(); -} - -//============================================================================== -void PlanarJoint::UniqueProperties::setYZPlane() -{ - mPlaneType = PT_YZ; - mRotAxis = Eigen::Vector3d::UnitX(); - mTransAxis1 = Eigen::Vector3d::UnitY(); - mTransAxis2 = Eigen::Vector3d::UnitZ(); -} - -//============================================================================== -void PlanarJoint::UniqueProperties::setZXPlane() -{ - mPlaneType = PT_ZX; - mRotAxis = Eigen::Vector3d::UnitY(); - mTransAxis1 = Eigen::Vector3d::UnitZ(); - mTransAxis2 = Eigen::Vector3d::UnitX(); -} - -//============================================================================== -void PlanarJoint::UniqueProperties::setArbitraryPlane( - const Eigen::Vector3d& _transAxis1, - const Eigen::Vector3d& _transAxis2) -{ - // Set plane type as arbitrary plane - mPlaneType = PT_ARBITRARY; - - // First translational axis - mTransAxis1 = _transAxis1.normalized(); - - // Second translational axis - mTransAxis2 = _transAxis2.normalized(); - - // Orthogonalize translational axese - double dotProduct = mTransAxis1.dot(mTransAxis2); - assert(std::abs(dotProduct) < 1.0 - 1e-6); - if (std::abs(dotProduct) > 1e-6) - mTransAxis2 = (mTransAxis2 - dotProduct * mTransAxis1).normalized(); - - // Rotational axis - mRotAxis = (mTransAxis1.cross(mTransAxis2)).normalized(); -} - -//============================================================================== -PlanarJoint::Properties::Properties( - const MultiDofJoint<3>::Properties& _multiDofProperties, - const PlanarJoint::UniqueProperties& _planarProperties) - : MultiDofJoint<3>::Properties(_multiDofProperties), - PlanarJoint::UniqueProperties(_planarProperties) -{ - // Do nothing -} - //============================================================================== PlanarJoint::~PlanarJoint() { @@ -151,27 +63,14 @@ void PlanarJoint::setProperties(const Properties& _properties) //============================================================================== void PlanarJoint::setProperties(const UniqueProperties& _properties) { - switch(_properties.mPlaneType) - { - case PT_XY: - setXYPlane(); - break; - case PT_YZ: - setYZPlane(); - break; - case PT_ZX: - setZXPlane(); - break; - case PT_ARBITRARY: - setArbitraryPlane(_properties.mTransAxis1, _properties.mTransAxis2); - break; - } + getPlanarJointAddon()->setProperties(_properties); } //============================================================================== PlanarJoint::Properties PlanarJoint::getPlanarJointProperties() const { - return Properties(getMultiDofJointProperties(), mPlanarP); + return Properties(getMultiDofJointProperties(), + getPlanarJointAddon()->getProperties()); } //============================================================================== @@ -221,7 +120,7 @@ bool PlanarJoint::isCyclic(size_t _index) const //============================================================================== void PlanarJoint::setXYPlane(bool _renameDofs) { - mPlanarP.setXYPlane(); + getPlanarJointAddon()->setXYPlane(); if (_renameDofs) updateDegreeOfFreedomNames(); @@ -231,7 +130,7 @@ void PlanarJoint::setXYPlane(bool _renameDofs) //============================================================================== void PlanarJoint::setYZPlane(bool _renameDofs) { - mPlanarP.setYZPlane(); + getPlanarJointAddon()->setYZPlane(); if (_renameDofs) updateDegreeOfFreedomNames(); @@ -241,7 +140,7 @@ void PlanarJoint::setYZPlane(bool _renameDofs) //============================================================================== void PlanarJoint::setZXPlane(bool _renameDofs) { - mPlanarP.setZXPlane(); + getPlanarJointAddon()->setZXPlane(); if (_renameDofs) updateDegreeOfFreedomNames(); @@ -253,7 +152,7 @@ void PlanarJoint::setArbitraryPlane(const Eigen::Vector3d& _transAxis1, const Eigen::Vector3d& _transAxis2, bool _renameDofs) { - mPlanarP.setArbitraryPlane(_transAxis1, _transAxis2); + getPlanarJointAddon()->setArbitraryPlane(_transAxis1, _transAxis2); if (_renameDofs) updateDegreeOfFreedomNames(); @@ -263,25 +162,25 @@ void PlanarJoint::setArbitraryPlane(const Eigen::Vector3d& _transAxis1, //============================================================================== PlanarJoint::PlaneType PlanarJoint::getPlaneType() const { - return mPlanarP.mPlaneType; + return getPlanarJointAddon()->getPlaneType(); } //============================================================================== const Eigen::Vector3d& PlanarJoint::getRotationalAxis() const { - return mPlanarP.mRotAxis; + return getPlanarJointAddon()->getRotAxis(); } //============================================================================== const Eigen::Vector3d& PlanarJoint::getTranslationalAxis1() const { - return mPlanarP.mTransAxis1; + return getPlanarJointAddon()->getTransAxis1(); } //============================================================================== const Eigen::Vector3d& PlanarJoint::getTranslationalAxis2() const { - return mPlanarP.mTransAxis2; + return getPlanarJointAddon()->getTransAxis2(); } //============================================================================== @@ -289,13 +188,14 @@ Eigen::Matrix PlanarJoint::getLocalJacobianStatic( const Eigen::Vector3d& _positions) const { Eigen::Matrix J = Eigen::Matrix::Zero(); - J.block<3, 1>(3, 0) = mPlanarP.mTransAxis1; - J.block<3, 1>(3, 1) = mPlanarP.mTransAxis2; - J.block<3, 1>(0, 2) = mPlanarP.mRotAxis; + J.block<3, 1>(3, 0) = getPlanarJointAddon()->getTransAxis1(); + J.block<3, 1>(3, 1) = getPlanarJointAddon()->getTransAxis2(); + J.block<3, 1>(0, 2) = getPlanarJointAddon()->getRotAxis(); J.leftCols<2>() = math::AdTJacFixed(mJointP.mT_ChildBodyToJoint - * math::expAngular(mPlanarP.mRotAxis * -_positions[2]), + * math::expAngular(getPlanarJointAddon()->getRotAxis() + * -_positions[2]), J.leftCols<2>()); J.col(2) = math::AdTJac(mJointP.mT_ChildBodyToJoint, J.col(2)); @@ -307,10 +207,13 @@ Eigen::Matrix PlanarJoint::getLocalJacobianStatic( //============================================================================== PlanarJoint::PlanarJoint(const Properties& _properties) - : MultiDofJoint<3>(_properties) + : detail::PlanarJointBase(_properties, common::NoArg) { - setProperties(_properties); - updateDegreeOfFreedomNames(); + createPlanarJointAddon(_properties); + + // Inherited Joint Properties must be set in the final joint class or else we + // get pure virtual function calls + MultiDofJoint<3>::setProperties(_properties); } //============================================================================== @@ -323,27 +226,28 @@ Joint* PlanarJoint::clone() const void PlanarJoint::updateDegreeOfFreedomNames() { std::vector affixes; - switch (mPlanarP.mPlaneType) + switch (getPlanarJointAddon()->getPlaneType()) { - case PT_XY: + case PlaneType::XY: affixes.push_back("_x"); affixes.push_back("_y"); break; - case PT_YZ: + case PlaneType::YZ: affixes.push_back("_y"); affixes.push_back("_z"); break; - case PT_ZX: + case PlaneType::ZX: affixes.push_back("_z"); affixes.push_back("_x"); break; - case PT_ARBITRARY: + case PlaneType::ARBITRARY: affixes.push_back("_1"); affixes.push_back("_2"); break; default: dterr << "Unsupported plane type in PlanarJoint named '" << mJointP.mName - << "' (" << mPlanarP.mPlaneType << ")\n"; + << "' (" << static_cast(getPlanarJointAddon()->getPlaneType()) + << ")\n"; } if (affixes.size() == 2) @@ -361,9 +265,9 @@ void PlanarJoint::updateLocalTransform() const { const Eigen::Vector3d& positions = getPositionsStatic(); mT = mJointP.mT_ParentBodyToJoint - * Eigen::Translation3d(mPlanarP.mTransAxis1 * positions[0]) - * Eigen::Translation3d(mPlanarP.mTransAxis2 * positions[1]) - * math::expAngular (mPlanarP.mRotAxis * positions[2]) + * Eigen::Translation3d(getPlanarJointAddon()->getTransAxis1() * positions[0]) + * Eigen::Translation3d(getPlanarJointAddon()->getTransAxis2() * positions[1]) + * math::expAngular (getPlanarJointAddon()->getRotAxis() * positions[2]) * mJointP.mT_ChildBodyToJoint.inverse(); // Verification @@ -380,23 +284,23 @@ void PlanarJoint::updateLocalJacobian(bool) const void PlanarJoint::updateLocalJacobianTimeDeriv() const { Eigen::Matrix J = Eigen::Matrix::Zero(); - J.block<3, 1>(3, 0) = mPlanarP.mTransAxis1; - J.block<3, 1>(3, 1) = mPlanarP.mTransAxis2; - J.block<3, 1>(0, 2) = mPlanarP.mRotAxis; + J.block<3, 1>(3, 0) = getPlanarJointAddon()->getTransAxis1(); + J.block<3, 1>(3, 1) = getPlanarJointAddon()->getTransAxis2(); + J.block<3, 1>(0, 2) = getPlanarJointAddon()->getRotAxis(); const Eigen::Matrix& Jacobian = getLocalJacobianStatic(); const Eigen::Vector3d& velocities = getVelocitiesStatic(); mJacobianDeriv.col(0) = -math::ad(Jacobian.col(2) * velocities[2], math::AdT(mJointP.mT_ChildBodyToJoint - * math::expAngular(mPlanarP.mRotAxis + * math::expAngular(getPlanarJointAddon()->getRotAxis() * -getPositionsStatic()[2]), J.col(0))); mJacobianDeriv.col(1) = -math::ad(Jacobian.col(2) * velocities[2], math::AdT(mJointP.mT_ChildBodyToJoint - * math::expAngular(mPlanarP.mRotAxis + * math::expAngular(getPlanarJointAddon()->getRotAxis() * -getPositionsStatic()[2]), J.col(1))); diff --git a/dart/dynamics/PlanarJoint.h b/dart/dynamics/PlanarJoint.h index db59468437cd0..cf103efc4302b 100644 --- a/dart/dynamics/PlanarJoint.h +++ b/dart/dynamics/PlanarJoint.h @@ -37,9 +37,7 @@ #ifndef DART_DYNAMICS_PLANARRJOINT_H_ #define DART_DYNAMICS_PLANARRJOINT_H_ -#include - -#include "dart/dynamics/MultiDofJoint.h" +#include "dart/dynamics/detail/PlanarJointProperties.h" namespace dart { namespace dynamics { @@ -50,76 +48,18 @@ namespace dynamics { /// First and second coordiantes represent translation along first and second /// translational axese, respectively. Third coordinate represents rotation /// along rotational axis. -class PlanarJoint : public MultiDofJoint<3> +class PlanarJoint : public detail::PlanarJointBase { public: friend class Skeleton; + using PlaneType = detail::PlaneType; + using UniqueProperties = detail::PlanarJointUniqueProperties; + using Properties = detail::PlanarJointProperties; + using Addon = detail::PlanarJointAddon; + using Base = detail::PlanarJointBase; - /// Plane type - enum PlaneType - { - PT_XY, - PT_YZ, - PT_ZX, - PT_ARBITRARY - }; - - /// Properties that are unique to PlanarJoints. Note that the mPlaneType - /// member has greater authority than the mTransAxis1 and mTransAxis2 members. - /// When copying properties into a PlanarJoint, it will first defer to - /// mPlaneType. If mPlaneType is PT_ARBITRARY, then and only then will it use - /// mTransAxis1 and mTransAxis2. mRotAxis has no authority; it will always be - /// recomputed from mTransAxis1 and mTransAxis2 when copying it into a - /// PlanarJoint - struct UniqueProperties - { - /// Plane type - PlaneType mPlaneType; - - /// First translational axis - Eigen::Vector3d mTransAxis1; - - /// Second translational axis - Eigen::Vector3d mTransAxis2; - - /// Rotational axis - Eigen::Vector3d mRotAxis; - - /// Constructor for pre-defined plane types. Defaults to the XY plane if - /// PT_ARBITRARY is specified. - UniqueProperties(PlaneType _planeType = PT_XY); - - /// Constructor for arbitrary plane types. mPlaneType will be set to - /// PT_ARBITRARY - UniqueProperties(const Eigen::Vector3d& _transAxis1, - const Eigen::Vector3d& _transAxis2); - - virtual ~UniqueProperties() = default; - - /// Set plane type as XY-plane - void setXYPlane(); - - /// Set plane type as YZ-plane - void setYZPlane(); - - /// Set plane type as ZX-plane - void setZXPlane(); - - /// Set plane type as arbitrary plane with two orthogonal translational axes - void setArbitraryPlane(const Eigen::Vector3d& _transAxis1, - const Eigen::Vector3d& _transAxis2); - }; - - struct Properties : MultiDofJoint<3>::Properties, PlanarJoint::UniqueProperties - { - Properties(const MultiDofJoint<3>::Properties& _multiDofProperties = - MultiDofJoint<3>::Properties(), - const PlanarJoint::UniqueProperties& _planarProperties = - PlanarJoint::UniqueProperties()); - - virtual ~Properties() = default; - }; + DART_BAKE_SPECIALIZED_ADDON_IRREGULAR(Addon, PlanarJointAddon) PlanarJoint(const PlanarJoint&) = delete; @@ -192,6 +132,8 @@ class PlanarJoint : public MultiDofJoint<3> Eigen::Matrix getLocalJacobianStatic( const Eigen::Vector3d& _positions) const override; + template friend void detail::JointPropertyUpdate(AddonType*); + protected: /// Constructor called by Skeleton class @@ -215,11 +157,6 @@ class PlanarJoint : public MultiDofJoint<3> // Documentation inherited virtual void updateLocalJacobianTimeDeriv() const override; -protected: - - /// PlanarJoint Properties - UniqueProperties mPlanarP; - public: // To get byte-aligned Eigen vectors EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/dart/dynamics/PrismaticJoint.cpp b/dart/dynamics/PrismaticJoint.cpp index 1977627690211..5597547f0ee51 100644 --- a/dart/dynamics/PrismaticJoint.cpp +++ b/dart/dynamics/PrismaticJoint.cpp @@ -45,23 +45,6 @@ namespace dart { namespace dynamics { -//============================================================================== -PrismaticJoint::UniqueProperties::UniqueProperties(const Eigen::Vector3d& _axis) - : mAxis(_axis.normalized()) -{ - // Do nothing -} - -//============================================================================== -PrismaticJoint::Properties::Properties( - const SingleDofJoint::Properties& _singleDofProperties, - const PrismaticJoint::UniqueProperties& _prismaticProperties) - : SingleDofJoint::Properties(_singleDofProperties), - PrismaticJoint::UniqueProperties(_prismaticProperties) -{ - // Do nothing -} - //============================================================================== PrismaticJoint::~PrismaticJoint() { @@ -85,7 +68,8 @@ void PrismaticJoint::setProperties(const UniqueProperties& _properties) //============================================================================== PrismaticJoint::Properties PrismaticJoint::getPrismaticJointProperties() const { - return Properties(getSingleDofJointProperties(), mPrismaticP); + return Properties(getSingleDofJointProperties(), + getPrismaticJointAddon()->getProperties()); } //============================================================================== @@ -135,23 +119,24 @@ bool PrismaticJoint::isCyclic(size_t _index) const //============================================================================== void PrismaticJoint::setAxis(const Eigen::Vector3d& _axis) { - mPrismaticP.mAxis = _axis.normalized(); - updateLocalJacobian(); - notifyPositionUpdate(); + getPrismaticJointAddon()->setAxis(_axis); } //============================================================================== const Eigen::Vector3d& PrismaticJoint::getAxis() const { - return mPrismaticP.mAxis; + return getPrismaticJointAddon()->getAxis(); } //============================================================================== PrismaticJoint::PrismaticJoint(const Properties& _properties) - : SingleDofJoint(_properties) + : detail::PrismaticJointBase(_properties, common::NoArg) { - setProperties(_properties); - updateDegreeOfFreedomNames(); + createPrismaticJointAddon(_properties); + + // Inherited Joint Properties must be set in the final joint class or else we + // get pure virtual function calls + SingleDofJoint::setProperties(_properties); } //============================================================================== @@ -164,7 +149,7 @@ Joint* PrismaticJoint::clone() const void PrismaticJoint::updateLocalTransform() const { mT = mJointP.mT_ParentBodyToJoint - * Eigen::Translation3d(mPrismaticP.mAxis * getPositionStatic()) + * Eigen::Translation3d(getAxis() * getPositionStatic()) * mJointP.mT_ChildBodyToJoint.inverse(); // Verification @@ -176,7 +161,7 @@ void PrismaticJoint::updateLocalJacobian(bool _mandatory) const { if(_mandatory) { - mJacobian = math::AdTLinear(mJointP.mT_ChildBodyToJoint, mPrismaticP.mAxis); + mJacobian = math::AdTLinear(mJointP.mT_ChildBodyToJoint, getAxis()); // Verification assert(!math::isNan(mJacobian)); diff --git a/dart/dynamics/PrismaticJoint.h b/dart/dynamics/PrismaticJoint.h index 2ad7a9026fe9d..bfc82c3e90b10 100644 --- a/dart/dynamics/PrismaticJoint.h +++ b/dart/dynamics/PrismaticJoint.h @@ -37,40 +37,23 @@ #ifndef DART_DYNAMICS_PRISMATICJOINT_H_ #define DART_DYNAMICS_PRISMATICJOINT_H_ -#include - -#include - -#include "dart/dynamics/SingleDofJoint.h" +#include "dart/dynamics/detail/PrismaticJointProperties.h" namespace dart { namespace dynamics { /// class RevoluteJoint -class PrismaticJoint : public SingleDofJoint +class PrismaticJoint : public detail::PrismaticJointBase { public: friend class Skeleton; + using UniqueProperties = detail::PrismaticJointUniqueProperties; + using Properties = detail::PrismaticJointProperties; + using Addon = detail::PrismaticJointAddon; + using Base = detail::PrismaticJointBase; - struct UniqueProperties - { - Eigen::Vector3d mAxis; - - UniqueProperties(const Eigen::Vector3d& _axis = Eigen::Vector3d::UnitZ()); - virtual ~UniqueProperties() = default; - }; - - struct Properties : SingleDofJoint::Properties, - PrismaticJoint::UniqueProperties - { - Properties( - const SingleDofJoint::Properties& _singleDofProperties = - SingleDofJoint::Properties(), - const PrismaticJoint::UniqueProperties& _prismaticProperties = - PrismaticJoint::UniqueProperties()); - virtual ~Properties() = default; - }; + DART_BAKE_SPECIALIZED_ADDON_IRREGULAR(Addon, PrismaticJointAddon) PrismaticJoint(const PrismaticJoint&) = delete; @@ -110,6 +93,8 @@ class PrismaticJoint : public SingleDofJoint /// const Eigen::Vector3d& getAxis() const; + template friend void detail::JointPropertyUpdate(AddonType*); + protected: /// Constructor called by Skeleton class @@ -127,11 +112,6 @@ class PrismaticJoint : public SingleDofJoint // Documentation inherited virtual void updateLocalJacobianTimeDeriv() const override; -protected: - - /// PrismaticJoint Properties - UniqueProperties mPrismaticP; - public: // To get byte-aligned Eigen vectors EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/dart/dynamics/ReferentialSkeleton.cpp b/dart/dynamics/ReferentialSkeleton.cpp index 476ba48edc99e..6466941464b89 100644 --- a/dart/dynamics/ReferentialSkeleton.cpp +++ b/dart/dynamics/ReferentialSkeleton.cpp @@ -66,17 +66,6 @@ size_t ReferentialSkeleton::getNumBodyNodes() const return mBodyNodes.size(); } -//============================================================================== -template -static T getVectorObjectIfAvailable(size_t _idx, const std::vector& _vec) -{ - if (_idx < _vec.size()) - return _vec[_idx]; - - assert( _idx < _vec.size() ); - return nullptr; -} - //============================================================================== BodyNode* ReferentialSkeleton::getBodyNode(size_t _idx) { diff --git a/dart/dynamics/RevoluteJoint.cpp b/dart/dynamics/RevoluteJoint.cpp index c288d09cd2c22..067a1288047bf 100644 --- a/dart/dynamics/RevoluteJoint.cpp +++ b/dart/dynamics/RevoluteJoint.cpp @@ -46,23 +46,6 @@ namespace dart { namespace dynamics { -//============================================================================== -RevoluteJoint::UniqueProperties::UniqueProperties(const Eigen::Vector3d& _axis) - : mAxis(_axis.normalized()) -{ - // Do nothing -} - -//============================================================================== -RevoluteJoint::Properties::Properties( - const SingleDofJoint::Properties& _singleDofJointProperties, - const RevoluteJoint::UniqueProperties& _revoluteProperties) - : SingleDofJoint::Properties(_singleDofJointProperties), - RevoluteJoint::UniqueProperties(_revoluteProperties) -{ - // Do nothing -} - //============================================================================== RevoluteJoint::~RevoluteJoint() { @@ -86,7 +69,8 @@ void RevoluteJoint::setProperties(const UniqueProperties& _properties) //============================================================================== RevoluteJoint::Properties RevoluteJoint::getRevoluteJointProperties() const { - return Properties(getSingleDofJointProperties(), mRevoluteP); + return Properties(getSingleDofJointProperties(), + getRevoluteJointAddon()->getProperties()); } //============================================================================== @@ -136,23 +120,25 @@ const std::string& RevoluteJoint::getStaticType() //============================================================================== void RevoluteJoint::setAxis(const Eigen::Vector3d& _axis) { - mRevoluteP.mAxis = _axis.normalized(); - updateLocalJacobian(); - notifyPositionUpdate(); + getRevoluteJointAddon()->setAxis(_axis); } //============================================================================== const Eigen::Vector3d& RevoluteJoint::getAxis() const { - return mRevoluteP.mAxis; + return getRevoluteJointAddon()->getAxis(); } //============================================================================== RevoluteJoint::RevoluteJoint(const Properties& _properties) - : SingleDofJoint(_properties) + : detail::RevoluteJointBase(_properties, common::NoArg) +// : detail::RevoluteJointBase(common::NextArgs, _properties) { - setProperties(_properties); - updateDegreeOfFreedomNames(); + createRevoluteJointAddon(_properties); + + // Inherited Joint Properties must be set in the final joint class or else we + // get pure virtual function calls + SingleDofJoint::setProperties(_properties); } //============================================================================== @@ -165,7 +151,7 @@ Joint* RevoluteJoint::clone() const void RevoluteJoint::updateLocalTransform() const { mT = mJointP.mT_ParentBodyToJoint - * math::expAngular(mRevoluteP.mAxis * getPositionStatic()) + * math::expAngular(getAxis() * getPositionStatic()) * mJointP.mT_ChildBodyToJoint.inverse(); // Verification @@ -177,7 +163,7 @@ void RevoluteJoint::updateLocalJacobian(bool _mandatory) const { if(_mandatory) { - mJacobian = math::AdTAngular(mJointP.mT_ChildBodyToJoint, mRevoluteP.mAxis); + mJacobian = math::AdTAngular(mJointP.mT_ChildBodyToJoint, getAxis()); // Verification assert(!math::isNan(mJacobian)); diff --git a/dart/dynamics/RevoluteJoint.h b/dart/dynamics/RevoluteJoint.h index 367cf50b79f84..127c7174b1c35 100644 --- a/dart/dynamics/RevoluteJoint.h +++ b/dart/dynamics/RevoluteJoint.h @@ -37,42 +37,23 @@ #ifndef DART_DYNAMICS_REVOLUTEJOINT_H_ #define DART_DYNAMICS_REVOLUTEJOINT_H_ -#include - -#include - -#include "dart/dynamics/SingleDofJoint.h" +#include "dart/dynamics/detail/RevoluteJointProperties.h" namespace dart { namespace dynamics { /// class RevoluteJoint -class RevoluteJoint : public SingleDofJoint +class RevoluteJoint : public detail::RevoluteJointBase { public: friend class Skeleton; + using UniqueProperties = detail::RevoluteJointUniqueProperties; + using Properties = detail::RevoluteJointProperties; + using Addon = detail::RevoluteJointAddon; + using Base = detail::RevoluteJointBase; - struct UniqueProperties - { - Eigen::Vector3d mAxis; - - UniqueProperties(const Eigen::Vector3d& _axis = Eigen::Vector3d::UnitZ()); - - virtual ~UniqueProperties() = default; - }; - - struct Properties : SingleDofJoint::Properties, - RevoluteJoint::UniqueProperties - { - Properties( - const SingleDofJoint::Properties& _singleDofJointProperties= - SingleDofJoint::Properties(), - const RevoluteJoint::UniqueProperties& _revoluteProperties = - RevoluteJoint::UniqueProperties()); - - virtual ~Properties() = default; - }; + DART_BAKE_SPECIALIZED_ADDON_IRREGULAR(Addon, RevoluteJointAddon) RevoluteJoint(const RevoluteJoint&) = delete; @@ -129,12 +110,10 @@ class RevoluteJoint : public SingleDofJoint // Documentation inherited virtual void updateLocalJacobianTimeDeriv() const override; -protected: +public: - /// RevoluteJoint Properties - UniqueProperties mRevoluteP; + template friend void detail::JointPropertyUpdate(AddonType*); -public: // To get byte-aligned Eigen vectors EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/dart/dynamics/ScrewJoint.cpp b/dart/dynamics/ScrewJoint.cpp index 4f6771df39f2c..1f9a109a616f3 100644 --- a/dart/dynamics/ScrewJoint.cpp +++ b/dart/dynamics/ScrewJoint.cpp @@ -45,25 +45,6 @@ namespace dart { namespace dynamics { -//============================================================================== -ScrewJoint::UniqueProperties::UniqueProperties( - const Eigen::Vector3d& _axis, double _pitch) - : mAxis(_axis.normalized()), - mPitch(_pitch) -{ - // Do nothing -} - -//============================================================================== -ScrewJoint::Properties::Properties( - const SingleDofJoint::Properties& _singleDofProperties, - const ScrewJoint::UniqueProperties& _screwProperties) - : SingleDofJoint::Properties(_singleDofProperties), - ScrewJoint::UniqueProperties(_screwProperties) -{ - // Do nothing -} - //============================================================================== ScrewJoint::~ScrewJoint() { @@ -88,7 +69,8 @@ void ScrewJoint::setProperties(const UniqueProperties& _properties) //============================================================================== ScrewJoint::Properties ScrewJoint::getScrewJointProperties() const { - return Properties(getSingleDofJointProperties(), mScrewP); + return Properties(getSingleDofJointProperties(), + getScrewJointAddon()->getProperties()); } //============================================================================== @@ -138,34 +120,36 @@ bool ScrewJoint::isCyclic(size_t _index) const //============================================================================== void ScrewJoint::setAxis(const Eigen::Vector3d& _axis) { - mScrewP.mAxis = _axis.normalized(); - notifyPositionUpdate(); + getScrewJointAddon()->setAxis(_axis); } //============================================================================== const Eigen::Vector3d& ScrewJoint::getAxis() const { - return mScrewP.mAxis; + return getScrewJointAddon()->getAxis(); } //============================================================================== void ScrewJoint::setPitch(double _pitch) { - mScrewP.mPitch = _pitch; - updateLocalJacobian(); + getScrewJointAddon()->setPitch(_pitch); } //============================================================================== double ScrewJoint::getPitch() const { - return mScrewP.mPitch; + return getScrewJointAddon()->getPitch(); } //============================================================================== ScrewJoint::ScrewJoint(const Properties& _properties) - : SingleDofJoint(_properties) + : detail::ScrewJointBase(_properties, common::NoArg) { - setProperties(_properties); + createScrewJointAddon(_properties); + + // Inherited Joint Properties must be set in the final joint class or else we + // get pure virtual function calls + SingleDofJoint::setProperties(_properties); } //============================================================================== @@ -178,8 +162,8 @@ Joint* ScrewJoint::clone() const void ScrewJoint::updateLocalTransform() const { Eigen::Vector6d S = Eigen::Vector6d::Zero(); - S.head<3>() = mScrewP.mAxis; - S.tail<3>() = mScrewP.mAxis*mScrewP.mPitch/DART_2PI; + S.head<3>() = getAxis(); + S.tail<3>() = getAxis()*getPitch()/DART_2PI; mT = mJointP.mT_ParentBodyToJoint * math::expMap(S * getPositionStatic()) * mJointP.mT_ChildBodyToJoint.inverse(); @@ -192,8 +176,8 @@ void ScrewJoint::updateLocalJacobian(bool _mandatory) const if(_mandatory) { Eigen::Vector6d S = Eigen::Vector6d::Zero(); - S.head<3>() = mScrewP.mAxis; - S.tail<3>() = mScrewP.mAxis*mScrewP.mPitch/DART_2PI; + S.head<3>() = getAxis(); + S.tail<3>() = getAxis()*getPitch()/DART_2PI; mJacobian = math::AdT(mJointP.mT_ChildBodyToJoint, S); assert(!math::isNan(mJacobian)); } diff --git a/dart/dynamics/ScrewJoint.h b/dart/dynamics/ScrewJoint.h index ba6910f9a244c..2ffcc1b5607c9 100644 --- a/dart/dynamics/ScrewJoint.h +++ b/dart/dynamics/ScrewJoint.h @@ -37,46 +37,23 @@ #ifndef DART_DYNAMICS_SCREWJOINT_H_ #define DART_DYNAMICS_SCREWJOINT_H_ -#include - -#include - -#include "dart/dynamics/SingleDofJoint.h" +#include "dart/dynamics/detail/ScrewJointProperties.h" namespace dart { namespace dynamics { /// class ScrewJoint -class ScrewJoint : public SingleDofJoint +class ScrewJoint : public detail::ScrewJointBase { public: friend class Skeleton; + using UniqueProperties = detail::ScrewJointUniqueProperties; + using Properties = detail::ScrewJointProperties; + using Addon = detail::ScrewJointAddon; + using Base = detail::ScrewJointBase; - struct UniqueProperties - { - /// Rotational axis - Eigen::Vector3d mAxis; - - /// Translational pitch - double mPitch; - - UniqueProperties(const Eigen::Vector3d& _axis = Eigen::Vector3d::UnitZ(), - double _pitch = 0.1); - - virtual ~UniqueProperties() = default; - }; - - struct Properties : SingleDofJoint::Properties, - ScrewJoint::UniqueProperties - { - Properties( - const SingleDofJoint::Properties& _singleDofProperties = - SingleDofJoint::Properties(), - const ScrewJoint::UniqueProperties& _screwProperties = - ScrewJoint::UniqueProperties()); - virtual ~Properties() = default; - }; + DART_BAKE_SPECIALIZED_ADDON_IRREGULAR(Addon, ScrewJointAddon) ScrewJoint(const ScrewJoint&) = delete; @@ -122,6 +99,8 @@ class ScrewJoint : public SingleDofJoint /// double getPitch() const; + template friend void detail::JointPropertyUpdate(AddonType*); + protected: /// Constructor called by Skeleton class @@ -139,11 +118,6 @@ class ScrewJoint : public SingleDofJoint // Documentation inherited virtual void updateLocalJacobianTimeDeriv() const override; -protected: - - /// ScrewJoint Properties - UniqueProperties mScrewP; - public: // To get byte-aligned Eigen vectors EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/dart/dynamics/SingleDofJoint.cpp b/dart/dynamics/SingleDofJoint.cpp index 0a01648cbed94..b7713c6cc3f3d 100644 --- a/dart/dynamics/SingleDofJoint.cpp +++ b/dart/dynamics/SingleDofJoint.cpp @@ -61,52 +61,6 @@ namespace dart { namespace dynamics { -//============================================================================== -SingleDofJoint::UniqueProperties::UniqueProperties( - double _positionLowerLimit, - double _positionUpperLimit, - double _velocityLowerLimit, - double _velocityUpperLimit, - double _accelerationLowerLimit, - double _accelerationUpperLimit, - double _forceLowerLimit, - double _forceUpperLimit, - double _springStiffness, - double _restPosition, - double _dampingCoefficient, - double _coulombFriction, - bool _preserveDofName, - std::string _dofName) - : mPositionLowerLimit(_positionLowerLimit), - mPositionUpperLimit(_positionUpperLimit), - mInitialPosition(0.0), - mVelocityLowerLimit(_velocityLowerLimit), - mVelocityUpperLimit(_velocityUpperLimit), - mInitialVelocity(0.0), - mAccelerationLowerLimit(_accelerationLowerLimit), - mAccelerationUpperLimit(_accelerationUpperLimit), - mForceLowerLimit(_forceLowerLimit), - mForceUpperLimit(_forceUpperLimit), - mSpringStiffness(_springStiffness), - mRestPosition(_restPosition), - mDampingCoefficient(_dampingCoefficient), - mFriction(_coulombFriction), - mPreserveDofName(_preserveDofName), - mDofName(_dofName) -{ - // Do nothing -} - -//============================================================================== -SingleDofJoint::Properties::Properties( - const Joint::Properties& _jointProperties, - const UniqueProperties& _singleDofProperties) - : Joint::Properties(_jointProperties), - UniqueProperties(_singleDofProperties) -{ - // Do nothing -} - //============================================================================== SingleDofJoint::~SingleDofJoint() { @@ -123,27 +77,13 @@ void SingleDofJoint::setProperties(const Properties& _properties) //============================================================================== void SingleDofJoint::setProperties(const UniqueProperties& _properties) { - setDofName(0, _properties.mDofName, _properties.mPreserveDofName); - setPositionLowerLimit(0, _properties.mPositionLowerLimit); - setPositionUpperLimit(0, _properties.mPositionUpperLimit); - setInitialPosition(0, _properties.mInitialPosition); - setVelocityLowerLimit(0, _properties.mVelocityLowerLimit); - setVelocityUpperLimit(0, _properties.mVelocityUpperLimit); - setInitialVelocity(0, _properties.mInitialVelocity); - setAccelerationLowerLimit(0, _properties.mAccelerationLowerLimit); - setAccelerationUpperLimit(0, _properties.mAccelerationUpperLimit); - setForceLowerLimit(0, _properties.mForceLowerLimit); - setForceUpperLimit(0, _properties.mForceUpperLimit); - setSpringStiffness(0, _properties.mSpringStiffness); - setRestPosition(0, _properties.mRestPosition); - setDampingCoefficient(0, _properties.mDampingCoefficient); - setCoulombFriction(0, _properties.mFriction); + getSingleDofJointAddon()->setProperties(_properties); } //============================================================================== SingleDofJoint::Properties SingleDofJoint::getSingleDofJointProperties() const { - return Properties(mJointP, mSingleDofP); + return Properties(mJointP, getSingleDofJointAddon()->getProperties()); } //============================================================================== @@ -235,21 +175,22 @@ const std::string& SingleDofJoint::setDofName(size_t _index, _index = 0; } - preserveDofName(_index, _preserveName); + preserveDofName(0, _preserveName); - if (_name == mSingleDofP.mDofName) - return mSingleDofP.mDofName; + if(_name == getSingleDofJointAddon()->mProperties.mDofName) + return getSingleDofJointAddon()->mProperties.mDofName; - SkeletonPtr skel = mChildBodyNode? mChildBodyNode->getSkeleton() : nullptr; + const SkeletonPtr& skel = getSkeleton(); if(skel) { - mSingleDofP.mDofName = + getSingleDofJointAddon()->mProperties.mDofName = skel->mNameMgrForDofs.changeObjectName(mDof, _name); + skel->incrementVersion(); } else - mSingleDofP.mDofName = _name; + getSingleDofJointAddon()->mProperties.mDofName = _name; - return mSingleDofP.mDofName; + return getSingleDofJointAddon()->mProperties.mDofName; } //============================================================================== @@ -261,7 +202,7 @@ void SingleDofJoint::preserveDofName(size_t _index, bool _preserve) return; } - mSingleDofP.mPreserveDofName = _preserve; + getSingleDofJointAddon()->setPreserveDofName(_preserve); } //============================================================================== @@ -272,7 +213,7 @@ bool SingleDofJoint::isDofNamePreserved(size_t _index) const SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( isDofNamePreserved, _index ); } - return mSingleDofP.mPreserveDofName; + return getSingleDofJointAddon()->getPreserveDofName(); } //============================================================================== @@ -286,7 +227,7 @@ const std::string& SingleDofJoint::getDofName(size_t _index) const assert(false); } - return mSingleDofP.mDofName; + return getSingleDofJointAddon()->getDofName(); } //============================================================================== @@ -302,8 +243,8 @@ void SingleDofJoint::setCommand(size_t _index, double _command) { case FORCE: mCommand = math::clip(_command, - mSingleDofP.mForceLowerLimit, - mSingleDofP.mForceUpperLimit); + getSingleDofJointAddon()->getForceLowerLimit(), + getSingleDofJointAddon()->getForceUpperLimit()); break; case PASSIVE: if(_command != 0.0) @@ -316,18 +257,18 @@ void SingleDofJoint::setCommand(size_t _index, double _command) break; case SERVO: mCommand = math::clip(_command, - mSingleDofP.mVelocityLowerLimit, - mSingleDofP.mVelocityUpperLimit); + getSingleDofJointAddon()->getVelocityLowerLimit(), + getSingleDofJointAddon()->getVelocityUpperLimit()); break; case ACCELERATION: mCommand = math::clip(_command, - mSingleDofP.mAccelerationLowerLimit, - mSingleDofP.mAccelerationUpperLimit); + getSingleDofJointAddon()->getAccelerationLowerLimit(), + getSingleDofJointAddon()->getAccelerationUpperLimit()); break; case VELOCITY: mCommand = math::clip(_command, - mSingleDofP.mVelocityLowerLimit, - mSingleDofP.mVelocityUpperLimit); + getSingleDofJointAddon()->getVelocityLowerLimit(), + getSingleDofJointAddon()->getVelocityUpperLimit()); // TODO: This possibly makes the acceleration to exceed the limits. break; case LOCKED: @@ -432,7 +373,7 @@ void SingleDofJoint::setPositionLowerLimit(size_t _index, double _position) return; } - mSingleDofP.mPositionLowerLimit = _position; + getSingleDofJointAddon()->setPositionLowerLimit(_position); } //============================================================================== @@ -444,7 +385,7 @@ double SingleDofJoint::getPositionLowerLimit(size_t _index) const return 0.0; } - return mSingleDofP.mPositionLowerLimit; + return getSingleDofJointAddon()->getPositionLowerLimit(); } //============================================================================== @@ -456,7 +397,7 @@ void SingleDofJoint::setPositionUpperLimit(size_t _index, double _position) return; } - mSingleDofP.mPositionUpperLimit = _position; + getSingleDofJointAddon()->setPositionUpperLimit(_position); } //============================================================================== @@ -468,7 +409,7 @@ double SingleDofJoint::getPositionUpperLimit(size_t _index) const return 0.0; } - return mSingleDofP.mPositionUpperLimit; + return getSingleDofJointAddon()->getPositionUpperLimit(); } //============================================================================== @@ -480,8 +421,8 @@ bool SingleDofJoint::hasPositionLimit(size_t _index) const return true; } - return std::isfinite(mSingleDofP.mPositionLowerLimit) - || std::isfinite(mSingleDofP.mPositionUpperLimit); + return std::isfinite(getSingleDofJointAddon()->getPositionLowerLimit()) + || std::isfinite(getSingleDofJointAddon()->getPositionUpperLimit()); } //============================================================================== @@ -493,13 +434,13 @@ void SingleDofJoint::resetPosition(size_t _index) return; } - setPositionStatic(mSingleDofP.mInitialPosition); + setPositionStatic(getSingleDofJointAddon()->getInitialPosition()); } //============================================================================== void SingleDofJoint::resetPositions() { - setPositionStatic(mSingleDofP.mInitialPosition); + setPositionStatic(getSingleDofJointAddon()->getInitialPosition()); } //============================================================================== @@ -511,7 +452,7 @@ void SingleDofJoint::setInitialPosition(size_t _index, double _initial) return; } - mSingleDofP.mInitialPosition = _initial; + getSingleDofJointAddon()->setInitialPosition(_initial); } //============================================================================== @@ -523,7 +464,7 @@ double SingleDofJoint::getInitialPosition(size_t _index) const return 0.0; } - return mSingleDofP.mInitialPosition; + return getSingleDofJointAddon()->getInitialPosition(); } //============================================================================== @@ -541,7 +482,8 @@ void SingleDofJoint::setInitialPositions(const Eigen::VectorXd& _initial) //============================================================================== Eigen::VectorXd SingleDofJoint::getInitialPositions() const { - return Eigen::Matrix::Constant(mSingleDofP.mInitialPosition); + return Eigen::Matrix::Constant( + getSingleDofJointAddon()->getInitialPosition()); } //============================================================================== @@ -607,7 +549,7 @@ void SingleDofJoint::setVelocityLowerLimit(size_t _index, double _velocity) return; } - mSingleDofP.mVelocityLowerLimit = _velocity; + getSingleDofJointAddon()->setVelocityLowerLimit(_velocity); } //============================================================================== @@ -619,7 +561,7 @@ double SingleDofJoint::getVelocityLowerLimit(size_t _index) const return 0.0; } - return mSingleDofP.mVelocityLowerLimit; + return getSingleDofJointAddon()->getVelocityLowerLimit(); } //============================================================================== @@ -631,7 +573,7 @@ void SingleDofJoint::setVelocityUpperLimit(size_t _index, double _velocity) return; } - mSingleDofP.mVelocityUpperLimit = _velocity; + getSingleDofJointAddon()->setVelocityUpperLimit(_velocity); } //============================================================================== @@ -643,7 +585,7 @@ double SingleDofJoint::getVelocityUpperLimit(size_t _index) const return 0.0; } - return mSingleDofP.mVelocityUpperLimit; + return getSingleDofJointAddon()->getVelocityUpperLimit(); } //============================================================================== @@ -655,13 +597,13 @@ void SingleDofJoint::resetVelocity(size_t _index) return; } - setVelocityStatic(mSingleDofP.mInitialVelocity); + setVelocityStatic(getSingleDofJointAddon()->getInitialVelocity()); } //============================================================================== void SingleDofJoint::resetVelocities() { - setVelocityStatic(mSingleDofP.mInitialVelocity); + setVelocityStatic(getSingleDofJointAddon()->getInitialVelocity()); } //============================================================================== @@ -673,7 +615,7 @@ void SingleDofJoint::setInitialVelocity(size_t _index, double _initial) return; } - mSingleDofP.mInitialVelocity = _initial; + getSingleDofJointAddon()->setInitialVelocity(_initial); } //============================================================================== @@ -685,7 +627,7 @@ double SingleDofJoint::getInitialVelocity(size_t _index) const return 0.0; } - return mSingleDofP.mInitialVelocity; + return getSingleDofJointAddon()->getInitialVelocity(); } //============================================================================== @@ -703,7 +645,8 @@ void SingleDofJoint::setInitialVelocities(const Eigen::VectorXd& _initial) //============================================================================== Eigen::VectorXd SingleDofJoint::getInitialVelocities() const { - return Eigen::Matrix::Constant(mSingleDofP.mInitialVelocity); + return Eigen::Matrix::Constant( + getSingleDofJointAddon()->getInitialVelocity()); } //============================================================================== @@ -776,7 +719,7 @@ void SingleDofJoint::setAccelerationLowerLimit(size_t _index, return; } - mSingleDofP.mAccelerationLowerLimit = _acceleration; + getSingleDofJointAddon()->setAccelerationLowerLimit(_acceleration); } //============================================================================== @@ -788,7 +731,7 @@ double SingleDofJoint::getAccelerationLowerLimit(size_t _index) const return 0.0; } - return mSingleDofP.mAccelerationLowerLimit; + return getSingleDofJointAddon()->getAccelerationLowerLimit(); } //============================================================================== @@ -801,7 +744,7 @@ void SingleDofJoint::setAccelerationUpperLimit(size_t _index, return; } - mSingleDofP.mAccelerationUpperLimit = _acceleration; + getSingleDofJointAddon()->setAccelerationUpperLimit(_acceleration); } //============================================================================== @@ -813,7 +756,7 @@ double SingleDofJoint::getAccelerationUpperLimit(size_t _index) const return 0.0; } - return mSingleDofP.mAccelerationUpperLimit; + return getSingleDofJointAddon()->getAccelerationUpperLimit(); } //============================================================================== @@ -939,7 +882,7 @@ void SingleDofJoint::setForceLowerLimit(size_t _index, double _force) return; } - mSingleDofP.mForceLowerLimit = _force; + getSingleDofJointAddon()->setForceLowerLimit(_force); } //============================================================================== @@ -951,7 +894,7 @@ double SingleDofJoint::getForceLowerLimit(size_t _index) const return 0.0; } - return mSingleDofP.mForceLowerLimit; + return getSingleDofJointAddon()->getForceLowerLimit(); } //============================================================================== @@ -963,7 +906,7 @@ void SingleDofJoint::setForceUpperLimit(size_t _index, double _force) return; } - mSingleDofP.mForceUpperLimit = _force; + getSingleDofJointAddon()->setForceUpperLimit(_force); } //============================================================================== @@ -975,7 +918,7 @@ double SingleDofJoint::getForceUpperLimit(size_t _index) const return 0.0; } - return mSingleDofP.mForceUpperLimit; + return getSingleDofJointAddon()->getForceUpperLimit(); } //============================================================================== @@ -1086,7 +1029,7 @@ void SingleDofJoint::setSpringStiffness(size_t _index, double _k) assert(_k >= 0.0); - mSingleDofP.mSpringStiffness = _k; + getSingleDofJointAddon()->setSpringStiffness(_k); } //============================================================================== @@ -1098,7 +1041,7 @@ double SingleDofJoint::getSpringStiffness(size_t _index) const return 0.0; } - return mSingleDofP.mSpringStiffness; + return getSingleDofJointAddon()->getSpringStiffness(); } //============================================================================== @@ -1110,18 +1053,19 @@ void SingleDofJoint::setRestPosition(size_t _index, double _q0) return; } - if (mSingleDofP.mPositionLowerLimit > _q0 - || mSingleDofP.mPositionUpperLimit < _q0) + if (getSingleDofJointAddon()->getPositionLowerLimit() > _q0 + || getSingleDofJointAddon()->getPositionUpperLimit() < _q0) { dtwarn << "[SingleDofJoint::setRestPosition] Value of _q0 [" << _q0 << "] is out of the limit range [" - << mSingleDofP.mPositionLowerLimit << ", " - << mSingleDofP.mPositionUpperLimit << "] for index [" << _index - << "] of Joint [" << getName() << "].\n"; + << getSingleDofJointAddon()->getPositionLowerLimit() << ", " + << getSingleDofJointAddon()->getPositionUpperLimit() + << "] for index [" << _index << "] of Joint [" + << getName() << "].\n"; return; } - mSingleDofP.mRestPosition = _q0; + getSingleDofJointAddon()->setRestPosition(_q0); } //============================================================================== @@ -1133,7 +1077,7 @@ double SingleDofJoint::getRestPosition(size_t _index) const return 0.0; } - return mSingleDofP.mRestPosition; + return getSingleDofJointAddon()->getRestPosition(); } //============================================================================== @@ -1147,7 +1091,7 @@ void SingleDofJoint::setDampingCoefficient(size_t _index, double _d) assert(_d >= 0.0); - mSingleDofP.mDampingCoefficient = _d; + getSingleDofJointAddon()->setDampingCoefficient(_d); } //============================================================================== @@ -1159,7 +1103,7 @@ double SingleDofJoint::getDampingCoefficient(size_t _index) const return 0.0; } - return mSingleDofP.mDampingCoefficient; + return getSingleDofJointAddon()->getDampingCoefficient(); } //============================================================================== @@ -1173,7 +1117,7 @@ void SingleDofJoint::setCoulombFriction(size_t _index, double _friction) assert(_friction >= 0.0); - mSingleDofP.mFriction = _friction; + getSingleDofJointAddon()->setFriction(_friction); } //============================================================================== @@ -1185,16 +1129,16 @@ double SingleDofJoint::getCoulombFriction(size_t _index) const return 0.0; } - return mSingleDofP.mFriction; + return getSingleDofJointAddon()->getFriction(); } //============================================================================== double SingleDofJoint::getPotentialEnergy() const { // Spring energy - double pe = 0.5 * mSingleDofP.mSpringStiffness - * (getPositionStatic() - mSingleDofP.mRestPosition) - * (getPositionStatic() - mSingleDofP.mRestPosition); + double pe = 0.5 * getSingleDofJointAddon()->getSpringStiffness() + * (getPositionStatic() - getSingleDofJointAddon()->getRestPosition()) + * (getPositionStatic() - getSingleDofJointAddon()->getRestPosition()); return pe; } @@ -1224,7 +1168,7 @@ SingleDofJoint::SingleDofJoint(const Properties& _properties) mInvM_a(0.0), mInvMassMatrixSegment(0.0) { - // Do nothing + createSingleDofJointAddon(_properties); } //============================================================================== @@ -1232,7 +1176,7 @@ void SingleDofJoint::registerDofs() { SkeletonPtr skel = getSkeleton(); if(skel) - mSingleDofP.mDofName = + getSingleDofJointAddon()->mProperties.mDofName = skel->mNameMgrForDofs.issueNewNameAndAdd(mDof->getName(), mDof); } @@ -1544,8 +1488,8 @@ void SingleDofJoint::updateInvProjArtInertiaImplicitDynamic( double projAI = Jacobian.dot(_artInertia * Jacobian); // Add additional inertia for implicit damping and spring force - projAI += _timeStep * mSingleDofP.mDampingCoefficient - + _timeStep * _timeStep * mSingleDofP.mSpringStiffness; + projAI += _timeStep * getSingleDofJointAddon()->getDampingCoefficient() + + _timeStep * _timeStep * getSingleDofJointAddon()->getSpringStiffness(); // Inversion of the projected articulated inertia for implicit damping and // spring force @@ -1737,11 +1681,12 @@ void SingleDofJoint::updateTotalForceDynamic( const double nextPosition = getPositionStatic() + _timeStep*getVelocityStatic(); const double springForce = - -mSingleDofP.mSpringStiffness * (nextPosition - mSingleDofP.mRestPosition); + -getSingleDofJointAddon()->getSpringStiffness() + * (nextPosition - getSingleDofJointAddon()->getRestPosition()); // Damping force const double dampingForce = - -mSingleDofP.mDampingCoefficient * getVelocityStatic(); + -getSingleDofJointAddon()->getDampingCoefficient() * getVelocityStatic(); // Compute alpha mTotalForce = mForce + springForce + dampingForce @@ -1897,7 +1842,7 @@ void SingleDofJoint::updateForceID(const Eigen::Vector6d& _bodyForce, if (_withDampingForces) { const double dampingForce = - -mSingleDofP.mDampingCoefficient * getVelocityStatic(); + -getSingleDofJointAddon()->getDampingCoefficient() * getVelocityStatic(); mForce -= dampingForce; } @@ -1907,7 +1852,8 @@ void SingleDofJoint::updateForceID(const Eigen::Vector6d& _bodyForce, const double nextPosition = getPositionStatic() + _timeStep*getVelocityStatic(); const double springForce = - -mSingleDofP.mSpringStiffness*(nextPosition - mSingleDofP.mRestPosition); + -getSingleDofJointAddon()->getSpringStiffness() + *(nextPosition - getSingleDofJointAddon()->getRestPosition()); mForce -= springForce; } } diff --git a/dart/dynamics/SingleDofJoint.h b/dart/dynamics/SingleDofJoint.h index f26213cfb22d1..cf56d9d393e2f 100644 --- a/dart/dynamics/SingleDofJoint.h +++ b/dart/dynamics/SingleDofJoint.h @@ -40,6 +40,8 @@ #include #include "dart/dynamics/Joint.h" +#include "dart/common/SpecializedAddonManager.h" +#include "dart/dynamics/detail/SingleDofJointProperties.h" namespace dart { namespace dynamics { @@ -53,86 +55,11 @@ class SingleDofJoint : public Joint { public: - struct UniqueProperties - { - /// Lower limit of position - double mPositionLowerLimit; + using UniqueProperties = detail::SingleDofJointUniqueProperties; + using Properties = detail::SingleDofJointProperties; + using Addon = detail::SingleDofJointAddon; - /// Upper limit of position - double mPositionUpperLimit; - - /// Initial position - double mInitialPosition; - - /// Lower limit of velocity - double mVelocityLowerLimit; - - /// Upper limit of velocity - double mVelocityUpperLimit; - - /// Initial velocity - double mInitialVelocity; - - /// Lower limit of acceleration - double mAccelerationLowerLimit; - - /// Upper limit of acceleration - double mAccelerationUpperLimit; - - /// Lower limit of force - double mForceLowerLimit; - - /// Upper limit of force - double mForceUpperLimit; - - /// Joint spring stiffness - double mSpringStiffness; - - /// Rest position for joint spring - double mRestPosition; - - /// Joint damping coefficient - double mDampingCoefficient; - - /// Coulomb friction force - double mFriction; - - /// True if the name of this Joint's DOF is not allowed to be overwritten - bool mPreserveDofName; - - /// The name of the DegreeOfFreedom for this Joint - std::string mDofName; - - /// Constructor - UniqueProperties(double _positionLowerLimit = -DART_DBL_INF, - double _positionUpperLimit = DART_DBL_INF, - double _velocityLowerLimit = -DART_DBL_INF, - double _velocityUpperLimit = DART_DBL_INF, - double _accelerationLowerLimit = -DART_DBL_INF, - double _accelerationUpperLimit = DART_DBL_INF, - double _forceLowerLimit = -DART_DBL_INF, - double _forceUpperLimit = DART_DBL_INF, - double _springStiffness = 0.0, - double _restPosition = 0.0, - double _dampingCoefficient = 0.0, - double _coulombFriction = 0.0, - bool _preserveDofName = false, - std::string _dofName = ""); - // TODO(MXG): In version 6.0, we should add mInitialPosition and - // mInitialVelocity to the constructor arguments. For now we must wait in - // order to avoid breaking the API - - virtual ~UniqueProperties() = default; - }; - - struct Properties : Joint::Properties, UniqueProperties - { - Properties( - const Joint::Properties& _jointProperties = Joint::Properties(), - const UniqueProperties& _singleDofProperties = UniqueProperties()); - - virtual ~Properties() = default; - }; + DART_BAKE_SPECIALIZED_ADDON_IRREGULAR(Addon, SingleDofJointAddon) /// Destructor virtual ~SingleDofJoint(); @@ -471,6 +398,8 @@ class SingleDofJoint : public Joint // Documentation inherited virtual Eigen::Vector6d getBodyConstraintWrench() const override; + template friend void detail::JointPropertyUpdate(AddonType*); + protected: /// Constructor called inheriting classes @@ -479,6 +408,9 @@ class SingleDofJoint : public Joint // Documentation inherited void registerDofs() override; + /// + std::string changeDofName(const std::string& name); + // Documentation inherited virtual void updateDegreeOfFreedomNames() override; @@ -653,8 +585,6 @@ class SingleDofJoint : public Joint protected: - UniqueProperties mSingleDofP; - /// \brief DegreeOfFreedom pointer DegreeOfFreedom* mDof; diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 4e30a27aa6588..c010b2bdf5980 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -65,6 +65,102 @@ namespace dynamics { #define ON_ALL_TREES( X ) for(size_t i=0; i < mTreeCache.size(); ++i) X (i); + +#define CHECK_CONFIG_VECTOR_SIZE( V ) \ + if( V .size() > 0 ) \ + { \ + if(nonzero_size != INVALID_INDEX \ + && V .size() != static_cast(nonzero_size)) \ + { \ + dterr << "[Skeleton::Configuration] Mismatch in size of vector [" << #V \ + << "] (expected " << nonzero_size << " | found " << V . size() \ + << "\n"; \ + assert(false); \ + } \ + else if(nonzero_size == INVALID_INDEX) \ + nonzero_size = V .size(); \ + } + +//============================================================================== +Skeleton::Configuration::Configuration( + const Eigen::VectorXd& positions, + const Eigen::VectorXd& velocities, + const Eigen::VectorXd& accelerations, + const Eigen::VectorXd& forces, + const Eigen::VectorXd& commands) + : mPositions(positions), + mVelocities(velocities), + mAccelerations(accelerations), + mForces(forces), + mCommands(commands) +{ + size_t nonzero_size = INVALID_INDEX; + + CHECK_CONFIG_VECTOR_SIZE(positions); + CHECK_CONFIG_VECTOR_SIZE(velocities); + CHECK_CONFIG_VECTOR_SIZE(accelerations); + CHECK_CONFIG_VECTOR_SIZE(forces); + CHECK_CONFIG_VECTOR_SIZE(commands); + + if(nonzero_size != INVALID_INDEX) + { + for(size_t i=0; i < nonzero_size; ++i) + mIndices.push_back(i); + } +} + +//============================================================================== +Skeleton::Configuration::Configuration( + const std::vector& indices, + const Eigen::VectorXd& positions, + const Eigen::VectorXd& velocities, + const Eigen::VectorXd& accelerations, + const Eigen::VectorXd& forces, + const Eigen::VectorXd& commands) + : mIndices(indices), + mPositions(positions), + mVelocities(velocities), + mAccelerations(accelerations), + mForces(forces), + mCommands(commands) +{ + size_t nonzero_size = indices.size(); + + CHECK_CONFIG_VECTOR_SIZE(positions); + CHECK_CONFIG_VECTOR_SIZE(velocities); + CHECK_CONFIG_VECTOR_SIZE(accelerations); + CHECK_CONFIG_VECTOR_SIZE(forces); + CHECK_CONFIG_VECTOR_SIZE(commands); +} + +//============================================================================== +#define RETURN_IF_CONFIG_VECTOR_IS_INEQ( V ) \ + if( V .size() != other. V .size() ) \ + return false; \ + if( V != other. V ) \ + return false; + +//============================================================================== +bool Skeleton::Configuration::operator==(const Configuration& other) const +{ + if(mIndices != other.mIndices) + return false; + + RETURN_IF_CONFIG_VECTOR_IS_INEQ(mPositions); + RETURN_IF_CONFIG_VECTOR_IS_INEQ(mVelocities); + RETURN_IF_CONFIG_VECTOR_IS_INEQ(mAccelerations); + RETURN_IF_CONFIG_VECTOR_IS_INEQ(mForces); + RETURN_IF_CONFIG_VECTOR_IS_INEQ(mCommands); + + return true; +} + +//============================================================================== +bool Skeleton::Configuration::operator!=(const Configuration& other) const +{ + return !(*this == other); +} + //============================================================================== Skeleton::Properties::Properties( const std::string& _name, @@ -72,13 +168,29 @@ Skeleton::Properties::Properties( const Eigen::Vector3d& _gravity, double _timeStep, bool _enabledSelfCollisionCheck, - bool _enableAdjacentBodyCheck) + bool _enableAdjacentBodyCheck, + size_t _version) : mName(_name), mIsMobile(_isMobile), mGravity(_gravity), mTimeStep(_timeStep), mEnabledSelfCollisionCheck(_enabledSelfCollisionCheck), - mEnabledAdjacentBodyCheck(_enableAdjacentBodyCheck) + mEnabledAdjacentBodyCheck(_enableAdjacentBodyCheck), + mVersion(_version) +{ + // Do nothing +} + +//============================================================================== +Skeleton::ExtendedProperties::ExtendedProperties( + const BodyNodeProperties& bodyNodeProperties, + const JointProperties& jointProperties, + const std::vector& parentNames, + const AddonProperties& addonProperties) + : mBodyNodeProperties(bodyNodeProperties), + mJointProperties(jointProperties), + mParentBodyNodeNames(parentNames), + mAddonProperties(addonProperties) { // Do nothing } @@ -109,6 +221,18 @@ ConstSkeletonPtr Skeleton::getPtr() const return mPtr.lock(); } +//============================================================================== +SkeletonPtr Skeleton::getSkeleton() +{ + return mPtr.lock(); +} + +//============================================================================== +ConstSkeletonPtr Skeleton::getSkeleton() const +{ + return mPtr.lock(); +} + //============================================================================== std::mutex& Skeleton::getMutex() const { @@ -125,7 +249,13 @@ Skeleton::~Skeleton() //============================================================================== SkeletonPtr Skeleton::clone() const { - SkeletonPtr skelClone = Skeleton::create(getName()); + return clone(getName()); +} + +//============================================================================== +SkeletonPtr Skeleton::clone(const std::string& cloneName) const +{ + SkeletonPtr skelClone = Skeleton::create(cloneName); for(size_t i=0; iclone(parentClone, joint); + BodyNode* newBody = getBodyNode(i)->clone(parentClone, joint, false); // The IK module gets cloned by the Skeleton and not by the BodyNode, // because IK modules rely on the Skeleton's structure and indexing. If the @@ -162,28 +292,84 @@ SkeletonPtr Skeleton::clone() const skelClone->registerBodyNode(newBody); } - for(size_t i=0; igetBodyNodePtr(); + BodyNode* newBn = skelClone->getBodyNode(originalBn->getName()); + node->cloneNode(newBn)->attach(); + } + } - // Identify the original parent BodyNode - const BodyNode* originalParent = originalEE->getParentBodyNode(); + skelClone->setProperties(getSkeletonProperties()); + skelClone->setName(cloneName); - // Grab the clone of the original parent - BodyNode* parentClone = skelClone->getBodyNode(originalParent->getName()); + return skelClone; +} - EndEffector* newEE = originalEE->clone(parentClone); +//============================================================================== +#define SET_CONFIG_VECTOR( V ) \ + if(configuration.m ## V . size() > 0) \ + { \ + if(static_cast(configuration.mIndices.size()) != \ + configuration.m ## V .size()) \ + { \ + dterr << "[Skeleton::setConfiguration] Mismatch in size of vector [" \ + << #V << "] (expected " << configuration.mIndices.size() \ + << " | found " << configuration.m ## V .size() << "\n"; \ + assert(false); \ + } \ + else \ + set ## V ( configuration.mIndices, configuration.m ## V ); \ + } - if(originalEE->getIK()) - newEE->mIK = originalEE->getIK()->clone(newEE); +//============================================================================== +void Skeleton::setConfiguration(const Configuration& configuration) +{ + SET_CONFIG_VECTOR(Positions); + SET_CONFIG_VECTOR(Velocities); + SET_CONFIG_VECTOR(Accelerations); + SET_CONFIG_VECTOR(Forces); + SET_CONFIG_VECTOR(Commands); +} - skelClone->registerEndEffector(newEE); - } +//============================================================================== +Skeleton::Configuration Skeleton::getConfiguration(int flags) const +{ + std::vector indices; + for(size_t i=0; i < getNumDofs(); ++i) + indices.push_back(i); - skelClone->setProperties(getSkeletonProperties()); + return getConfiguration(indices, flags); +} - return skelClone; +//============================================================================== +Skeleton::Configuration Skeleton::getConfiguration( + const std::vector& indices, int flags) const +{ + Configuration config(indices); + if(flags == CONFIG_NOTHING) + return config; + + if( (flags & CONFIG_POSITIONS) == CONFIG_POSITIONS ) + config.mPositions = getPositions(indices); + + if( (flags & CONFIG_VELOCITIES) == CONFIG_VELOCITIES ) + config.mVelocities = getVelocities(indices); + + if( (flags & CONFIG_ACCELERATIONS) == CONFIG_ACCELERATIONS ) + config.mAccelerations = getAccelerations(indices); + + if( (flags & CONFIG_FORCES) == CONFIG_FORCES ) + config.mForces = getForces(indices); + + if( (flags & CONFIG_COMMANDS) == CONFIG_COMMANDS ) + config.mCommands = getCommands(indices); + + return config; } //============================================================================== @@ -225,8 +411,10 @@ const std::string& Skeleton::setName(const std::string& _name) "Skeleton::DegreeOfFreedom | "+mSkeletonP.mName); mNameMgrForMarkers.setManagerName( "Skeleton::Marker | "+mSkeletonP.mName); - mNameMgrForEndEffectors.setManagerName( - "Skeleton::EndEffector | "+mSkeletonP.mName); + + for(auto& mgr : mNodeNameMgrMap) + mgr.second.setManagerName( std::string("Skeleton::") + mgr.first.name() + + " | " + mSkeletonP.mName ); ConstMetaSkeletonPtr me = mPtr.lock(); mNameChangedSignal.raise(me, oldName, mSkeletonP.mName); @@ -262,13 +450,6 @@ const std::string& Skeleton::addEntryToJointNameMgr(Joint* _newJoint, return _newJoint->mJointP.mName; } -//============================================================================== -void Skeleton::addEntryToEndEffectorNameMgr(EndEffector* _ee) -{ - _ee->mEntityP.mName = - mNameMgrForEndEffectors.issueNewNameAndAdd(_ee->getName(), _ee); -} - //============================================================================== void Skeleton::addEntryToSoftBodyNodeNameMgr(SoftBodyNode* _newNode) { @@ -369,6 +550,18 @@ const Eigen::Vector3d& Skeleton::getGravity() const return mSkeletonP.mGravity; } +//============================================================================== +size_t Skeleton::incrementVersion() +{ + return ++mSkeletonP.mVersion; +} + +//============================================================================== +size_t Skeleton::getVersion() const +{ + return mSkeletonP.mVersion; +} + //============================================================================== size_t Skeleton::getNumBodyNodes() const { @@ -393,17 +586,6 @@ size_t Skeleton::getNumTrees() const return mTreeCache.size(); } -//============================================================================== -template -static T getVectorObjectIfAvailable(size_t _idx, const std::vector& _vec) -{ - if (_idx < _vec.size()) - return _vec[_idx]; - - assert( _idx < _vec.size() ); - return nullptr; -} - //============================================================================== BodyNode* Skeleton::getRootBodyNode(size_t _treeIdx) { @@ -546,6 +728,16 @@ size_t Skeleton::getIndexOf(const BodyNode* _bn, bool _warning) const //============================================================================== const std::vector& Skeleton::getTreeBodyNodes(size_t _treeIdx) { + if(_treeIdx >= mTreeCache.size()) + { + size_t count = mTreeCache.size(); + dterr << "[Skeleton::getTreeBodyNodes] Requesting an invalid tree (" + << _treeIdx << ") " + << (count > 0? (std::string("when the max tree index is (") + std::to_string(count-1) + ")\n" ) : + std::string("when there are no trees in this Skeleton\n") ); + assert(false); + } + return mTreeCache[_treeIdx].mBodyNodes; } @@ -664,36 +856,6 @@ const std::vector& Skeleton::getTreeDofs( mTreeCache[_treeIdx].mDofs, mTreeCache[_treeIdx].mConstDofs); } -//============================================================================== -size_t Skeleton::getNumEndEffectors() const -{ - return mEndEffectors.size(); -} - -//============================================================================== -EndEffector* Skeleton::getEndEffector(size_t _idx) -{ - return getVectorObjectIfAvailable(_idx, mEndEffectors); -} - -//============================================================================== -const EndEffector* Skeleton::getEndEffector(size_t _idx) const -{ - return getVectorObjectIfAvailable(_idx, mEndEffectors); -} - -//============================================================================== -EndEffector* Skeleton::getEndEffector(const std::string& _name) -{ - return mNameMgrForEndEffectors.getObject(_name); -} - -//============================================================================== -const EndEffector* Skeleton::getEndEffector(const std::string& _name) const -{ - return mNameMgrForEndEffectors.getObject(_name); -} - //============================================================================== const std::shared_ptr& Skeleton::getIK(bool _createIfNull) { @@ -746,6 +908,9 @@ const Marker* Skeleton::getMarker(const std::string& _name) const return const_cast(this)->getMarker(_name); } +//============================================================================== +DART_BAKE_SPECIALIZED_NODE_SKEL_DEFINITIONS( Skeleton, EndEffector ) + //============================================================================== void Skeleton::setState(const Eigen::VectorXd& _state) { @@ -1396,6 +1561,27 @@ void Skeleton::setPtr(const SkeletonPtr& _ptr) resetUnion(); } +//============================================================================== +void Skeleton::constructNewTree() +{ + mTreeCache.push_back(DataCache()); + + mTreeNodeMaps.push_back(NodeMap()); + NodeMap& nodeMap = mTreeNodeMaps.back(); + + // Create the machinery needed to directly call on specialized node types + for(auto& nodeType : mSpecializedTreeNodes) + { + const std::type_index& index = nodeType.first; + nodeMap[index] = std::vector(); + + std::vector* nodeVec = nodeType.second; + nodeVec->push_back(nodeMap.find(index)); + + assert(nodeVec->size() == mTreeCache.size()); + } +} + //============================================================================== void Skeleton::registerBodyNode(BodyNode* _newBodyNode) { @@ -1417,8 +1603,9 @@ void Skeleton::registerBodyNode(BodyNode* _newBodyNode) mSkelCache.mBodyNodes.push_back(_newBodyNode); if(nullptr == _newBodyNode->getParentBodyNode()) { + // Create a new tree and add the new BodyNode to it _newBodyNode->mIndexInTree = 0; - mTreeCache.push_back(DataCache()); + constructNewTree(); mTreeCache.back().mBodyNodes.push_back(_newBodyNode); _newBodyNode->mTreeIndex = mTreeCache.size()-1; } @@ -1445,9 +1632,10 @@ void Skeleton::registerBodyNode(BodyNode* _newBodyNode) _newBodyNode->init(getPtr()); - std::vector& endEffectors = _newBodyNode->mEndEffectors; - for(EndEffector* ee : endEffectors) - registerEndEffector(ee); + BodyNode::NodeMap& nodeMap = _newBodyNode->mNodeMap; + for(auto& nodeType : nodeMap) + for(auto& node : nodeType.second) + registerNode(node); updateTotalMass(); updateCacheDimensions(_newBodyNode->mTreeIndex); @@ -1524,30 +1712,74 @@ void Skeleton::registerJoint(Joint* _newJoint) } //============================================================================== -void Skeleton::registerEndEffector(EndEffector* _newEndEffector) +void Skeleton::registerNode(NodeMap& nodeMap, Node* _newNode, size_t& _index) { -#ifndef NDEBUG // Debug mode - std::vector::iterator it = find(mEndEffectors.begin(), - mEndEffectors.end(), - _newEndEffector); + NodeMap::iterator it = nodeMap.find(typeid(*_newNode)); - if(it != mEndEffectors.end()) + if(nodeMap.end() == it) { - dterr << "[Skeleton::registerEndEffector] Attempting to double-register " - << "an EndEffector named [" << _newEndEffector->getName() << "] (" - << _newEndEffector << ") in the Skeleton named [" << getName() - << "] (" << this << "). This is most likely a bug. Please report " - << "this!\n"; - assert(false); - return; + nodeMap[typeid(*_newNode)] = std::vector(); + it = nodeMap.find(typeid(*_newNode)); } -#endif // ------- Debug mode - mEndEffectors.push_back(_newEndEffector); - _newEndEffector->mIndexInSkeleton = mEndEffectors.size()-1; + std::vector& nodes = it->second; + + if(INVALID_INDEX == _index) + { + // If this Node believes its index is invalid, then it should not exist + // anywhere in the vector + assert(std::find(nodes.begin(), nodes.end(), _newNode) == nodes.end()); + + nodes.push_back(_newNode); + _index = nodes.size()-1; + } + + assert(std::find(nodes.begin(), nodes.end(), _newNode) != nodes.end()); +} + +//============================================================================== +void Skeleton::registerNode(Node* _newNode) +{ + registerNode(mNodeMap, _newNode, _newNode->mIndexInSkeleton); + + registerNode(mTreeNodeMaps[_newNode->getBodyNodePtr()->getTreeIndex()], + _newNode, _newNode->mIndexInTree); + + const std::type_info& info = typeid(*_newNode); + NodeNameMgrMap::iterator it = mNodeNameMgrMap.find(info); + if(mNodeNameMgrMap.end() == it) + { + mNodeNameMgrMap[info] = common::NameManager( + std::string("Skeleton::") + info.name() + " | " + mSkeletonP.mName, + info.name() ); + + it = mNodeNameMgrMap.find(info); + } + + common::NameManager& mgr = it->second; + _newNode->setName(mgr.issueNewNameAndAdd(_newNode->getName(), _newNode)); +} + +//============================================================================== +void Skeleton::destructOldTree(size_t tree) +{ + mTreeCache.erase(mTreeCache.begin() + tree); + + // Decrease the tree index of every BodyNode whose tree index is higher than + // the one which is being removed. None of the BodyNodes that predate the + // current one can have a higher tree index, so they can be ignored. + for(size_t i=tree; i < mTreeCache.size(); ++i) + { + DataCache& loweredTree = mTreeCache[i]; + for(size_t j=0; j < loweredTree.mBodyNodes.size(); ++j) + loweredTree.mBodyNodes[j]->mTreeIndex = i; + } - // The EndEffector name gets added when the EndEffector is constructed, so we - // don't need to add it here. + for(auto& nodeType : mSpecializedTreeNodes) + { + std::vector* nodeRepo = nodeType.second; + nodeRepo->erase(nodeRepo->begin() + tree); + } } //============================================================================== @@ -1555,9 +1787,10 @@ void Skeleton::unregisterBodyNode(BodyNode* _oldBodyNode) { unregisterJoint(_oldBodyNode->getParentJoint()); - std::vector& endEffectors = _oldBodyNode->mEndEffectors; - for(EndEffector* ee : endEffectors) - unregisterEndEffector(ee); + BodyNode::NodeMap& nodeMap = _oldBodyNode->mNodeMap; + for(auto& nodeType : nodeMap) + for(auto& node : nodeType.second) + unregisterNode(node); mNameMgrForBodyNodes.removeName(_oldBodyNode->getName()); @@ -1583,18 +1816,8 @@ void Skeleton::unregisterBodyNode(BodyNode* _oldBodyNode) size_t tree = _oldBodyNode->getTreeIndex(); assert(mTreeCache[tree].mBodyNodes.size() == 1); assert(mTreeCache[tree].mBodyNodes[0] == _oldBodyNode); - mTreeCache.erase(mTreeCache.begin() + tree); - - // Decrease the tree index of every BodyNode whose tree index is higher than - // the one which is being removed. None of the BodyNodes that predate the - // current one can have a higher tree index, so they can be ignored. - for(size_t i=index; i < mSkelCache.mBodyNodes.size(); ++i) - { - BodyNode* bn = mSkelCache.mBodyNodes[i]; - if(bn->mTreeIndex > tree) - --bn->mTreeIndex; - } + destructOldTree(tree); updateCacheDimensions(mSkelCache); } else @@ -1670,19 +1893,59 @@ void Skeleton::unregisterJoint(Joint* _oldJoint) } //============================================================================== -void Skeleton::unregisterEndEffector(EndEffector* _oldEndEffector) +void Skeleton::unregisterNode(NodeMap& nodeMap, Node* _oldNode, size_t& _index) { - size_t index = _oldEndEffector->getIndexInSkeleton(); - assert(mEndEffectors[index] == _oldEndEffector); - mEndEffectors.erase(mEndEffectors.begin() + index); + NodeMap::iterator it = nodeMap.find(typeid(*_oldNode)); - for(size_t i=index; i < mEndEffectors.size(); ++i) + if(nodeMap.end() == it) { - EndEffector* ee = mEndEffectors[i]; - ee->mIndexInSkeleton = i; + // If the Node was not in the map, then its index should be invalid + assert(INVALID_INDEX == _index); + return; } - mNameMgrForEndEffectors.removeName(_oldEndEffector->getName()); + std::vector& nodes = it->second; + + // This Node's index in the vector should be referring to this Node + assert(nodes[_index] == _oldNode); + nodes.erase(nodes.begin() + _index); + + _index = INVALID_INDEX; +} + +//============================================================================== +void Skeleton::unregisterNode(Node* _oldNode) +{ + const size_t indexInSkel = _oldNode->mIndexInSkeleton; + unregisterNode(mNodeMap, _oldNode, _oldNode->mIndexInSkeleton); + + NodeMap::iterator node_it = mNodeMap.find(typeid(*_oldNode)); + assert(mNodeMap.end() != node_it); + + const std::vector& skelNodes = node_it->second; + for(size_t i=indexInSkel; i < skelNodes.size(); ++i) + skelNodes[i]->mIndexInSkeleton = i; + + const size_t indexInTree = _oldNode->mIndexInTree; + const size_t treeIndex = _oldNode->getBodyNodePtr()->getTreeIndex(); + NodeMap& treeNodeMap = mTreeNodeMaps[treeIndex]; + unregisterNode(treeNodeMap, _oldNode, _oldNode->mIndexInTree); + + node_it = treeNodeMap.find(typeid(*_oldNode)); + assert(treeNodeMap.end() != node_it); + + const std::vector& treeNodes = node_it->second; + for(size_t i=indexInTree; i < treeNodes.size(); ++i) + treeNodes[i]->mIndexInTree = i; + + // Remove it from the NameManager, if a NameManager is being used for this + // type. + NodeNameMgrMap::iterator name_it = mNodeNameMgrMap.find(typeid(*_oldNode)); + if(mNodeNameMgrMap.end() != name_it) + { + common::NameManager& mgr = name_it->second; + mgr.removeObject(_oldNode); + } } //============================================================================== @@ -1822,7 +2085,7 @@ std::pair Skeleton::cloneBodyNodeTree( BodyNode* newParent = i==0 ? _parentNode : nameMap[original->getParentBodyNode()->getName()]; - BodyNode* clone = original->clone(newParent, joint); + BodyNode* clone = original->clone(newParent, joint, true); clones.push_back(clone); nameMap[clone->getName()] = clone; diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index 50282dc240af0..6ebc3c1f1f68a 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -43,6 +43,8 @@ #include "dart/dynamics/MetaSkeleton.h" #include "dart/dynamics/SmartPointer.h" #include "dart/dynamics/HierarchicalIK.h" +#include "dart/dynamics/Joint.h" +#include "dart/dynamics/BodyNode.h" namespace dart { namespace renderer { @@ -53,16 +55,78 @@ class RenderInterface; namespace dart { namespace dynamics { -class EndEffector; - /// class Skeleton -class Skeleton : public MetaSkeleton +class Skeleton : public virtual common::AddonManager, + public MetaSkeleton, + public virtual SpecializedNodeManagerForSkeleton { public: + enum ConfigFlag_t + { + CONFIG_NOTHING = 0, + CONFIG_POSITIONS = 1 << 1, + CONFIG_VELOCITIES = 1 << 2, + CONFIG_ACCELERATIONS = 1 << 3, + CONFIG_FORCES = 1 << 4, + CONFIG_COMMANDS = 1 << 5, + CONFIG_ALL = 0xFF + }; + + /// The Configuration struct represents the joint configuration of a Skeleton. + /// The size of each Eigen::VectorXd member in this struct must be equal to + /// the number of degrees of freedom in the Skeleton or it must be zero. We + /// assume that any Eigen::VectorXd member with zero entries should be + /// ignored. + struct Configuration + { + Configuration( + const Eigen::VectorXd& positions = Eigen::VectorXd(), + const Eigen::VectorXd& velocities = Eigen::VectorXd(), + const Eigen::VectorXd& accelerations = Eigen::VectorXd(), + const Eigen::VectorXd& forces = Eigen::VectorXd(), + const Eigen::VectorXd& commands = Eigen::VectorXd()); + + Configuration( + const std::vector& indices, + const Eigen::VectorXd& positions = Eigen::VectorXd(), + const Eigen::VectorXd& velocities = Eigen::VectorXd(), + const Eigen::VectorXd& accelerations = Eigen::VectorXd(), + const Eigen::VectorXd& forces = Eigen::VectorXd(), + const Eigen::VectorXd& commands = Eigen::VectorXd()); + + /// A list of degree of freedom indices that each entry in the + /// Eigen::VectorXd members correspond to. + std::vector mIndices; + + /// Joint positions + Eigen::VectorXd mPositions; + + /// Joint velocities + Eigen::VectorXd mVelocities; + + /// Joint accelerations + Eigen::VectorXd mAccelerations; + + /// Joint forces + Eigen::VectorXd mForces; + + /// Joint commands + Eigen::VectorXd mCommands; + + /// Equality comparison operator + bool operator==(const Configuration& other) const; + + /// Inequality comparison operator + bool operator!=(const Configuration& other) const; + }; + + /// The Properties of this Skeleton which are independent of the components + /// within the Skeleton, such as its BodyNodes and Joints. This does not + /// include any Properties of the Skeleton's Addons. struct Properties { - /// Name + /// Name of the Skeleton std::string mName; /// If the skeleton is not mobile, its dynamic effect is equivalent @@ -85,13 +149,54 @@ class Skeleton : public MetaSkeleton /// ignored. bool mEnabledAdjacentBodyCheck; + /// Version number of the Skeleton. This will get incremented each time any + /// Property of the Skeleton or a component within the Skeleton is changed. + /// If you create a custom Addon or Node, you must increment the Skeleton + /// version number each time one of its Properties is changed, or else the + /// machinery used to record Skeletons and Worlds might fail to capture its + /// Property changes. + size_t mVersion; + + /// Default constructor Properties( const std::string& _name = "Skeleton", bool _isMobile = true, const Eigen::Vector3d& _gravity = Eigen::Vector3d(0.0, 0.0, -9.81), double _timeStep = 0.001, bool _enabledSelfCollisionCheck = false, - bool _enableAdjacentBodyCheck = false); + bool _enableAdjacentBodyCheck = false, + size_t _version = 0); + }; + + using BodyNodeProperties = std::vector; + using JointProperties = std::vector; + using AddonProperties = common::AddonManager::Properties; + + /// The Properties of this Skeleton and everything within the Skeleton, + /// including Addons and Nodes that are attached to the + struct ExtendedProperties : Properties + { + /// Properties of all the BodyNodes in this Skeleton + BodyNodeProperties mBodyNodeProperties; + + /// Properties of all the Joints in this Skeleton + JointProperties mJointProperties; + + /// A list of the name of the parent of each BodyNode in this Skeleton. This + /// allows the layout of the Skeleton to be reconstructed. + std::vector mParentBodyNodeNames; + + /// Properties of any Addons that are attached directly to this Skeleton + /// object (does NOT include Addons that are attached to BodyNodes or Joints + /// within this Skeleton). + AddonProperties mAddonProperties; + + /// Default constructor + ExtendedProperties( + const BodyNodeProperties& bodyNodeProperties = BodyNodeProperties(), + const JointProperties& jointProperties = JointProperties(), + const std::vector& parentNames = std::vector(), + const AddonProperties& addonProperties = AddonProperties()); }; //---------------------------------------------------------------------------- @@ -110,6 +215,14 @@ class Skeleton : public MetaSkeleton /// Get the shared_ptr that manages this Skeleton ConstSkeletonPtr getPtr() const; + /// Same as getPtr(), but this allows Skeleton to have a similar interface as + /// BodyNode and Joint for template programming. + SkeletonPtr getSkeleton(); + + /// Same as getPtr(), but this allows Skeleton to have a similar interface as + /// BodyNode and Joint for template programming. + ConstSkeletonPtr getSkeleton() const; + /// Get the mutex that protects the state of this Skeleton std::mutex& getMutex() const; @@ -127,8 +240,28 @@ class Skeleton : public MetaSkeleton /// properties will be [TODO(MXG): copy the state as well] SkeletonPtr clone() const; + /// Create an identical clone of this Skeleton, except that it has a new name. + /// + /// Note: the state of the Skeleton will NOT be cloned, only the structure and + /// properties will be [TODO(MXG): copy the state as well] + SkeletonPtr clone(const std::string& cloneName) const; + /// \} + //---------------------------------------------------------------------------- + /// \{ \name Configuration + //---------------------------------------------------------------------------- + + /// Set the configuration of this Skeleton + void setConfiguration(const Configuration& configuration); + + /// Get the configuration of this Skeleton + Configuration getConfiguration(int flags = CONFIG_ALL) const; + + /// Get the configuration of the specified indices in this Skeleton + Configuration getConfiguration(const std::vector& indices, + int flags = CONFIG_ALL) const; + //---------------------------------------------------------------------------- /// \{ \name Properties //---------------------------------------------------------------------------- @@ -180,6 +313,13 @@ class Skeleton : public MetaSkeleton /// Get 3-dim gravitational acceleration. const Eigen::Vector3d& getGravity() const; + /// Increment the version number of this Skeleton and return the resulting + /// (new) version number. + size_t incrementVersion(); + + /// Get the current version number of this Skeleton + size_t getVersion() const; + /// \} //---------------------------------------------------------------------------- @@ -328,21 +468,6 @@ class Skeleton : public MetaSkeleton /// Get the DegreesOfFreedom belonging to a tree in this Skeleton const std::vector& getTreeDofs(size_t _treeIdx) const; - /// Get the number of EndEffectors on this Skeleton - size_t getNumEndEffectors() const; - - /// Get EndEffector whose index is _idx - EndEffector* getEndEffector(size_t _idx); - - /// Get EndEffector whose index is _idx - const EndEffector* getEndEffector(size_t _idx) const; - - /// Get EndEffector whose name is _name - EndEffector* getEndEffector(const std::string& _name); - - /// Get EndEffector whose name is _name - const EndEffector* getEndEffector(const std::string &_name) const; - /// Get a pointer to a WholeBodyIK module for this Skeleton. If _createIfNull /// is true, then the IK module will be generated if one does not already /// exist. @@ -376,6 +501,8 @@ class Skeleton : public MetaSkeleton /// Get const marker whose name is _name const Marker* getMarker(const std::string& _name) const; + DART_BAKE_SPECIALIZED_NODE_SKEL_DECLARATIONS( EndEffector ) + /// \} //---------------------------------------------------------------------------- @@ -831,6 +958,7 @@ class Skeleton : public MetaSkeleton friend class SingleDofJoint; template friend class MultiDofJoint; friend class DegreeOfFreedom; + friend class Node; friend class EndEffector; protected: @@ -842,14 +970,23 @@ class Skeleton : public MetaSkeleton /// Setup this Skeleton with its shared_ptr void setPtr(const SkeletonPtr& _ptr); + /// Construct a new tree in the Skeleton + void constructNewTree(); + /// Register a BodyNode with the Skeleton. Internal use only. void registerBodyNode(BodyNode* _newBodyNode); /// Register a Joint with the Skeleton. Internal use only. void registerJoint(Joint* _newJoint); - /// Register an EndEffector with the Skeleton. Internal use only. - void registerEndEffector(EndEffector* _newEndEffector); + /// Register a Node with the Skeleton. Internal use only. + void registerNode(NodeMap& nodeMap, Node* _newNode, size_t& _index); + + /// Register a Node with the Skeleton. Internal use only. + void registerNode(Node* _newNode); + + /// Remove an old tree from the Skeleton + void destructOldTree(size_t tree); /// Remove a BodyNode from the Skeleton. Internal use only. void unregisterBodyNode(BodyNode* _oldBodyNode); @@ -857,8 +994,11 @@ class Skeleton : public MetaSkeleton /// Remove a Joint from the Skeleton. Internal use only. void unregisterJoint(Joint* _oldJoint); - /// Remove an EndEffector from the Skeleton. Internal use only. - void unregisterEndEffector(EndEffector* _oldEndEffector); + /// Remove a Node from the Skeleton. Internal use only. + void unregisterNode(NodeMap& nodeMap, Node* _oldNode, size_t& _index); + + /// Remove a Node from the Skeleton. Internal use only. + void unregisterNode(Node* _oldNode); /// Move a subtree of BodyNodes from this Skeleton to another Skeleton bool moveBodyNodeTree(Joint* _parentJoint, BodyNode* _bodyNode, @@ -983,9 +1123,6 @@ class Skeleton : public MetaSkeleton /// Add a Joint to to the Joint NameManager const std::string& addEntryToJointNameMgr(Joint* _newJoint, bool _updateDofNames=true); - /// Add an EndEffector to the EndEffector NameManager - void addEntryToEndEffectorNameMgr(EndEffector* _ee); - /// Add a SoftBodyNode to the SoftBodyNode NameManager void addEntryToSoftBodyNodeNameMgr(SoftBodyNode* _newNode); @@ -1009,9 +1146,6 @@ class Skeleton : public MetaSkeleton /// List of Soft body node list in the skeleton std::vector mSoftBodyNodes; - /// List of EndEffectors in the Skeleton - std::vector mEndEffectors; - /// NameManager for tracking BodyNodes dart::common::NameManager mNameMgrForBodyNodes; @@ -1027,9 +1161,6 @@ class Skeleton : public MetaSkeleton /// NameManager for tracking Markers dart::common::NameManager mNameMgrForMarkers; - /// NameManager for tracking EndEffectors - dart::common::NameManager mNameMgrForEndEffectors; - /// WholeBodyIK module for this Skeleton std::shared_ptr mWholeBodyIK; @@ -1145,6 +1276,10 @@ class Skeleton : public MetaSkeleton mutable DataCache mSkelCache; + using SpecializedTreeNodes = std::map*>; + + SpecializedTreeNodes mSpecializedTreeNodes; + /// Total mass. double mTotalMass; @@ -1179,9 +1314,13 @@ class Skeleton : public MetaSkeleton EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -#include "dart/dynamics/detail/Skeleton.h" - } // namespace dynamics } // namespace dart +#include "dart/dynamics/EndEffector.h" +#include "dart/dynamics/detail/SpecializedNodeManager.h" + +#include "dart/dynamics/detail/Skeleton.h" +#include "dart/dynamics/detail/BodyNode.h" + #endif // DART_DYNAMICS_SKELETON_H_ diff --git a/dart/dynamics/SmartPointer.h b/dart/dynamics/SmartPointer.h index 08519a68ad975..ada68ca4c0577 100644 --- a/dart/dynamics/SmartPointer.h +++ b/dart/dynamics/SmartPointer.h @@ -55,6 +55,8 @@ namespace dynamics { DART_COMMON_MAKE_SHARED_WEAK(SimpleFrame) +DART_COMMON_MAKE_SHARED_WEAK(NodeDestructor) + //----------------------------------------------------------------------------- // Skeleton Smart Pointers //----------------------------------------------------------------------------- diff --git a/dart/dynamics/SoftBodyNode.cpp b/dart/dynamics/SoftBodyNode.cpp index 020c04e33cabf..c0643cb4a3b1b 100644 --- a/dart/dynamics/SoftBodyNode.cpp +++ b/dart/dynamics/SoftBodyNode.cpp @@ -283,7 +283,6 @@ SoftBodyNode::SoftBodyNode(BodyNode* _parentBodyNode, const Properties& _properties) : Entity(Frame::World(), "", false), Frame(Frame::World(), ""), - Node(ConstructBodyNode), BodyNode(_parentBodyNode, _parentJoint, _properties) { mNotifier = new PointMassNotifier(this, "PointMassNotifier"); @@ -292,10 +291,17 @@ SoftBodyNode::SoftBodyNode(BodyNode* _parentBodyNode, //============================================================================== BodyNode* SoftBodyNode::clone(BodyNode* _parentBodyNode, - Joint* _parentJoint) const + Joint* _parentJoint, bool cloneNodes) const { - return new SoftBodyNode(_parentBodyNode, _parentJoint, - getSoftBodyNodeProperties()); + SoftBodyNode* clonedBn = new SoftBodyNode( + _parentBodyNode, _parentJoint, getSoftBodyNodeProperties()); + + clonedBn->matchAddons(this); + + if(cloneNodes) + clonedBn->matchNodes(this); + + return clonedBn; } //============================================================================== diff --git a/dart/dynamics/SoftBodyNode.h b/dart/dynamics/SoftBodyNode.h index 41793544365b6..6331805c212f0 100644 --- a/dart/dynamics/SoftBodyNode.h +++ b/dart/dynamics/SoftBodyNode.h @@ -211,7 +211,8 @@ class SoftBodyNode : public BodyNode /// Create a clone of this SoftBodyNode. This may only be called by the /// Skeleton class. - virtual BodyNode* clone(BodyNode* _parentBodyNode, Joint* _parentJoint) const override; + virtual BodyNode* clone(BodyNode* _parentBodyNode, Joint* _parentJoint, + bool cloneNodes) const override; //-------------------------------------------------------------------------- // Sub-functions for Recursive Kinematics Algorithms diff --git a/dart/dynamics/SpecializedNodeManager.h b/dart/dynamics/SpecializedNodeManager.h new file mode 100644 index 0000000000000..f3d89913da64a --- /dev/null +++ b/dart/dynamics/SpecializedNodeManager.h @@ -0,0 +1,213 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_DYNAMICS_SPECIALIZEDNODEMANAGER_H_ +#define DART_DYNAMICS_SPECIALIZEDNODEMANAGER_H_ + +#include "dart/common/Virtual.h" +#include "dart/dynamics/detail/BasicNodeManager.h" +#include "dart/dynamics/NodeManagerJoiner.h" + +namespace dart { +namespace dynamics { + +class BodyNode; +class Skeleton; + +//============================================================================== +/// Declaration of the variadic template +template +class SpecializedNodeManagerForBodyNode { }; + +//============================================================================== +/// SpecializedNodeManagerForBodyNode allows classes that inherit BodyNode to +/// have constant-time access to a specific type of Node +template +class SpecializedNodeManagerForBodyNode : + public virtual detail::BasicNodeManagerForBodyNode +{ +public: + + /// Default constructor + SpecializedNodeManagerForBodyNode(); + + /// Get the number of Nodes corresponding to the specified type + template + size_t getNumNodes() const; + + /// Get the Node of the specified type and the specified index + template + NodeType* getNode(size_t index); + + /// Get the Node of the specified type and the specified index + template + const NodeType* getNode(size_t index) const; + + /// Check if this Manager is specialized for a specific type of Node + template + static constexpr bool isSpecializedForNode(); + +protected: + + /// Redirect to BasicNodeManagerForBodyNode::getNumNodes() + template + size_t _getNumNodes(type) const; + + /// Specialized implementation of getNumNodes() + size_t _getNumNodes(type) const; + + /// Redirect to BasicNodeManagerForBodyNode::getNode(size_t) + template + NodeType* _getNode(type, size_t index); + + /// Specialized implementation of getNode(size_t) + SpecNode* _getNode(type, size_t index); + + /// Return false + template + static constexpr bool _isSpecializedForNode(type); + + /// Return true + static constexpr bool _isSpecializedForNode(type); + + /// Iterator that allows direct access to the specialized Nodes + BasicNodeManagerForBodyNode::NodeMap::iterator mSpecNodeIterator; + +}; + +//============================================================================== +/// This is the variadic version of the SpecializedNodeManagerForBodyNode class +/// which allows you to include arbitrarily many specialized types in the +/// specialization. +template +class SpecializedNodeManagerForBodyNode : + public NodeManagerJoinerForBodyNode< + common::Virtual< SpecializedNodeManagerForBodyNode >, + common::Virtual< SpecializedNodeManagerForBodyNode > > { }; + +//============================================================================== +/// Declaration of the variadic template +template +class SpecializedNodeManagerForSkeleton { }; + +//============================================================================== +/// SpecializedNodeManagerForSkeleton allows classes that inherit Skeleton to +/// have constant-time access to a specific type of Node +template +class SpecializedNodeManagerForSkeleton : + public virtual detail::BasicNodeManagerForSkeleton, + public virtual SpecializedNodeManagerForBodyNode +{ +public: + + using SpecializedNodeManagerForBodyNode::getNode; + using SpecializedNodeManagerForBodyNode::getNumNodes; + using SpecializedNodeManagerForBodyNode::isSpecializedForNode; + + SpecializedNodeManagerForSkeleton(); + + /// Get the number of Nodes of the specified type that are in the treeIndexth + /// tree of this Skeleton + template + size_t getNumNodes(size_t treeIndex) const; + + /// Get the nodeIndexth Node of the specified type within the tree of + /// treeIndex. + template + NodeType* getNode(size_t treeIndex, size_t nodeIndex); + + /// Get the nodeIndexth Node of the specified type within the tree of + /// treeIndex. + template + const NodeType* getNode(size_t treeIndex, size_t nodeIndex) const; + + /// Get the Node of the specified type with the given name. + template + NodeType* getNode(const std::string& name); + + /// Get the Node of the specified type with the given name. + template + const NodeType* getNode(const std::string& name) const; + +protected: + + /// Redirect to BasicNodeManagerForSkeleton::getNumNodes(size_t) + template + size_t _getNumNodes(type, size_t treeIndex) const; + + /// Specialized implementation of getNumNodes(size_t) + size_t _getNumNodes(type, size_t treeIndex) const; + + /// Redirect to BasicNodeManagerForSkeleton::getNode(size_t, size_t) + template + NodeType* _getNode(type, size_t treeIndex, size_t nodeIndex); + + /// Specialized implementation of getNode(size_t, size_t) + SpecNode* _getNode(type, size_t treeIndex, size_t nodeIndex); + + /// Redirect to BasicNodeManagerForSkeleton::getNode(const std::string&) + template + NodeType* _getNode(type, const std::string& name); + + /// Specialized implementation of getNode(const std::string&) + SpecNode* _getNode(type, const std::string& name); + + /// std::vector of iterators that allow direct access to the specialized Nodes + /// of each tree + std::vector mTreeSpecNodeIterators; + + /// Iterator that gives direct access to the name manager of the specialized + /// Nodes + NodeNameMgrMap::iterator mSpecNodeNameMgrIterator; + +}; + +//============================================================================== +/// This is the variadic version of the SpecializedNodeManagerForSkeleton class +/// which allows you to include arbitrarily many specialized types in the +/// specialization. +template +class SpecializedNodeManagerForSkeleton : + public NodeManagerJoinerForSkeleton< + common::Virtual< SpecializedNodeManagerForSkeleton >, + common::Virtual< SpecializedNodeManagerForSkeleton > > { }; + +} // namespace dynamics +} // namespace dart + +#include "dart/dynamics/BodyNode.h" + +#endif // DART_DYNAMICS_SPECIALIZEDNODEMANAGER_H_ diff --git a/dart/dynamics/TemplatedJacobianNode.h b/dart/dynamics/TemplatedJacobianNode.h index c8bdb753e2f7b..5e1433e668413 100644 --- a/dart/dynamics/TemplatedJacobianNode.h +++ b/dart/dynamics/TemplatedJacobianNode.h @@ -50,7 +50,7 @@ namespace dynamics { /// This style of implementation allows BodyNode and EndEffector to share the /// implementations of these various auxiliary Jacobian functions without any /// penalty from dynamic overload resolution. -template +template class TemplatedJacobianNode : public JacobianNode { public: @@ -122,9 +122,8 @@ class TemplatedJacobianNode : public JacobianNode protected: - /// Default constructor. This is only a formality, because Entity and Frame - /// do not offer default constructors. - TemplatedJacobianNode(); + /// Constructor + TemplatedJacobianNode(BodyNode* bn); }; diff --git a/dart/dynamics/TranslationalJoint.cpp b/dart/dynamics/TranslationalJoint.cpp index 0ae6490190466..537c90eca6faf 100644 --- a/dart/dynamics/TranslationalJoint.cpp +++ b/dart/dynamics/TranslationalJoint.cpp @@ -76,8 +76,9 @@ TranslationalJoint::Properties TranslationalJoint::getTranslationalJointProperti TranslationalJoint::TranslationalJoint(const Properties& _properties) : MultiDofJoint<3>(_properties) { - setProperties(_properties); - updateDegreeOfFreedomNames(); + // Inherited Joint Properties must be set in the final joint class or else we + // get pure virtual function calls + MultiDofJoint<3>::setProperties(_properties); } //============================================================================== diff --git a/dart/dynamics/UniversalJoint.cpp b/dart/dynamics/UniversalJoint.cpp index 925f7916793e3..64f3a38627b1f 100644 --- a/dart/dynamics/UniversalJoint.cpp +++ b/dart/dynamics/UniversalJoint.cpp @@ -44,24 +44,6 @@ namespace dart { namespace dynamics { -//============================================================================== -UniversalJoint::UniqueProperties::UniqueProperties( - const Eigen::Vector3d& _axis1, const Eigen::Vector3d& _axis2) - : mAxis({_axis1.normalized(), _axis2.normalized()}) -{ - // Do nothing -} - -//============================================================================== -UniversalJoint::Properties::Properties( - const MultiDofJoint::Properties& _multiDofProperties, - const UniversalJoint::UniqueProperties& _universalProperties) - : MultiDofJoint::Properties(_multiDofProperties), - UniversalJoint::UniqueProperties(_universalProperties) -{ - // Do nothing -} - //============================================================================== UniversalJoint::~UniversalJoint() { @@ -86,7 +68,8 @@ void UniversalJoint::setProperties(const UniqueProperties& _properties) //============================================================================== UniversalJoint::Properties UniversalJoint::getUniversalJointProperties() const { - return Properties(getMultiDofJointProperties(), mUniversalP); + return Properties(getMultiDofJointProperties(), + getUniversalJointAddon()->getProperties()); } //============================================================================== @@ -136,27 +119,25 @@ bool UniversalJoint::isCyclic(size_t _index) const //============================================================================== void UniversalJoint::setAxis1(const Eigen::Vector3d& _axis) { - mUniversalP.mAxis[0] = _axis.normalized(); - notifyPositionUpdate(); + getUniversalJointAddon()->setAxis1(_axis); } //============================================================================== void UniversalJoint::setAxis2(const Eigen::Vector3d& _axis) { - mUniversalP.mAxis[1] = _axis.normalized(); - notifyPositionUpdate(); + getUniversalJointAddon()->setAxis2(_axis); } //============================================================================== const Eigen::Vector3d& UniversalJoint::getAxis1() const { - return mUniversalP.mAxis[0]; + return getUniversalJointAddon()->getAxis1(); } //============================================================================== const Eigen::Vector3d& UniversalJoint::getAxis2() const { - return mUniversalP.mAxis[1]; + return getUniversalJointAddon()->getAxis2(); } //============================================================================== @@ -166,20 +147,21 @@ Eigen::Matrix UniversalJoint::getLocalJacobianStatic( Eigen::Matrix J; J.col(0) = math::AdTAngular( mJointP.mT_ChildBodyToJoint - * math::expAngular(-mUniversalP.mAxis[1] * _positions[1]), - mUniversalP.mAxis[0]); - J.col(1) = math::AdTAngular(mJointP.mT_ChildBodyToJoint, - mUniversalP.mAxis[1]); + * math::expAngular(-getAxis2() * _positions[1]), getAxis1()); + J.col(1) = math::AdTAngular(mJointP.mT_ChildBodyToJoint, getAxis2()); assert(!math::isNan(J)); return J; } //============================================================================== UniversalJoint::UniversalJoint(const Properties& _properties) - : MultiDofJoint<2>(_properties) + : detail::UniversalJointBase(_properties, common::NoArg) { - setProperties(_properties); - updateDegreeOfFreedomNames(); + createUniversalJointAddon(_properties); + + // Inherited Joint Properties must be set in the final joint class or else we + // get pure virtual function calls + MultiDofJoint<2>::setProperties(_properties); } //============================================================================== @@ -202,8 +184,8 @@ void UniversalJoint::updateLocalTransform() const { const Eigen::Vector2d& positions = getPositionsStatic(); mT = mJointP.mT_ParentBodyToJoint - * Eigen::AngleAxisd(positions[0], mUniversalP.mAxis[0]) - * Eigen::AngleAxisd(positions[1], mUniversalP.mAxis[1]) + * Eigen::AngleAxisd(positions[0], getAxis1()) + * Eigen::AngleAxisd(positions[1], getAxis2()) * mJointP.mT_ChildBodyToJoint.inverse(); assert(math::verifyTransform(mT)); } @@ -220,12 +202,11 @@ void UniversalJoint::updateLocalJacobianTimeDeriv() const Eigen::Vector6d tmpV1 = getLocalJacobianStatic().col(1) * getVelocitiesStatic()[1]; - Eigen::Isometry3d tmpT = math::expAngular(-mUniversalP.mAxis[1] - * getPositionsStatic()[1]); + Eigen::Isometry3d tmpT = math::expAngular( + -getAxis2() * getPositionsStatic()[1]); Eigen::Vector6d tmpV2 - = math::AdTAngular(mJointP.mT_ChildBodyToJoint * tmpT, - mUniversalP.mAxis[0]); + = math::AdTAngular(mJointP.mT_ChildBodyToJoint * tmpT, getAxis1()); mJacobianDeriv.col(0) = -math::ad(tmpV1, tmpV2); diff --git a/dart/dynamics/UniversalJoint.h b/dart/dynamics/UniversalJoint.h index dccf8bbfdf1fb..465d2720be20a 100644 --- a/dart/dynamics/UniversalJoint.h +++ b/dart/dynamics/UniversalJoint.h @@ -37,41 +37,23 @@ #ifndef DART_DYNAMICS_UNIVERSALJOINT_H_ #define DART_DYNAMICS_UNIVERSALJOINT_H_ -#include - -#include - -#include "dart/dynamics/MultiDofJoint.h" +#include "dart/dynamics/detail/UniversalJointProperties.h" namespace dart { namespace dynamics { /// class UniversalJoint -class UniversalJoint : public MultiDofJoint<2> +class UniversalJoint : public detail::UniversalJointBase { public: friend class Skeleton; + using UniqueProperties = detail::UniversalJointUniqueProperties; + using Properties = detail::UniversalJointProperties; + using Addon = detail::UniversalJointAddon; + using Base = detail::UniversalJointBase; - struct UniqueProperties - { - std::array mAxis; - - UniqueProperties(const Eigen::Vector3d& _axis1 = Eigen::Vector3d::UnitX(), - const Eigen::Vector3d& _axis2 = Eigen::Vector3d::UnitY()); - - virtual ~UniqueProperties() = default; - }; - - struct Properties : MultiDofJoint<2>::Properties, - UniversalJoint::UniqueProperties - { - Properties(const MultiDofJoint<2>::Properties& _multiDofProperties = - MultiDofJoint<2>::Properties(), - const UniversalJoint::UniqueProperties& _universalProperties = - UniversalJoint::UniqueProperties()); - virtual ~Properties() = default; - }; + DART_BAKE_SPECIALIZED_ADDON_IRREGULAR(Addon, UniversalJointAddon) UniversalJoint(const UniversalJoint&) = delete; @@ -121,6 +103,8 @@ class UniversalJoint : public MultiDofJoint<2> Eigen::Matrix getLocalJacobianStatic( const Eigen::Vector2d& _positions) const override; + template friend void detail::JointPropertyUpdate(AddonType*); + protected: /// Constructor called by Skeleton class @@ -143,11 +127,6 @@ class UniversalJoint : public MultiDofJoint<2> // Documentation inherited virtual void updateLocalJacobianTimeDeriv() const override; -protected: - - /// UniversalJoint Properties - UniqueProperties mUniversalP; - public: // To get byte-aligned Eigen vectors EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/dart/dynamics/WeldJoint.cpp b/dart/dynamics/WeldJoint.cpp index e8c054ef4b277..5f19846933bbd 100644 --- a/dart/dynamics/WeldJoint.cpp +++ b/dart/dynamics/WeldJoint.cpp @@ -102,7 +102,9 @@ void WeldJoint::setTransformFromChildBodyNode(const Eigen::Isometry3d& _T) WeldJoint::WeldJoint(const Properties& _properties) : ZeroDofJoint(_properties) { - setProperties(_properties); + // Inherited Joint Properties must be set in the final joint class or else we + // get pure virtual function calls + ZeroDofJoint::setProperties(_properties); } //============================================================================== diff --git a/dart/dynamics/detail/Addon.h b/dart/dynamics/detail/Addon.h new file mode 100644 index 0000000000000..9a965936353e1 --- /dev/null +++ b/dart/dynamics/detail/Addon.h @@ -0,0 +1,464 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_DYNAMICS_DETAIL_ADDON_H_ +#define DART_DYNAMICS_DETAIL_ADDON_H_ + +#include "dart/dynamics/Addon.h" + +namespace dart { +namespace dynamics { + +//============================================================================== +template +AddonWithProtectedPropertiesInSkeleton< + BaseT, PropertiesDataT, ManagerT, updateProperties, OptionalT>:: +AddonWithProtectedPropertiesInSkeleton( + common::AddonManager* mgr, const PropertiesData& properties) + : Addon(mgr), + mProperties(properties) +{ + DART_COMMON_CAST_NEW_MANAGER_TYPE( + Base, ManagerType, mgr, castedManager, constructor); + mManager = castedManager; +} + +//============================================================================== +template +std::unique_ptr AddonWithProtectedPropertiesInSkeleton< + BaseT, PropertiesDataT, ManagerT, updateProperties, OptionalT>:: +cloneAddon(common::AddonManager* newManager) const +{ + DART_COMMON_CAST_NEW_MANAGER_TYPE_AND_RETURN_NULL_IF_BAD( + Base, ManagerType, newManager, castedManager, clone); + return std::unique_ptr(new Base(castedManager, mProperties)); +} + +//============================================================================== +template +void AddonWithProtectedPropertiesInSkeleton< + BaseT, PropertiesDataT, ManagerT, updateProperties, OptionalT>:: +setAddonProperties(const Addon::Properties& someProperties) +{ + setProperties(static_cast(someProperties)); +} + +//============================================================================== +template +const common::Addon::Properties* AddonWithProtectedPropertiesInSkeleton< + BaseT, PropertiesDataT, ManagerT, updateProperties, OptionalT>:: +getAddonProperties() const +{ + return &mProperties; +} + +//============================================================================== +template +void AddonWithProtectedPropertiesInSkeleton< + BaseT, PropertiesDataT, ManagerT, updateProperties, OptionalT>:: +setProperties(const PropertiesData& properties) +{ + static_cast(mProperties) = properties; + + UpdateProperties(static_cast(this)); + incrementSkeletonVersion(); +} + +//============================================================================== +template +auto AddonWithProtectedPropertiesInSkeleton< + BaseT, PropertiesDataT, ManagerT, updateProperties, OptionalT>:: +getProperties() const -> const Properties& +{ + return mProperties; +} + +//============================================================================== +template +bool AddonWithProtectedPropertiesInSkeleton< + BaseT, PropertiesDataT, ManagerT, updateProperties, OptionalT>:: +isOptional(common::AddonManager* oldManager) +{ + if(Optional) + return true; + + // If the Addon is not optional, we should check whether the Manager type is + // the kind that this Addon belongs to. + return (nullptr == dynamic_cast(oldManager)); +} + +//============================================================================== +template +SkeletonPtr AddonWithProtectedPropertiesInSkeleton< + BaseT, PropertiesDataT, ManagerT, updateProperties, OptionalT>::getSkeleton() +{ + if(mManager) + return mManager->getSkeleton(); + + return nullptr; +} + +//============================================================================== +template +ConstSkeletonPtr AddonWithProtectedPropertiesInSkeleton< + BaseT, PropertiesDataT, ManagerT, updateProperties, OptionalT>::getSkeleton() const +{ + if(mManager) + return mManager->getSkeleton(); + + return nullptr; +} + +//============================================================================== +template +ManagerT* AddonWithProtectedPropertiesInSkeleton< + BaseT, PropertiesDataT, ManagerT, updateProperties, OptionalT>::getManager() +{ + return mManager; +} + +//============================================================================== +template +const ManagerT* AddonWithProtectedPropertiesInSkeleton< + BaseT, PropertiesDataT, ManagerT, updateProperties, OptionalT>::getManager() const +{ + return mManager; +} + +//============================================================================== +template +void AddonWithProtectedPropertiesInSkeleton< + BaseT, PropertiesDataT, ManagerT, updateProperties, OptionalT>:: +incrementSkeletonVersion() +{ + if(const SkeletonPtr& skel = getSkeleton()) + skel->incrementVersion(); +} + +//============================================================================== +template +void AddonWithProtectedPropertiesInSkeleton< + BaseT, PropertiesDataT, ManagerT, updateProperties, OptionalT>:: +setManager(common::AddonManager* newManager, bool /*transfer*/) +{ + DART_COMMON_CAST_NEW_MANAGER_TYPE( + Base, ManagerType, newManager, castedManager, setManager); + mManager = castedManager; + + if(mManager) + { + UpdateProperties(static_cast(this)); + incrementSkeletonVersion(); + } +} + +//============================================================================== +template +AddonWithProtectedStateAndPropertiesInSkeleton< + BaseT, StateDataT, PropertiesDataT, + ManagerT, updateState, updateProperties, OptionalT>:: +AddonWithProtectedStateAndPropertiesInSkeleton( + common::AddonManager* mgr, + const StateData& state, + const PropertiesData& properties) + : common::Addon(mgr), + mState(state), + mProperties(properties) +{ + DART_COMMON_CAST_NEW_MANAGER_TYPE( + Base, ManagerType, mgr, castedManager, constructor); + mManager = castedManager; +} + +//============================================================================== +template +AddonWithProtectedStateAndPropertiesInSkeleton< + BaseT, StateDataT, PropertiesDataT, + ManagerT, updateState, updateProperties, OptionalT>:: +AddonWithProtectedStateAndPropertiesInSkeleton( + common::AddonManager* mgr, + const PropertiesData& properties, + const StateData& state) + : common::Addon(mgr), + mState(state), + mProperties(properties) +{ + DART_COMMON_CAST_NEW_MANAGER_TYPE( + Base, ManagerType, mgr, castedManager, constructor); + mManager = castedManager; +} + +//============================================================================== +template +std::unique_ptr AddonWithProtectedStateAndPropertiesInSkeleton< + BaseT, StateDataT, PropertiesDataT, + ManagerT, updateState, updateProperties, OptionalT>:: +cloneAddon(common::AddonManager* newManager) const +{ + DART_COMMON_CAST_NEW_MANAGER_TYPE_AND_RETURN_NULL_IF_BAD( + Base, ManagerType, newManager, castedManager, clone); + return std::unique_ptr(new Base(castedManager, mState, mProperties)); +} + +//============================================================================== +template +void AddonWithProtectedStateAndPropertiesInSkeleton< + BaseT, StateDataT, PropertiesDataT, + ManagerT, updateState, updateProperties, OptionalT>:: +setAddonState(const Addon::State& otherState) +{ + setState(static_cast(otherState)); +} + +//============================================================================== +template +const common::Addon::State* AddonWithProtectedStateAndPropertiesInSkeleton< + BaseT, StateDataT, PropertiesDataT, + ManagerT, updateState, updateProperties, OptionalT>:: +getAddonState() const +{ + return &mState; +} + +//============================================================================== +template +void AddonWithProtectedStateAndPropertiesInSkeleton< + BaseT, StateDataT, PropertiesDataT, + ManagerT, updateState, updateProperties, OptionalT>:: +setState(const StateData& state) +{ + static_cast(mState) = state; + UpdateState(static_cast(this)); +} + +//============================================================================== +template +auto AddonWithProtectedStateAndPropertiesInSkeleton< + BaseT, StateDataT, PropertiesDataT, + ManagerT, updateState, updateProperties, OptionalT>:: +getState() const -> const State& +{ + return mState; +} + +//============================================================================== +template +void AddonWithProtectedStateAndPropertiesInSkeleton< + BaseT, StateDataT, PropertiesDataT, + ManagerT, updateState, updateProperties, OptionalT>:: +setAddonProperties(const Addon::Properties& properties) +{ + setProperties(static_cast(properties)); +} + +//============================================================================== +template +const common::Addon::Properties* AddonWithProtectedStateAndPropertiesInSkeleton< + BaseT, StateDataT, PropertiesDataT, + ManagerT, updateState, updateProperties, OptionalT>:: +getAddonProperties() const +{ + return &mProperties; +} + +//============================================================================== +template +void AddonWithProtectedStateAndPropertiesInSkeleton< + BaseT, StateDataT, PropertiesDataT, + ManagerT, updateState, updateProperties, OptionalT>:: +setProperties(const PropertiesData& properties) +{ + static_cast(mProperties) = properties; + + UpdateProperties(static_cast(this)); + incrementSkeletonVersion(); +} + +//============================================================================== +template +auto AddonWithProtectedStateAndPropertiesInSkeleton< + BaseT, StateDataT, PropertiesDataT, + ManagerT, updateState, updateProperties, OptionalT>:: +getProperties() const -> const Properties& +{ + return mProperties; +} + +//============================================================================== +template +bool AddonWithProtectedStateAndPropertiesInSkeleton< + BaseT, StateDataT, PropertiesDataT, + ManagerT, updateState, updateProperties, OptionalT>:: +isOptional(common::AddonManager* oldManager) +{ + if(Optional) + return true; + + // If the Addon is not optional, we should check whether the Manager type is + // the kind that this Addon belongs to. + return (nullptr == dynamic_cast(oldManager)); +} + +//============================================================================== +template +SkeletonPtr AddonWithProtectedStateAndPropertiesInSkeleton< + BaseT, StateDataT, PropertiesDataT, + ManagerT, updateState, updateProperties, OptionalT>:: +getSkeleton() +{ + if(mManager) + return mManager->getSkeleton(); + + return nullptr; +} + +//============================================================================== +template +ConstSkeletonPtr AddonWithProtectedStateAndPropertiesInSkeleton< + BaseT, StateDataT, PropertiesDataT, + ManagerT, updateState, updateProperties, OptionalT>:: +getSkeleton() const +{ + if(mManager) + return mManager->getSkeleton(); + + return nullptr; +} + +//============================================================================== +template +ManagerT* AddonWithProtectedStateAndPropertiesInSkeleton< + BaseT, StateDataT, PropertiesDataT, + ManagerT, updateState, updateProperties, OptionalT>:: +getManager() +{ + return mManager; +} + +//============================================================================== +template +const ManagerT* AddonWithProtectedStateAndPropertiesInSkeleton< + BaseT, StateDataT, PropertiesDataT, + ManagerT, updateState, updateProperties, OptionalT>:: +getManager() const +{ + return mManager; +} + +//============================================================================== +template +void AddonWithProtectedStateAndPropertiesInSkeleton< + BaseT, StateDataT, PropertiesDataT, + ManagerT, updateState, updateProperties, OptionalT>:: +incrementSkeletonVersion() +{ + if(const SkeletonPtr& skel = getSkeleton()) + skel->incrementVersion(); +} + +//============================================================================== +template +void AddonWithProtectedStateAndPropertiesInSkeleton< + BaseT, StateDataT, PropertiesDataT, + ManagerT, updateState, updateProperties, OptionalT>:: +setManager(common::AddonManager* newManager, bool /*transfer*/) +{ + DART_COMMON_CAST_NEW_MANAGER_TYPE( + Base, ManagerType, newManager, castedManager, setManager); + + mManager = castedManager; + + if(mManager) + { + UpdateState(static_cast(this)); + UpdateProperties(static_cast(this)); + incrementSkeletonVersion(); + } +} + +} // namespace dynamics +} // namespace dart + +#endif // DART_DYNAMICS_DETAIL_ADDON_H_ diff --git a/dart/dynamics/detail/BasicNodeManager.h b/dart/dynamics/detail/BasicNodeManager.h new file mode 100644 index 0000000000000..70860d701596d --- /dev/null +++ b/dart/dynamics/detail/BasicNodeManager.h @@ -0,0 +1,363 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_DYNAMICS_DETAIL_BASICNODEMANAGER_H_ +#define DART_DYNAMICS_DETAIL_BASICNODEMANAGER_H_ + +#include +#include +#include + +#include "dart/common/NameManager.h" +#include "dart/common/Empty.h" +#include "dart/dynamics/Node.h" + +namespace dart { +namespace dynamics { +namespace detail { + +class BasicNodeManagerForBodyNode +{ +public: + + using NodeMap = std::map >; + using NodeDestructorSet = std::unordered_set; + using NodeNameMgrMap = std::map< std::type_index, common::NameManager >; + using SpecializedTreeNodes = std::map*>; + + /// Default constructor + BasicNodeManagerForBodyNode() = default; + + /// Delete copy constructors and assignment operators + BasicNodeManagerForBodyNode(const BasicNodeManagerForBodyNode&) = delete; + BasicNodeManagerForBodyNode& operator=(const BasicNodeManagerForBodyNode&) = delete; + + /// Get the number of Nodes corresponding to the specified type + template + size_t getNumNodes() const; + + /// Get the Node of the specified type and the specified index + template + NodeType* getNode(size_t index); + + /// Get the Node of the specified type and the specified index + template + const NodeType* getNode(size_t index) const; + + /// Check if this Manager is specialized for a specific type of Node + template + static constexpr bool isSpecializedForNode(); + +protected: + + template struct type { }; + + /// Map that retrieves the Nodes of a specified type + NodeMap mNodeMap; + + /// A set for storing the Node destructors + NodeDestructorSet mNodeDestructors; + +}; + +class BasicNodeManagerForSkeleton : public virtual BasicNodeManagerForBodyNode +{ +public: + + using BasicNodeManagerForBodyNode::getNumNodes; + using BasicNodeManagerForBodyNode::getNode; + + /// Get the number of Nodes of the specified type that are in the treeIndexth + /// tree of this Skeleton + template + size_t getNumNodes(size_t treeIndex) const; + + /// Get the nodeIndexth Node of the specified type within the tree of + /// treeIndex. + template + NodeType* getNode(size_t treeIndex, size_t nodeIndex); + + /// Get the nodeIndexth Node of the specified type within the tree of + /// treeIndex. + template + const NodeType* getNode(size_t treeIndex, size_t nodeIndex) const; + + /// Get the Node of the specified type with the given name. + template + NodeType* getNode(const std::string& name); + + /// Get the Node of the specified type with the given name. + template + const NodeType* getNode(const std::string& name) const; + +protected: + + /// NameManager for tracking Nodes + NodeNameMgrMap mNodeNameMgrMap; + + /// A NodeMap for each tree to allow tree Nodes to be accessed independently + std::vector mTreeNodeMaps; + + /// A map that allows SpecializedNodeManagers to have a direct iterator to + /// the tree-wise storage of its specialized Node. Each entry in this map + /// contains a pointer to a vector of iterators. Each vector of iterators is + /// stored in its corresponding SpecializedNodeManager. This system allows + /// Node specialization to be extensible, enabling custom derived Skeleton + /// types that are specialized for more than the default specialized Nodes. + SpecializedTreeNodes mSpecializedTreeNodes; + +}; + +//============================================================================== +template +size_t BasicNodeManagerForBodyNode::getNumNodes() const +{ + NodeMap::const_iterator it = mNodeMap.find(typeid(NodeType)); + if(mNodeMap.end() == it) + return 0; + + return it->second.size(); +} + +//============================================================================== +template +NodeType* BasicNodeManagerForBodyNode::getNode(size_t index) +{ + NodeMap::const_iterator it = mNodeMap.find(typeid(NodeType)); + if(mNodeMap.end() == it) + return nullptr; + + return static_cast( + getVectorObjectIfAvailable(index, it->second)); +} + +//============================================================================== +template +const NodeType* BasicNodeManagerForBodyNode::getNode(size_t index) const +{ + return const_cast(this)->getNode(index); +} + +//============================================================================== +template +constexpr bool BasicNodeManagerForBodyNode::isSpecializedForNode() +{ + // When invoked through a BasicNodeManager, this should always return false. + return false; +} + +//============================================================================== +template +size_t BasicNodeManagerForSkeleton::getNumNodes(size_t treeIndex) const +{ + if(treeIndex >= mTreeNodeMaps.size()) + { + dterr << "[Skeleton::getNumNodes<" << typeid(NodeType).name() << ">] " + << "Requested tree index (" << treeIndex << "), but there are only (" + << mTreeNodeMaps.size() << ") trees available\n"; + assert(false); + return 0; + } + + const NodeMap& nodeMap = mTreeNodeMaps[treeIndex]; + NodeMap::const_iterator it = nodeMap.find(typeid(NodeType)); + if(nodeMap.end() == it) + return 0; + + return it->second.size(); +} + +//============================================================================== +template +NodeType* BasicNodeManagerForSkeleton::getNode( + size_t treeIndex, size_t nodeIndex) +{ + if(treeIndex >= mTreeNodeMaps.size()) + { + dterr << "[Skeleton::getNode<" << typeid(NodeType).name() << ">] " + << "Requested tree index (" << treeIndex << "), but there are only (" + << mTreeNodeMaps.size() << ") trees available\n"; + assert(false); + return nullptr; + } + + const NodeMap& nodeMap = mTreeNodeMaps[treeIndex]; + NodeMap::const_iterator it = nodeMap.find(typeid(NodeType)); + if(nodeMap.end() == it) + { + dterr << "[Skeleton::getNode<" << typeid(NodeType).name() << ">] " + << "Requested index (" << nodeIndex << ") within tree (" << treeIndex + << "), but there are no Nodes of the requested type in this tree\n"; + assert(false); + return nullptr; + } + + if(nodeIndex >= it->second.size()) + { + dterr << "[Skeleton::getNode<" << typeid(NodeType).name() << ">] " + << "Requested index (" << nodeIndex << ") within tree (" << treeIndex + << "), but there are only (" << it->second.size() << ") Nodes of the " + << "requested type within that tree\n"; + assert(false); + return nullptr; + } + + return static_cast(it->second[nodeIndex]); +} + +//============================================================================== +template +const NodeType* BasicNodeManagerForSkeleton::getNode( + size_t treeIndex, size_t nodeIndex) const +{ + return const_cast(this)->getNode( + treeIndex, nodeIndex); +} + +//============================================================================== +template +NodeType* BasicNodeManagerForSkeleton::getNode(const std::string& name) +{ + NodeNameMgrMap::const_iterator it = mNodeNameMgrMap.find(typeid(NodeType)); + + if(mNodeNameMgrMap.end() == it) + return nullptr; + + return static_cast(it->second.getObject(name)); +} + +//============================================================================== +template +const NodeType* BasicNodeManagerForSkeleton::getNode( + const std::string& name) const +{ + return const_cast( + this)->getNode(name); +} + +//============================================================================== +#define DART_BAKE_SPECIALIZED_NODE_IRREGULAR( TypeName, AddonName, PluralAddonName )\ + inline size_t getNum ## PluralAddonName () const\ + { return getNumNodes< TypeName >(); }\ + inline TypeName * get ## AddonName (size_t index)\ + { return getNode< TypeName >(index); }\ + inline const TypeName * get ## AddonName (size_t index) const\ + { return getNode< TypeName >(index); } + +//============================================================================== +#define DART_BAKE_SPECIALIZED_NODE( AddonName )\ + DART_BAKE_SPECIALIZED_NODE_IRREGULAR( AddonName, AddonName, AddonName ## s ) + +//============================================================================== +#define DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR( TypeName, AddonName, PluralAddonName )\ + DART_BAKE_SPECIALIZED_NODE_IRREGULAR( TypeName, AddonName, PluralAddonName )\ + inline size_t getNum ## PluralAddoName (size_t treeIndex) const\ + { return getNumNodes< TypeName >(treeIndex); }\ + inline TypeName * get ## AddonName (size_t treeIndex, size_t nodeIndex)\ + { return getNode< TypeName >(treeIndex, nodeIndex); }\ + inline const TypeName * get ## AddonName (size_t treeIndex, size_t nodeIndex) const\ + { return getNode< TypeName >(treeIndex, nodeIndex); }\ + \ + inline TypeName * get ## AddonName (const std::string& name)\ + { return getNode< TypeName >(name); }\ + inline const TypeName * get ## AddonName (const std::string& name) const\ + { return getNode< TypeName >(name); } + +//============================================================================== +#define DART_BAKE_SPECIALIZED_NODE_SKEL( AddonName )\ + DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR( AddonName, AddonName, AddonName ## s) + +//============================================================================== +#define DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DECLARATIONS( TypeName, AddonName, PluralAddonName )\ + size_t getNum ## PluralAddonName () const;\ + TypeName * get ## AddonName (size_t index);\ + const TypeName * get ## AddonName (size_t index) const; + +//============================================================================== +#define DART_BAKE_SPECIALIZED_NODE_DECLARATIONS( AddonName )\ + DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DECLARATIONS( AddonName, AddonName, AddonName ## s ) + +//============================================================================== +#define DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR_DECLARATIONS( TypeName, AddonName, PluralAddonName )\ + DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DECLARATIONS( TypeName, AddonName, PluralAddonName )\ + size_t getNum ## PluralAddoName (size_t treeIndex) const;\ + TypeName * get ## AddonName (size_t treeIndex, size_t nodeIndex);\ + const TypeName * get ## AddonName (size_t treeIndex, size_t nodeIndex) const;\ + \ + TypeName * get ## AddonName (const std::string& name);\ + const TypeName * get ## AddonName (const std::string& name) const; + +//============================================================================== +#define DART_BAKE_SPECIALIZED_NODE_SKEL_DECLARATIONS( AddonName )\ + DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR_DECLARATIONS( AddonName, AddonName, AddonName ## s ) + +//============================================================================== +#define DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DEFINITIONS( ClassName, TypeName, AddonName, PluralAddonName )\ + size_t ClassName :: getNum ## PluralAddonName () const\ + { return getNumNodes< TypeName >(); }\ + TypeName * ClassName :: get ## AddonName (size_t index)\ + { return getNode< TypeName >(index); }\ + const TypeName * ClassName :: get ## AddonName (size_t index) const\ + { return getNode< TypeName >(index); } + +//============================================================================== +#define DART_BAKE_SPECIALIZED_NODE_DEFINITIONS( ClassName, AddonName )\ + DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DEFINITIONS( ClassName, AddonName, AddonName, AddonName ## s ) + +//============================================================================== +#define DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR_DEFINITIONS( ClassName, TypeName, AddonName, PluralAddonName )\ + DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DEFINITIONS( ClassName, TypeName, AddonName, PluralAddonName )\ + size_t ClassName :: getNum ## PluralAddoName (size_t treeIndex) const\ + { return getNumNodes< TypeName >(treeIndex); }\ + TypeName * ClassName :: get ## AddonName (size_t treeIndex, size_t nodeIndex)\ + { return getNode< TypeName >(treeIndex, nodeIndex); }\ + const TypeName * ClassName :: get ## AddonName (size_t treeIndex, size_t nodeIndex) const\ + { return getNode< TypeName >(treeIndex, nodeIndex); }\ + \ + TypeName * ClassName :: get ## AddonName (const std::string& name)\ + { return getNode< TypeName >(name); }\ + const TypeName * ClassName :: get ## AddonName (const std::string& name) const\ + { return getNode< TypeName >(name); } + +//============================================================================== +#define DART_BAKE_SPECIALIZED_NODE_SKEL_DEFINITIONS( ClassName, AddonName )\ + DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR_DEFINITIONS( ClassName, AddonName, AddonName, AddonName ## s ) + +} // namespace detail +} // namespace dynamics +} // namespace dart + +#endif // DART_DYNAMICS_DETAIL_BASICNODEMANAGER_H_ diff --git a/dart/dynamics/detail/BodyNode.h b/dart/dynamics/detail/BodyNode.h index 30b187601e2f3..9b38a1d7fba18 100644 --- a/dart/dynamics/detail/BodyNode.h +++ b/dart/dynamics/detail/BodyNode.h @@ -37,6 +37,13 @@ #ifndef DART_DYNAMICS_DETAIL_BODYNODE_H_ #define DART_DYNAMICS_DETAIL_BODYNODE_H_ +#include + +#include "dart/dynamics/Skeleton.h" + +namespace dart { +namespace dynamics { + //============================================================================== template JointType* BodyNode::moveTo(BodyNode* _newParent, @@ -126,4 +133,55 @@ std::pair BodyNode::createChildJointAndBodyNodePair( this, _jointProperties, _bodyProperties); } +////============================================================================== +//template +//size_t BodyNode::getNumNodes() const +//{ +// NodeMap::const_iterator it = mNodeMap.find(typeid(NodeType)); +// if(mNodeMap.end() == it) +// return 0; + +// return it->second.size(); +//} + +////============================================================================== +//template +//NodeType* BodyNode::getNode(size_t index) +//{ +// NodeMap::const_iterator it = mNodeMap.find(typeid(NodeType)); +// if(mNodeMap.end() == it) +// return nullptr; + +// return static_cast( +// getVectorObjectIfAvailable(index, it->second)); +//} + +////============================================================================== +//template +//const NodeType* BodyNode::getNode(size_t index) const +//{ +// return const_cast(this)->getNode(index); +//} + +//============================================================================== +template +NodeType* BodyNode::createNode(Args&&... args) +{ + NodeType* node = new NodeType(this, std::forward(args)...); + node->attach(); + + return node; +} + +//============================================================================== +template +EndEffector* BodyNode::createEndEffector( + const EndEffectorProperties& _properties) +{ + return createNode(_properties); +} + +} // namespace dynamics +} // namespace dart + #endif // DART_DYNAMICS_DETAIL_BODYNODE_H_ diff --git a/dart/dynamics/detail/EulerJointProperties.cpp b/dart/dynamics/detail/EulerJointProperties.cpp new file mode 100644 index 0000000000000..81fc06e332a55 --- /dev/null +++ b/dart/dynamics/detail/EulerJointProperties.cpp @@ -0,0 +1,63 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/dynamics/EulerJoint.h" + +namespace dart { +namespace dynamics { +namespace detail { + +//============================================================================== +EulerJointUniqueProperties::EulerJointUniqueProperties(AxisOrder _axisOrder) + : mAxisOrder(_axisOrder) +{ + // Do nothing +} + +//============================================================================== +EulerJointProperties::EulerJointProperties( + const MultiDofJoint<3>::Properties& _multiDofProperties, + const EulerJointUniqueProperties& _eulerJointProperties) + : MultiDofJoint<3>::Properties(_multiDofProperties), + EulerJointUniqueProperties(_eulerJointProperties) +{ + // Do nothing +} + +} // namespace detail +} // namespace dynamics +} // namespace dart + diff --git a/dart/dynamics/detail/EulerJointProperties.h b/dart/dynamics/detail/EulerJointProperties.h new file mode 100644 index 0000000000000..f8f5dd687af17 --- /dev/null +++ b/dart/dynamics/detail/EulerJointProperties.h @@ -0,0 +1,107 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_DYNAMICS_DETAIL_EULERJOINTPROPERTIES_H_ +#define DART_DYNAMICS_DETAIL_EULERJOINTPROPERTIES_H_ + +#include + +#include "dart/dynamics/MultiDofJoint.h" +#include "dart/dynamics/Addon.h" + +namespace dart { +namespace dynamics { + +class EulerJoint; + +namespace detail { + +//============================================================================== +/// Axis order +enum class AxisOrder : int +{ + ZYX = 0, + XYZ = 1 +}; + +//============================================================================== +struct EulerJointUniqueProperties +{ + /// Euler angle order + AxisOrder mAxisOrder; + + /// Constructor + EulerJointUniqueProperties(AxisOrder _axisOrder = AxisOrder::XYZ); + + virtual ~EulerJointUniqueProperties() = default; +}; + +//============================================================================== +struct EulerJointProperties : + MultiDofJoint<3>::Properties, + EulerJointUniqueProperties +{ + /// Composed constructor + EulerJointProperties( + const MultiDofJoint<3>::Properties& _multiDofProperties = + MultiDofJoint<3>::Properties(), + const EulerJointUniqueProperties& _eulerJointProperties = + EulerJointUniqueProperties()); + + virtual ~EulerJointProperties() = default; +}; + +//============================================================================== +class EulerJointAddon final : + public AddonWithProtectedPropertiesInSkeleton< + EulerJointAddon, EulerJointUniqueProperties, EulerJoint, + detail::JointPropertyUpdate, false > +{ +public: + DART_DYNAMICS_JOINT_ADDON_CONSTRUCTOR( EulerJointAddon ) + DART_DYNAMICS_SET_GET_ADDON_PROPERTY( AxisOrder, AxisOrder ) +}; + +//============================================================================== +using EulerJointBase = common::AddonManagerJoiner< + MultiDofJoint<3>, common::SpecializedAddonManager >; + +} // namespace detail +} // namespace dynamics +} // namespace dart + + +#endif // DART_DYNAMICS_DETAIL_EULERJOINTPROPERTIES_H_ diff --git a/dart/dynamics/detail/InverseKinematics.h b/dart/dynamics/detail/InverseKinematics.h index 273b19c3d31b6..ea72fc28fad21 100644 --- a/dart/dynamics/detail/InverseKinematics.h +++ b/dart/dynamics/detail/InverseKinematics.h @@ -39,6 +39,8 @@ #include +#include "dart/dynamics/InverseKinematics.h" + namespace dart { namespace dynamics { diff --git a/dart/dynamics/detail/MultiDofJoint.h b/dart/dynamics/detail/MultiDofJoint.h index 740a79695cd73..07c393e692bf5 100644 --- a/dart/dynamics/detail/MultiDofJoint.h +++ b/dart/dynamics/detail/MultiDofJoint.h @@ -37,6 +37,8 @@ #ifndef DART_DYNAMICS_DETAIL_MULTIDOFJOINT_H_ #define DART_DYNAMICS_DETAIL_MULTIDOFJOINT_H_ +#include "dart/dynamics/MultiDofJoint.h" + #define MULTIDOFJOINT_REPORT_DIM_MISMATCH( func, arg ) \ dterr << "[MultiDofJoint::" #func "] Mismatch beteween size of " \ << #arg " [" << arg .size() << "] and the number of " \ @@ -55,86 +57,8 @@ << mJointP.mActuatorType << ") for Joint [" << getName() << "].\n"; \ assert(false); -//============================================================================== -template -MultiDofJoint::UniqueProperties::UniqueProperties( - const Vector& _positionLowerLimits, - const Vector& _positionUpperLimits, - const Vector& _velocityLowerLimits, - const Vector& _velocityUpperLimits, - const Vector& _accelerationLowerLimits, - const Vector& _accelerationUpperLimits, - const Vector& _forceLowerLimits, - const Vector& _forceUpperLimits, - const Vector& _springStiffness, - const Vector& _restPosition, - const Vector& _dampingCoefficient, - const Vector& _coulombFrictions) - : mPositionLowerLimits(_positionLowerLimits), - mPositionUpperLimits(_positionUpperLimits), - mInitialPositions(Vector::Zero()), - mVelocityLowerLimits(_velocityLowerLimits), - mVelocityUpperLimits(_velocityUpperLimits), - mInitialVelocities(Vector::Zero()), - mAccelerationLowerLimits(_accelerationLowerLimits), - mAccelerationUpperLimits(_accelerationUpperLimits), - mForceLowerLimits(_forceLowerLimits), - mForceUpperLimits(_forceUpperLimits), - mSpringStiffnesses(_springStiffness), - mRestPositions(_restPosition), - mDampingCoefficients(_dampingCoefficient), - mFrictions(_coulombFrictions) -{ - for (size_t i = 0; i < DOF; ++i) - { - mPreserveDofNames[i] = false; - mDofNames[i] = std::string(); - } -} - -//============================================================================== -template -MultiDofJoint::UniqueProperties::UniqueProperties( - const UniqueProperties& _other) - : mPositionLowerLimits(_other.mPositionLowerLimits), - mPositionUpperLimits(_other.mPositionUpperLimits), - mInitialPositions(_other.mInitialPositions), - mVelocityLowerLimits(_other.mVelocityLowerLimits), - mVelocityUpperLimits(_other.mVelocityUpperLimits), - mInitialVelocities(_other.mInitialVelocities), - mAccelerationLowerLimits(_other.mAccelerationLowerLimits), - mAccelerationUpperLimits(_other.mAccelerationUpperLimits), - mForceLowerLimits(_other.mForceLowerLimits), - mForceUpperLimits(_other.mForceUpperLimits), - mSpringStiffnesses(_other.mSpringStiffnesses), - mRestPositions(_other.mRestPositions), - mDampingCoefficients(_other.mDampingCoefficients), - mFrictions(_other.mFrictions) -{ - for (size_t i = 0; i < DOF; ++i) - { - mPreserveDofNames[i] = _other.mPreserveDofNames[i]; - mDofNames[i] = _other.mDofNames[i]; - } -} - -//============================================================================== -template -MultiDofJoint::Properties::Properties( - const Joint::Properties& _jointProperties, - const UniqueProperties& _multiDofProperties) - : Joint::Properties(_jointProperties), - UniqueProperties(_multiDofProperties) -{ - // Do nothing -} - -//============================================================================== -template -MultiDofJoint::Properties::~Properties() -{ - // Do nothing -} +namespace dart { +namespace dynamics { //============================================================================== template @@ -181,7 +105,8 @@ template typename MultiDofJoint::Properties MultiDofJoint::getMultiDofJointProperties() const { - return MultiDofJoint::Properties(mJointP, mMultiDofP); + return MultiDofJoint::Properties( + mJointP, getMultiDofJointAddon()->getProperties()); } //============================================================================== @@ -253,7 +178,7 @@ const std::string& MultiDofJoint::setDofName(size_t _index, } preserveDofName(_index, _preserveName); - std::string& dofName = mMultiDofP.mDofNames[_index]; + std::string& dofName = getMultiDofJointAddon()->_getDofNameReference(_index); if(_name == dofName) return dofName; @@ -280,7 +205,7 @@ void MultiDofJoint::preserveDofName(size_t _index, bool _preserve) return; } - mMultiDofP.mPreserveDofNames[_index] = _preserve; + getMultiDofJointAddon()->setPreserveDofName(_index, _preserve); } //============================================================================== @@ -293,7 +218,7 @@ bool MultiDofJoint::isDofNamePreserved(size_t _index) const _index = 0; } - return mMultiDofP.mPreserveDofNames[_index]; + return getMultiDofJointAddon()->getPreserveDofName(_index); } //============================================================================== @@ -306,10 +231,10 @@ const std::string& MultiDofJoint::getDofName(size_t _index) const << _index << "] in Joint [" << getName() << "], but that is out of " << "bounds (max " << DOF-1 << "). Returning name of DOF 0.\n"; assert(false); - return mMultiDofP.mDofNames[0]; + return getMultiDofJointAddon()->getDofName(0); } - return mMultiDofP.mDofNames[_index]; + return getMultiDofJointAddon()->getDofName(_index); } //============================================================================== @@ -358,8 +283,8 @@ void MultiDofJoint::setCommand(size_t _index, double _command) { case FORCE: mCommands[_index] = math::clip(_command, - mMultiDofP.mForceLowerLimits[_index], - mMultiDofP.mForceUpperLimits[_index]); + getMultiDofJointAddon()->getForceLowerLimit(_index), + getMultiDofJointAddon()->getForceUpperLimit(_index)); break; case PASSIVE: if(0.0 != _command) @@ -372,18 +297,18 @@ void MultiDofJoint::setCommand(size_t _index, double _command) break; case SERVO: mCommands[_index] = math::clip(_command, - mMultiDofP.mVelocityLowerLimits[_index], - mMultiDofP.mVelocityUpperLimits[_index]); + getMultiDofJointAddon()->getVelocityLowerLimit(_index), + getMultiDofJointAddon()->getVelocityUpperLimit(_index)); break; case ACCELERATION: mCommands[_index] = math::clip(_command, - mMultiDofP.mAccelerationLowerLimits[_index], - mMultiDofP.mAccelerationUpperLimits[_index]); + getMultiDofJointAddon()->getAccelerationLowerLimit(_index), + getMultiDofJointAddon()->getAccelerationUpperLimit(_index)); break; case VELOCITY: mCommands[_index] = math::clip(_command, - mMultiDofP.mVelocityLowerLimits[_index], - mMultiDofP.mVelocityUpperLimits[_index]); + getMultiDofJointAddon()->getVelocityLowerLimit(_index), + getMultiDofJointAddon()->getVelocityUpperLimit(_index)); // TODO: This possibly makes the acceleration to exceed the limits. break; case LOCKED: @@ -428,8 +353,8 @@ void MultiDofJoint::setCommands(const Eigen::VectorXd& _commands) { case FORCE: mCommands = math::clip(_commands, - mMultiDofP.mForceLowerLimits, - mMultiDofP.mForceUpperLimits); + getMultiDofJointAddon()->getForceLowerLimits(), + getMultiDofJointAddon()->getForceUpperLimits()); break; case PASSIVE: if(Vector::Zero() != _commands) @@ -442,18 +367,18 @@ void MultiDofJoint::setCommands(const Eigen::VectorXd& _commands) break; case SERVO: mCommands = math::clip(_commands, - mMultiDofP.mVelocityLowerLimits, - mMultiDofP.mVelocityUpperLimits); + getMultiDofJointAddon()->getVelocityLowerLimits(), + getMultiDofJointAddon()->getVelocityUpperLimits()); break; case ACCELERATION: mCommands = math::clip(_commands, - mMultiDofP.mAccelerationLowerLimits, - mMultiDofP.mAccelerationUpperLimits); + getMultiDofJointAddon()->getAccelerationLowerLimits(), + getMultiDofJointAddon()->getAccelerationUpperLimits()); break; case VELOCITY: mCommands = math::clip(_commands, - mMultiDofP.mVelocityLowerLimits, - mMultiDofP.mVelocityUpperLimits); + getMultiDofJointAddon()->getVelocityLowerLimits(), + getMultiDofJointAddon()->getVelocityUpperLimits()); // TODO: This possibly makes the acceleration to exceed the limits. break; case LOCKED: @@ -546,7 +471,7 @@ void MultiDofJoint::setPositionLowerLimit(size_t _index, double _position) return; } - mMultiDofP.mPositionLowerLimits[_index] = _position; + getMultiDofJointAddon()->setPositionLowerLimit(_index, _position); } //============================================================================== @@ -559,7 +484,7 @@ double MultiDofJoint::getPositionLowerLimit(size_t _index) const return 0.0; } - return mMultiDofP.mPositionLowerLimits[_index]; + return getMultiDofJointAddon()->getPositionLowerLimit(_index); } //============================================================================== @@ -572,7 +497,7 @@ void MultiDofJoint::setPositionUpperLimit(size_t _index, double _position) return; } - mMultiDofP.mPositionUpperLimits[_index] = _position; + getMultiDofJointAddon()->setPositionUpperLimit(_index, _position); } //============================================================================== @@ -585,14 +510,14 @@ void MultiDofJoint::resetPosition(size_t _index) return; } - setPosition(_index, mMultiDofP.mInitialPositions[_index]); + setPosition(_index, getMultiDofJointAddon()->getInitialPosition(_index)); } //============================================================================== template void MultiDofJoint::resetPositions() { - setPositionsStatic(mMultiDofP.mInitialPositions); + setPositionsStatic(getMultiDofJointAddon()->getInitialPositions()); } //============================================================================== @@ -605,7 +530,7 @@ void MultiDofJoint::setInitialPosition(size_t _index, double _initial) return; } - mMultiDofP.mInitialPositions[_index] = _initial; + getMultiDofJointAddon()->setInitialPosition(_index, _initial); } //============================================================================== @@ -618,7 +543,7 @@ double MultiDofJoint::getInitialPosition(size_t _index) const return 0.0; } - return mMultiDofP.mInitialPositions[_index]; + return getMultiDofJointAddon()->getInitialPosition(_index); } //============================================================================== @@ -631,14 +556,14 @@ void MultiDofJoint::setInitialPositions(const Eigen::VectorXd& _initial) return; } - mMultiDofP.mInitialPositions = _initial; + getMultiDofJointAddon()->setInitialPositions(_initial); } //============================================================================== template Eigen::VectorXd MultiDofJoint::getInitialPositions() const { - return mMultiDofP.mInitialPositions; + return getMultiDofJointAddon()->getInitialPositions(); } //============================================================================== @@ -651,7 +576,7 @@ double MultiDofJoint::getPositionUpperLimit(size_t _index) const return 0.0; } - return mMultiDofP.mPositionUpperLimits[_index]; + return getMultiDofJointAddon()->getPositionUpperLimit(_index); } //============================================================================== @@ -664,8 +589,8 @@ bool MultiDofJoint::hasPositionLimit(size_t _index) const return true; } - return std::isfinite(mMultiDofP.mPositionUpperLimits[_index]) - || std::isfinite(mMultiDofP.mPositionLowerLimits[_index]); + return std::isfinite(getMultiDofJointAddon()->getPositionUpperLimit(_index)) + || std::isfinite(getMultiDofJointAddon()->getPositionLowerLimit(_index)); } //============================================================================== @@ -741,7 +666,7 @@ void MultiDofJoint::setVelocityLowerLimit(size_t _index, double _velocity) return; } - mMultiDofP.mVelocityLowerLimits[_index] = _velocity; + getMultiDofJointAddon()->setVelocityLowerLimit(_index, _velocity); } //============================================================================== @@ -754,7 +679,7 @@ double MultiDofJoint::getVelocityLowerLimit(size_t _index) const return 0.0; } - return mMultiDofP.mVelocityLowerLimits[_index]; + return getMultiDofJointAddon()->getVelocityLowerLimit(_index); } //============================================================================== @@ -767,7 +692,7 @@ void MultiDofJoint::setVelocityUpperLimit(size_t _index, double _velocity) return; } - mMultiDofP.mVelocityUpperLimits[_index] = _velocity; + getMultiDofJointAddon()->setVelocityUpperLimit(_index, _velocity); } //============================================================================== @@ -780,7 +705,7 @@ double MultiDofJoint::getVelocityUpperLimit(size_t _index) const return 0.0; } - return mMultiDofP.mVelocityUpperLimits[_index]; + return getMultiDofJointAddon()->getVelocityUpperLimit(_index); } //============================================================================== @@ -793,14 +718,14 @@ void MultiDofJoint::resetVelocity(size_t _index) return; } - setVelocity(_index, mMultiDofP.mInitialVelocities[_index]); + setVelocity(_index, getMultiDofJointAddon()->getInitialVelocity(_index)); } //============================================================================== template void MultiDofJoint::resetVelocities() { - setVelocitiesStatic(mMultiDofP.mInitialVelocities); + setVelocitiesStatic(getMultiDofJointAddon()->getInitialVelocities()); } //============================================================================== @@ -813,7 +738,7 @@ void MultiDofJoint::setInitialVelocity(size_t _index, double _initial) return; } - mMultiDofP.mInitialVelocities[_index] = _initial; + getMultiDofJointAddon()->setInitialVelocity(_index, _initial); } //============================================================================== @@ -826,7 +751,7 @@ double MultiDofJoint::getInitialVelocity(size_t _index) const return 0.0; } - return mMultiDofP.mInitialVelocities[_index]; + return getMultiDofJointAddon()->getInitialVelocity(_index); } //============================================================================== @@ -839,14 +764,14 @@ void MultiDofJoint::setInitialVelocities(const Eigen::VectorXd& _initial) return; } - mMultiDofP.mInitialVelocities = _initial; + getMultiDofJointAddon()->setInitialVelocities(_initial); } //============================================================================== template Eigen::VectorXd MultiDofJoint::getInitialVelocities() const { - return mMultiDofP.mInitialVelocities; + return getMultiDofJointAddon()->getInitialVelocities(); } //============================================================================== @@ -930,7 +855,7 @@ void MultiDofJoint::setAccelerationLowerLimit(size_t _index, return; } - mMultiDofP.mAccelerationLowerLimits[_index] = _acceleration; + getMultiDofJointAddon()->setAccelerationLowerLimit(_index, _acceleration); } //============================================================================== @@ -943,7 +868,7 @@ double MultiDofJoint::getAccelerationLowerLimit(size_t _index) const return 0.0; } - return mMultiDofP.mAccelerationLowerLimits[_index]; + return getMultiDofJointAddon()->getAccelerationLowerLimit(_index); } //============================================================================== @@ -957,7 +882,7 @@ void MultiDofJoint::setAccelerationUpperLimit(size_t _index, return; } - mMultiDofP.mAccelerationUpperLimits[_index] = _acceleration; + getMultiDofJointAddon()->setAccelerationUpperLimit(_index, _acceleration); } //============================================================================== @@ -970,7 +895,7 @@ double MultiDofJoint::getAccelerationUpperLimit(size_t _index) const return 0.0; } - return mMultiDofP.mAccelerationUpperLimits[_index]; + return getMultiDofJointAddon()->getAccelerationUpperLimit(_index); } //============================================================================== @@ -1111,7 +1036,7 @@ void MultiDofJoint::setForceLowerLimit(size_t _index, double _force) return; } - mMultiDofP.mForceLowerLimits[_index] = _force; + getMultiDofJointAddon()->setForceLowerLimit(_index, _force); } //============================================================================== @@ -1124,7 +1049,7 @@ double MultiDofJoint::getForceLowerLimit(size_t _index) const return 0.0; } - return mMultiDofP.mForceLowerLimits[_index]; + return getMultiDofJointAddon()->getForceLowerLimit(_index); } //============================================================================== @@ -1137,7 +1062,7 @@ void MultiDofJoint::setForceUpperLimit(size_t _index, double _force) return; } - mMultiDofP.mForceUpperLimits[_index] = _force; + getMultiDofJointAddon()->setForceUpperLimit(_index, _force); } //============================================================================== @@ -1150,7 +1075,7 @@ double MultiDofJoint::getForceUpperLimit(size_t _index) const return 0.0; } - return mMultiDofP.mForceUpperLimits[_index]; + return getMultiDofJointAddon()->getForceUpperLimit(_index); } //============================================================================== @@ -1272,7 +1197,7 @@ void MultiDofJoint::setSpringStiffness(size_t _index, double _k) assert(_k >= 0.0); - mMultiDofP.mSpringStiffnesses[_index] = _k; + getMultiDofJointAddon()->setSpringStiffness(_index, _k); } //============================================================================== @@ -1285,7 +1210,7 @@ double MultiDofJoint::getSpringStiffness(size_t _index) const return 0.0; } - return mMultiDofP.mSpringStiffnesses[_index]; + return getMultiDofJointAddon()->getSpringStiffness(_index); } //============================================================================== @@ -1298,18 +1223,19 @@ void MultiDofJoint::setRestPosition(size_t _index, double _q0) return; } - if (mMultiDofP.mPositionLowerLimits[_index] > _q0 - || mMultiDofP.mPositionUpperLimits[_index] < _q0) + if (getMultiDofJointAddon()->getPositionLowerLimit(_index) > _q0 + || getMultiDofJointAddon()->getPositionUpperLimit(_index) < _q0) { dtwarn << "[MultiDofJoint::setRestPosition] Value of _q0 [" << _q0 << "], is out of the limit range [" - << mMultiDofP.mPositionLowerLimits[_index] << ", " - << mMultiDofP.mPositionUpperLimits[_index] << "] for index [" - << _index << "] of Joint [" << getName() << "].\n"; + << getMultiDofJointAddon()->getPositionLowerLimit(_index) << ", " + << getMultiDofJointAddon()->getPositionUpperLimit(_index) + << "] for index [" << _index << "] of Joint [" << getName() + << "].\n"; return; } - mMultiDofP.mRestPositions[_index] = _q0; + getMultiDofJointAddon()->setRestPosition(_index, _q0); } //============================================================================== @@ -1322,7 +1248,7 @@ double MultiDofJoint::getRestPosition(size_t _index) const return 0.0; } - return mMultiDofP.mRestPositions[_index]; + return getMultiDofJointAddon()->getRestPosition(_index); } //============================================================================== @@ -1337,8 +1263,7 @@ void MultiDofJoint::setDampingCoefficient(size_t _index, double _d) assert(_d >= 0.0); - mMultiDofP.mDampingCoefficients[_index] = _d; - + getMultiDofJointAddon()->setDampingCoefficient(_index, _d); } //============================================================================== @@ -1351,7 +1276,7 @@ double MultiDofJoint::getDampingCoefficient(size_t _index) const return 0.0; } - return mMultiDofP.mDampingCoefficients[_index]; + return getMultiDofJointAddon()->getDampingCoefficient(_index); } //============================================================================== @@ -1366,7 +1291,7 @@ void MultiDofJoint::setCoulombFriction(size_t _index, double _friction) assert(_friction >= 0.0); - mMultiDofP.mFrictions[_index] = _friction; + getMultiDofJointAddon()->setFriction(_index, _friction); } //============================================================================== @@ -1379,7 +1304,7 @@ double MultiDofJoint::getCoulombFriction(size_t _index) const return 0.0; } - return mMultiDofP.mFrictions[_index]; + return getMultiDofJointAddon()->getFriction(_index); } //============================================================================== @@ -1387,9 +1312,11 @@ template double MultiDofJoint::getPotentialEnergy() const { // Spring energy - Eigen::VectorXd displacement = getPositionsStatic() - mMultiDofP.mRestPositions; - double pe = 0.5 * displacement.dot(mMultiDofP.mSpringStiffnesses.asDiagonal() - * displacement); + Eigen::VectorXd displacement = + getPositionsStatic() - getMultiDofJointAddon()->getRestPositions(); + double pe = 0.5 * displacement.dot( + getMultiDofJointAddon()->getSpringStiffnesses().asDiagonal() + * displacement); return pe; } @@ -1406,7 +1333,6 @@ Eigen::Vector6d MultiDofJoint::getBodyConstraintWrench() const template MultiDofJoint::MultiDofJoint(const Properties& _properties) : Joint(_properties), - mMultiDofP(_properties), mCommands(Vector::Zero()), mPositions(Vector::Zero()), mPositionDeriv(Vector::Zero()), @@ -1426,6 +1352,8 @@ MultiDofJoint::MultiDofJoint(const Properties& _properties) mTotalForce(Vector::Zero()), mTotalImpulse(Vector::Zero()) { + createMultiDofJointAddon(_properties); + for (size_t i = 0; i < DOF; ++i) mDofs[i] = createDofPointer(i); } @@ -1437,7 +1365,7 @@ void MultiDofJoint::registerDofs() const SkeletonPtr& skel = mChildBodyNode->getSkeleton(); for (size_t i = 0; i < DOF; ++i) { - mMultiDofP.mDofNames[i] = + getMultiDofJointAddon()->mProperties.mDofNames[i] = skel->mNameMgrForDofs.issueNewNameAndAdd(mDofs[i]->getName(), mDofs[i]); } } @@ -1781,8 +1709,8 @@ void MultiDofJoint::updateInvProjArtInertiaImplicitDynamic( // Add additional inertia for implicit damping and spring force for (size_t i = 0; i < DOF; ++i) { - projAI(i, i) += _timeStep * mMultiDofP.mDampingCoefficients[i] - + _timeStep * _timeStep * mMultiDofP.mSpringStiffnesses[i]; + projAI(i, i) += _timeStep * getMultiDofJointAddon()->getDampingCoefficient(i) + + _timeStep * _timeStep * getMultiDofJointAddon()->getSpringStiffness(i); } // Inversion of projected articulated inertia @@ -1999,13 +1927,14 @@ void MultiDofJoint::updateTotalForceDynamic( { // Spring force const Eigen::Matrix springForce - = (-mMultiDofP.mSpringStiffnesses).asDiagonal() - *(getPositionsStatic() - mMultiDofP.mRestPositions + = (-getMultiDofJointAddon()->getSpringStiffnesses()).asDiagonal() + *(getPositionsStatic() - getMultiDofJointAddon()->getRestPositions() + getVelocitiesStatic()*_timeStep); // Damping force const Eigen::Matrix dampingForce - = (-mMultiDofP.mDampingCoefficients).asDiagonal()*getVelocitiesStatic(); + = (-getMultiDofJointAddon()->getDampingCoefficients()).asDiagonal()* + getVelocitiesStatic(); // mTotalForce = mForces + springForce + dampingForce; @@ -2179,7 +2108,8 @@ void MultiDofJoint::updateForceID(const Eigen::Vector6d& _bodyForce, if (_withDampingForces) { const Eigen::Matrix dampingForces - = (-mMultiDofP.mDampingCoefficients).asDiagonal()*getVelocitiesStatic(); + = (-getMultiDofJointAddon()->getDampingCoefficients()).asDiagonal() + *getVelocitiesStatic(); mForces -= dampingForces; } @@ -2187,8 +2117,8 @@ void MultiDofJoint::updateForceID(const Eigen::Vector6d& _bodyForce, if (_withSpringForces) { const Eigen::Matrix springForces - = (-mMultiDofP.mSpringStiffnesses).asDiagonal() - *(getPositionsStatic() - mMultiDofP.mRestPositions + = (-getMultiDofJointAddon()->getSpringStiffnesses()).asDiagonal() + *(getPositionsStatic() - getMultiDofJointAddon()->getRestPositions() + getVelocitiesStatic()*_timeStep); mForces -= springForces; } @@ -2404,4 +2334,7 @@ Eigen::VectorXd MultiDofJoint::getSpatialToGeneralized( return getLocalJacobianStatic().transpose() * _spatial; } +} // namespace dynamics +} // namespace dart + #endif // DART_DYNAMICS_DETAIL_MULTIDOFJOINT_H_ diff --git a/dart/dynamics/detail/MultiDofJointProperties.h b/dart/dynamics/detail/MultiDofJointProperties.h new file mode 100644 index 0000000000000..8f19724cb77fd --- /dev/null +++ b/dart/dynamics/detail/MultiDofJointProperties.h @@ -0,0 +1,316 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_DYNAMICS_DETAIL_MULTIDOFJOINTPROPERTIES_H_ +#define DART_DYNAMICS_DETAIL_MULTIDOFJOINTPROPERTIES_H_ + +#include "dart/dynamics/Joint.h" +#include "dart/dynamics/Addon.h" + +namespace dart { +namespace dynamics { + +// Forward declare the MultiDofJoint class +template class MultiDofJoint; + +namespace detail { + +template +struct MultiDofJointUniqueProperties +{ + constexpr static size_t NumDofs = DOF; + using Vector = Eigen::Matrix; + using BoolArray = std::array; + using StringArray = std::array; + + /// Lower limit of position + Vector mPositionLowerLimits; + + /// Upper limit of position + Vector mPositionUpperLimits; + + /// Initial positions + Vector mInitialPositions; + + /// Min value allowed. + Vector mVelocityLowerLimits; + + /// Max value allowed. + Vector mVelocityUpperLimits; + + /// Initial velocities + Vector mInitialVelocities; + + /// Min value allowed. + Vector mAccelerationLowerLimits; + + /// upper limit of generalized acceleration + Vector mAccelerationUpperLimits; + + /// Min value allowed. + Vector mForceLowerLimits; + + /// Max value allowed. + Vector mForceUpperLimits; + + /// Joint spring stiffness + Vector mSpringStiffnesses; + + /// Rest joint position for joint spring + Vector mRestPositions; + + /// Joint damping coefficient + Vector mDampingCoefficients; + + /// Joint Coulomb friction + Vector mFrictions; + + /// True if the name of the corresponding DOF is not allowed to be + /// overwritten + BoolArray mPreserveDofNames; + + /// The name of the DegreesOfFreedom for this Joint + StringArray mDofNames; + + /// Default constructor + MultiDofJointUniqueProperties( + const Vector& _positionLowerLimits = Vector::Constant(-DART_DBL_INF), + const Vector& _positionUpperLimits = Vector::Constant( DART_DBL_INF), + const Vector& _velocityLowerLimits = Vector::Constant(-DART_DBL_INF), + const Vector& _velocityUpperLimits = Vector::Constant( DART_DBL_INF), + const Vector& _accelerationLowerLimits = Vector::Constant(-DART_DBL_INF), + const Vector& _accelerationUpperLimits = Vector::Constant( DART_DBL_INF), + const Vector& _forceLowerLimits = Vector::Constant(-DART_DBL_INF), + const Vector& _forceUpperLimits = Vector::Constant( DART_DBL_INF), + const Vector& _springStiffness = Vector::Constant(0.0), + const Vector& _restPosition = Vector::Constant(0.0), + const Vector& _dampingCoefficient = Vector::Constant(0.0), + const Vector& _coulombFrictions = Vector::Constant(0.0)); + // TODO(MXG): In version 6.0, we should add mInitialPositions and + // mInitialVelocities to the constructor arguments. For now we must wait in + // order to avoid breaking the API. + + /// Copy constructor + // Note: we only need this because VS2013 lacks full support for std::array + // Once std::array is properly supported, this should be removed. + MultiDofJointUniqueProperties(const MultiDofJointUniqueProperties& _other); + + virtual ~MultiDofJointUniqueProperties() = default; + +public: + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; + +//============================================================================== +template +struct MultiDofJointProperties : + Joint::Properties, + MultiDofJointUniqueProperties +{ + MultiDofJointProperties( + const Joint::Properties& _jointProperties = Joint::Properties(), + const MultiDofJointUniqueProperties& _multiDofProperties = + MultiDofJointUniqueProperties()); + + virtual ~MultiDofJointProperties() = default; + +public: + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +//============================================================================== +template +class MultiDofJointAddon final : + public AddonWithProtectedPropertiesInSkeleton< + MultiDofJointAddon, MultiDofJointUniqueProperties, MultiDofJoint, + common::detail::NoOp*>, false > +{ +public: + MultiDofJointAddon(const MultiDofJointAddon&) = delete; + + MultiDofJointAddon(common::AddonManager* mgr, + const typename MultiDofJointAddon::PropertiesData& properties); + + constexpr static size_t NumDofs = DOF; + using Vector = Eigen::Matrix; + using BoolArray = std::array; + using StringArray = std::array; + +// void setPositionLowerLimit(size_t index, const double& value) +// { +// MultiDofJoint::Addon::UpdateProperties(this); +// } + + DART_DYNAMICS_SET_GET_MULTIDOF_ADDON(double, Vector, PositionLowerLimit) + DART_DYNAMICS_SET_GET_MULTIDOF_ADDON(double, Vector, PositionUpperLimit) + DART_DYNAMICS_SET_GET_MULTIDOF_ADDON(double, Vector, InitialPosition) + DART_DYNAMICS_SET_GET_MULTIDOF_ADDON(double, Vector, VelocityLowerLimit) + DART_DYNAMICS_SET_GET_MULTIDOF_ADDON(double, Vector, VelocityUpperLimit) + DART_DYNAMICS_IRREGULAR_SET_GET_MULTIDOF_ADDON(double, Vector, InitialVelocity, InitialVelocities) + DART_DYNAMICS_SET_GET_MULTIDOF_ADDON(double, Vector, AccelerationLowerLimit) + DART_DYNAMICS_SET_GET_MULTIDOF_ADDON(double, Vector, AccelerationUpperLimit) + DART_DYNAMICS_SET_GET_MULTIDOF_ADDON(double, Vector, ForceLowerLimit) + DART_DYNAMICS_SET_GET_MULTIDOF_ADDON(double, Vector, ForceUpperLimit) + DART_DYNAMICS_IRREGULAR_SET_GET_MULTIDOF_ADDON(double, Vector, SpringStiffness, SpringStiffnesses) + DART_DYNAMICS_SET_GET_MULTIDOF_ADDON(double, Vector, RestPosition) + DART_DYNAMICS_SET_GET_MULTIDOF_ADDON(double, Vector, DampingCoefficient) + DART_DYNAMICS_SET_GET_MULTIDOF_ADDON(double, Vector, Friction) + DART_DYNAMICS_SET_GET_MULTIDOF_ADDON(bool, BoolArray, PreserveDofName) + + const std::string& setDofName(size_t index, const std::string& name, bool preserveName); + DART_DYNAMICS_GET_ADDON_PROPERTY_ARRAY(MultiDofJointAddon, std::string, StringArray, DofName, DofNames, DOF) + + friend class MultiDofJoint; + +private: + /// Used by the MultiDofJoint class to get a mutable reference to one of the + /// DOF names. Only the MultiDofJoint class should ever use this function. + std::string& _getDofNameReference(size_t index); +}; + +//============================================================================== +template +MultiDofJointUniqueProperties::MultiDofJointUniqueProperties( + const Vector& _positionLowerLimits, + const Vector& _positionUpperLimits, + const Vector& _velocityLowerLimits, + const Vector& _velocityUpperLimits, + const Vector& _accelerationLowerLimits, + const Vector& _accelerationUpperLimits, + const Vector& _forceLowerLimits, + const Vector& _forceUpperLimits, + const Vector& _springStiffness, + const Vector& _restPosition, + const Vector& _dampingCoefficient, + const Vector& _coulombFrictions) + : mPositionLowerLimits(_positionLowerLimits), + mPositionUpperLimits(_positionUpperLimits), + mInitialPositions(Vector::Zero()), + mVelocityLowerLimits(_velocityLowerLimits), + mVelocityUpperLimits(_velocityUpperLimits), + mInitialVelocities(Vector::Zero()), + mAccelerationLowerLimits(_accelerationLowerLimits), + mAccelerationUpperLimits(_accelerationUpperLimits), + mForceLowerLimits(_forceLowerLimits), + mForceUpperLimits(_forceUpperLimits), + mSpringStiffnesses(_springStiffness), + mRestPositions(_restPosition), + mDampingCoefficients(_dampingCoefficient), + mFrictions(_coulombFrictions) +{ + for (size_t i = 0; i < DOF; ++i) + { + mPreserveDofNames[i] = false; + mDofNames[i] = std::string(); + } +} + +//============================================================================== +template +MultiDofJointUniqueProperties::MultiDofJointUniqueProperties( + const MultiDofJointUniqueProperties& _other) + : mPositionLowerLimits(_other.mPositionLowerLimits), + mPositionUpperLimits(_other.mPositionUpperLimits), + mInitialPositions(_other.mInitialPositions), + mVelocityLowerLimits(_other.mVelocityLowerLimits), + mVelocityUpperLimits(_other.mVelocityUpperLimits), + mInitialVelocities(_other.mInitialVelocities), + mAccelerationLowerLimits(_other.mAccelerationLowerLimits), + mAccelerationUpperLimits(_other.mAccelerationUpperLimits), + mForceLowerLimits(_other.mForceLowerLimits), + mForceUpperLimits(_other.mForceUpperLimits), + mSpringStiffnesses(_other.mSpringStiffnesses), + mRestPositions(_other.mRestPositions), + mDampingCoefficients(_other.mDampingCoefficients), + mFrictions(_other.mFrictions) +{ + for (size_t i = 0; i < DOF; ++i) + { + mPreserveDofNames[i] = _other.mPreserveDofNames[i]; + mDofNames[i] = _other.mDofNames[i]; + } +} + +//============================================================================== +template +MultiDofJointProperties::MultiDofJointProperties( + const Joint::Properties& _jointProperties, + const MultiDofJointUniqueProperties& _multiDofProperties) + : Joint::Properties(_jointProperties), + MultiDofJointUniqueProperties(_multiDofProperties) +{ + // Do nothing +} + +//============================================================================== +template +MultiDofJointAddon::MultiDofJointAddon( + common::AddonManager* mgr, + const typename MultiDofJointAddon::PropertiesData& properties) + : AddonWithProtectedPropertiesInSkeleton< + typename MultiDofJointAddon::Base, + typename MultiDofJointAddon::PropertiesData, + typename MultiDofJointAddon::ManagerType, + &common::detail::NoOp::Base*>, + MultiDofJointAddon::Optional>(mgr, properties) +{ + // Do nothing +} + +//============================================================================== +template +const std::string& MultiDofJointAddon::setDofName( + size_t index, const std::string& name, bool preserveName) +{ + return this->getManager()->setDofName(index, name, preserveName); +} + +//============================================================================== +template +std::string& MultiDofJointAddon::_getDofNameReference(size_t index) +{ + return this->mProperties.mDofNames[index]; +} + +} // namespace detail +} // namespace dynamics +} // namespace dart + +#endif // DART_DYNAMICS_DETAIL_MULTIDOFJOINTPROPERTIES_H_ + diff --git a/dart/dynamics/detail/Node.h b/dart/dynamics/detail/Node.h new file mode 100644 index 0000000000000..16acc04c276ff --- /dev/null +++ b/dart/dynamics/detail/Node.h @@ -0,0 +1,350 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_DYNAMICS_DETAIL_NODE_H_ +#define DART_DYNAMICS_DETAIL_NODE_H_ + +#include + +#include "dart/dynamics/Node.h" + +namespace dart { +namespace dynamics { + +//============================================================================== +template +static T getVectorObjectIfAvailable(size_t _index, const std::vector& _vec) +{ + assert(_index < _vec.size()); + if(_index < _vec.size()) + return _vec[_index]; + + return nullptr; +} + +//============================================================================== +template +size_t AccessoryNode::getIndexInBodyNode() const +{ + return static_cast(this)->mIndexInBodyNode; +} + +//============================================================================== +template +size_t AccessoryNode::getIndexInSkeleton() const +{ + return static_cast(this)->mIndexInSkeleton; +} + +//============================================================================== +template +size_t AccessoryNode::getIndexInTree() const +{ + return static_cast(this)->mIndexInTree; +} + +//============================================================================== +template +size_t AccessoryNode::getTreeIndex() const +{ + return static_cast(this)->getBodyNodePtr()->getTreeIndex(); +} + +//============================================================================== +template +void AccessoryNode::remove() +{ + return static_cast(this)->stageForRemoval(); +} + +//============================================================================== +template +void AccessoryNode::reattach() +{ + static_cast(this)->attach(); +} + +} // namespace dynamics +} // namespace dart + +//============================================================================== +// Macros for specializing Nodes within BodyNodes +//============================================================================== +#define DART_ENABLE_NODE_SPECIALIZATION() \ + public: \ + template size_t getNumNodes() const { return dart::dynamics::BodyNode::getNumNodes(); } \ + template T* getNode(size_t index) { return dart::dynamics::BodyNode::getNode(index); } \ + template const T* getNode(size_t index) const { return dart::dynamics::BodyNode::getNode(index); } + +//============================================================================== +#define DETAIL_DART_INSTANTIATE_SPECIALIZED_NODE( NodeName, it ) \ + mNodeMap[typeid( NodeName )] = std::vector(); \ + it = mNodeMap.find(typeid( NodeName )); + +//============================================================================== +#define DART_INSTANTIATE_SPECIALIZED_NODE_IRREGULAR( NodeName, PluralName ) \ + DETAIL_DART_INSTANTIATE_SPECIALIZED_NODE( NodeName, m ## PluralName ## Iterator ) + +//============================================================================== +#define DART_INSTANTIATE_SPECALIZED_NODE( NodeName ) \ + DART_INSTANTIATE_SPECIALIZED_NODE_IRREGULAR( NodeName, NodeName ## s ) + +//============================================================================== +#define DETAIL_DART_SPECIALIZED_NODE_INLINE( NodeName, PluralName, it ) \ + private: dart::dynamics::BodyNode::NodeMap::iterator it ; public: \ + \ + inline size_t getNum ## PluralName () const \ + { return it->second.size(); } \ + \ + inline NodeName * get ## NodeName (size_t index) \ + { return static_cast< NodeName *>(getVectorObjectIfAvailable(index, it->second)); } \ + \ + inline const NodeName * get ## NodeName (size_t index) const \ + { return static_cast(getVectorObjectIfAvailable(index, it->second)); } + +//============================================================================== +#define DART_SPECIALIZED_NODE_INLINE_IRREGULAR( NodeName, PluralName ) \ + DETAIL_DART_SPECIALIZED_NODE_INLINE( NodeName, PluralName, m ## PluralName ## Iterator ) + +//============================================================================== +#define DART_SPECIALIZED_NODE_INTERNAL( NodeName ) \ + DART_SPECIALIZED_NODE_INLINE_IRREGULAR( NodeName, NodeName ## s ) + +//============================================================================== +#define DETAIL_DART_SPECIALIZED_NODE_DECLARE( NodeName, PluralName, it ) \ + private: dart::dynamics::BodyNode::NodeMap::iterator it ; public:\ + size_t getNum ## PluralName() const;\ + NodeName * get ## NodeName (size_t index);\ + const NodeName * get ## NodeName(size_t index) const; + +//============================================================================== +#define DART_SPECIALIZED_NODE_DECLARE_IRREGULAR( NodeName, PluralName )\ + DETAIL_DART_SPECIALIZED_NODE_DECLARE( NodeName, PluralName, m ## PluralName ## Iterator ) + +//============================================================================== +#define DART_SPECIALIZED_NODE_DECLARE( NodeName )\ + DART_SPECIALIZED_NODE_DECLARE_IRREGULAR( NodeName, NodeName ## s ) + +//============================================================================== +#define DETAIL_DART_SPECIALIZED_NODE_DEFINE( BodyNodeType, NodeName, PluralName, it )\ + size_t BodyNodeType :: getNum ## PluralName () const\ + { return it->second.size(); }\ + \ + NodeName * BodyNodeType :: get ## NodeName (size_t index)\ + { return static_cast(getVectorObjectIfAvailable(index, it->second)); }\ + \ + const NodeName * BodyNodeType :: get ## NodeName (size_t index) const\ + { return static_cast(getVectorObjectIfAvailable(index, it->second)); } + +//============================================================================== +#define DART_SPECIALIZED_NODE_DEFINE_IRREGULAR( BodyNodeType, NodeName, PluralName )\ + DETAIL_DART_SPECIALIZED_NODE_DEFINE( BodyNodeType, NodeName, PluralName, m ## PluralName ## Iterator ) + +//============================================================================== +#define DART_SPECIALIZED_NODE_DEFINE( BodyNodeType, NodeName )\ + DART_SPECIALIZED_NODE_DEFINE_IRREGULAR( BodyNodeType, NodeName, NodeName ## s ) + +//============================================================================== +#define DETAIL_DART_SPECIALIZED_NODE_TEMPLATE( BodyNodeType, NodeName, PluralName ) \ + template <> inline size_t BodyNodeType :: getNumNodes< NodeName >() const { return getNum ## PluralName (); } \ + template <> inline NodeName * BodyNodeType :: getNode< NodeName >(size_t index) { return get ## NodeName (index); } \ + template <> inline const NodeName * BodyNodeType :: getNode< NodeName >(size_t index) const { return get ## NodeName (index); } + +//============================================================================== +#define DART_SPECIALIZED_NODE_TEMPLATE_IRREGULAR( BodyNodeType, NodeName, PluralName ) \ + DETAIL_DART_SPECIALIZED_NODE_TEMPLATE( BodyNodeType, NodeName, PluralName ) + +//============================================================================== +#define DART_SPECIALIZED_NODE_TEMPLATE( BodyNodeType, NodeName ) \ + DART_SPECIALIZED_NODE_TEMPLATE_IRREGULAR( BodyNodeType, NodeName, NodeName ## s ) + +//============================================================================== +// Macros for specializing Nodes within Skeletons +//============================================================================== +#define DART_SKEL_ENABLE_NODE_SPECIALIZATION() \ + public: \ + template size_t getNumNodes() const { return dart::dynamics::Skeleton::getNumNodes(); } \ + template size_t getNumNodes(size_t treeIndex) const { return dart::dynamics::Skeleton::getNumNodes(treeIndex); } \ + template T* getNode(size_t index) { return dart::dynamics::Skeleton::getNode(index); } \ + template T* getNode(size_t nodeIdx, size_t treeIndex) { return dart::dynamics::Skeleton::getNode(nodeIdx, treeIndex); } \ + template const T* getNode(size_t index) const { return dart::dynamics::Skeleton::getNode(index); } \ + template const T* getNode(size_t nodeIdx, size_t treeIndex) const { return dart::dynamics::Skeleton::getNode(nodeIdx, treeIndex); } \ + template T* getNode(const std::string& name) { return dart::dynamics::Skeleton::getNode(name); } \ + template const T* getNode(const std::string& name) const { return dart::dynamics::Skeleton::getNode(name); } + +//============================================================================== +#define DART_SKEL_INSTANTIATE_SPECIALIZED_NODE_IRREGULAR( NodeName, PluralName ) \ + mSpecializedTreeNodes[typeid( NodeName )] = &mTree ## PluralName ## Iterators; \ + mSkelCache.mNodeMap[typeid( NodeName )] = std::vector(); \ + m ## PluralName ## Iterator = mSkelCache.mNodeMap.find(typeid( NodeName )); \ + mNodeNameMgrMap[typeid( NodeName )] = dart::common::NameManager( \ + std::string("Skeleton::") + #NodeName + " | " + mSkeletonP.mName ); \ + mNameMgrFor ## PluralName = &mNodeNameMgrMap.find(typeid( NodeName) )->second; + +//============================================================================== +#define DART_SKEL_INSTANTIATE_SPECIALIZED_NODE( NodeName ) \ + DART_SKEL_INSTANTIATE_SPECIALIZED_NODE_IRREGULAR( NodeName, NodeName ## s ); + +//============================================================================== +#define DETAIL_DART_WARN_TREE_INDEX(treeIts, treeIndex, func) \ + if(treeIndex >= treeIts.size()) \ + { dterr << "[" << #func << "] Requesting an invalid tree (" << treeIndex << "). " \ + << "The number of trees in this Skeleton is: " << treeIts.size() << "\n"; \ + assert(false); return 0; } + +//============================================================================== +#define DETAIL_DART_SKEL_SPECIALIZED_NODE_INLINE( NodeName, PluralName, skelIt, treeIts, NameMgr ) \ + private: \ + dart::dynamics::Skeleton::NodeMap::iterator skelIt; \ + std::vector treeIts; \ + dart::common::NameManager* NameMgr; \ + public: \ + inline size_t getNum ## PluralName () const \ + { return skelIt->second.size(); } \ + inline size_t getNum ## PluralName (size_t treeIndex) const \ + { DETAIL_DART_WARN_TREE_INDEX(treeIts, treeIndex, getNum ## PluralName); \ + treeIts [treeIndex]->second.size(); } \ + \ + inline NodeName * get ## NodeName (size_t index) \ + { return static_cast< NodeName *>(getVectorObjectIfAvailable(index, skelIt ->second)); } \ + inline NodeName * get ## NodeName (size_t treeIndex, size_t nodeIndex) \ + { DETAIL_DART_WARN_TREE_INDEX(treeIts, treeIndex, get ## NodeName); \ + return static_cast< NodeName *>(getVectorObjectIfAvailable(nodeIndex, treeIts[treeIndex]->second)); } \ + \ + inline const NodeName * get ## NodeName (size_t index) const \ + { return static_cast(getVectorObjectIfAvailable(index, skelIt ->second)); } \ + inline const NodeName * get ## NodeName (size_t treeIndex, size_t nodeIndex) const \ + { DETAIL_DART_WARN_TREE_INDEX(treeIts, treeIndex, get ## NodeName); \ + return static_cast(getVectorObjectIfAvailable(nodeIndex, treeIts[treeIndex]->second)); } \ + \ + inline NodeName * get ## NodeName (const std::string& name) \ + { return static_cast< NodeName *>(NameMgr->getObject(name)); } \ + inline const NodeName * get ## NodeName (const std::string& name) const \ + { return static_cast(NameMgr->getObject(name)); } + +//============================================================================== +#define DART_SKEL_SPECIALIZED_NODE_INLINE_IRREGULAR( NodeName, PluralName ) \ + DETAIL_DART_SKEL_SPECIALIZED_NODE_INLINE( NodeName, PluralName, m ## PluralName ## Iterator, mTree ## PluralName ## Iterators, mNameMgrFor ## PluralName ); + +//============================================================================== +#define DART_SKEL_SPECIALIZED_NODE_INLINE( NodeName ) \ + DART_SKEL_SPECIALIZED_NODE_INLINE_IRREGULAR( NodeName, NodeName ## s); + +//============================================================================== +#define DETAIL_DART_SKEL_SPECIALIZED_NODE_DECLARE( NodeName, PluralName, skelIt, treeIts, NameMgr ) \ + private:\ + dart::dynamics::Skeleton::NodeMap::iterator skelIt; \ + std::vector treeIts; \ + dart::common::NameManager* NameMgr; \ + public: \ + size_t getNum ## PluralName () const; \ + \ + size_t getNum ## PluralName (size_t treeIndex) const; \ + \ + NodeName * get ## NodeName (size_t index); \ + \ + NodeName * get ## NodeName (size_t treeIndex, size_t nodeIndex); \ + \ + const NodeName * get ## NodeName (size_t index) const; \ + \ + const NodeName * get ## NodeName (size_t treeIndex, size_t nodeIndex) const; \ + \ + NodeName * get ## NodeName (const std::string& name); \ + \ + const NodeName * get ## NodeName (const std::string& name) const; + +//============================================================================== +#define DART_SKEL_SPECIALIZED_NODE_DECLARE_IRREGULAR( NodeName, PluralName )\ + DETAIL_DART_SKEL_SPECIALIZED_NODE_DECLARE( NodeName, PluralName, m ## PluralName ## Iterator, mTree ## PluralName ## Iterators, mNameMgrFor ## PluralName ); + +//============================================================================== +#define DART_SKEL_SPECIALIZED_NODE_DECLARE( NodeName )\ + DART_SKEL_SPECIALIZED_NODE_DECLARE_IRREGULAR( NodeName, NodeName ## s ); + +//============================================================================== +#define DETAIL_DART_SKEL_SPECIALIZED_NODE_DEFINE( SkeletonType, NodeName, PluralName, skelIt, treeIts, NameMgr )\ + size_t SkeletonType :: getNum ## PluralName () const \ + { return skelIt->second.size(); } \ + size_t SkeletonType :: getNum ## PluralName (size_t treeIndex) const \ + { DETAIL_DART_WARN_TREE_INDEX(treeIts, treeIndex, getNum ## PluralName); \ + return treeIts [treeIndex]->second.size(); } \ + \ + NodeName * SkeletonType :: get ## NodeName (size_t index) \ + { return static_cast< NodeName *>(getVectorObjectIfAvailable(index, skelIt ->second)); } \ + NodeName * SkeletonType :: get ## NodeName (size_t treeIndex, size_t nodeIndex) \ + { DETAIL_DART_WARN_TREE_INDEX(treeIts, treeIndex, get ## NodeName); \ + return static_cast< NodeName *>(getVectorObjectIfAvailable(nodeIndex, treeIts[treeIndex]->second)); } \ + \ + const NodeName * SkeletonType :: get ## NodeName (size_t index) const \ + { return static_cast(getVectorObjectIfAvailable(index, skelIt ->second)); } \ + const NodeName * SkeletonType :: get ## NodeName (size_t treeIndex, size_t nodeIndex) const \ + { DETAIL_DART_WARN_TREE_INDEX(treeIts, treeIndex, get ## NodeName); \ + return static_cast(getVectorObjectIfAvailable(nodeIndex, treeIts[treeIndex]->second)); } \ + \ + NodeName * SkeletonType :: get ## NodeName (const std::string& name) \ + { return static_cast< NodeName *>(NameMgr->getObject(name)); } \ + const NodeName * SkeletonType :: get ## NodeName (const std::string& name) const \ + { return static_cast(NameMgr->getObject(name)); } + +//============================================================================== +#define DART_SKEL_SPECIALIZED_NODE_DEFINED_IRREGULAR( SkeletonType, NodeName, PluralName )\ + DETAIL_DART_SKEL_SPECIALIZED_NODE_DEFINE( SkeletonType, NodeName, PluralName, m ## PluralName ## Iterator, mTree ## PluralName ## Iterators, mNameMgrFor ## PluralName ); + +//============================================================================== +#define DART_SKEL_SPECIALIZED_NODE_DEFINE( SkeletonType, NodeName )\ + DART_SKEL_SPECIALIZED_NODE_DEFINED_IRREGULAR( SkeletonType, NodeName, NodeName ## s ); + +//============================================================================== +#define DETAIL_DART_SKEL_SPECIALIZED_NODE_TEMPLATE( SkelType, NodeName, PluralName ) \ + template <> inline size_t SkelType :: getNumNodes< NodeName >() const { return getNum ## PluralName (); } \ + template <> inline size_t SkelType :: getNumNodes< NodeName >(size_t index) const { return getNum ## PluralName(index); } \ + template <> inline NodeName* SkelType :: getNode< NodeName >(size_t index) { return get ## NodeName (index); } \ + template <> inline NodeName* SkelType :: getNode< NodeName >(size_t treeIndex, size_t nodeIndex) { return get ## NodeName(treeIndex, nodeIndex); } \ + template <> inline const NodeName* SkelType :: getNode< NodeName >(size_t index) const { return get ## NodeName (index); } \ + template <> inline const NodeName* SkelType :: getNode< NodeName >(size_t treeIndex, size_t nodeIndex) const { return get ## NodeName(treeIndex, nodeIndex); } \ + template <> inline NodeName* SkelType::getNode< NodeName >(const std::string& name) { return get ## NodeName (name); } \ + template <> inline const NodeName* SkelType::getNode< NodeName >(const std::string& name) const { return get ## NodeName (name); } + +//============================================================================== +#define DART_SKEL_SPECIALIZED_NODE_TEMPLATE_IRREGULAR( SkelType, NodeName, PluralName ) \ + DETAIL_DART_SKEL_SPECIALIZED_NODE_TEMPLATE( SkelType, NodeName, PluralName ) + +//============================================================================== +#define DART_SKEL_SPECIALIZED_NODE_TEMPLATE( SkelType, NodeName ) \ + DART_SKEL_SPECIALIZED_NODE_TEMPLATE_IRREGULAR( SkelType, NodeName, NodeName ## s ); + + +#endif // DART_DYNAMICS_DETAIL_NODE_H_ diff --git a/dart/dynamics/detail/NodeManagerJoiner.h b/dart/dynamics/detail/NodeManagerJoiner.h new file mode 100644 index 0000000000000..1d11dc3d3b549 --- /dev/null +++ b/dart/dynamics/detail/NodeManagerJoiner.h @@ -0,0 +1,135 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_DYNAMICS_DETAIL_NODEMANAGERJOINER_H_ +#define DART_DYNAMICS_DETAIL_NODEMANAGERJOINER_H_ + +#include "dart/dynamics/NodeManagerJoiner.h" +#include "dart/common/detail/TemplateJoinerDispatchMacro.h" + +namespace dart { +namespace dynamics { + +//============================================================================== +template +template +NodeManagerJoinerForBodyNode::NodeManagerJoinerForBodyNode( + Base1Arg&& arg1, Base2Args&&... args2) + : Base1(std::forward(arg1)), + Base2(std::forward(args2)...) +{ + // Do nothing +} + +//============================================================================== +template +template +NodeManagerJoinerForBodyNode::NodeManagerJoinerForBodyNode( + Base1Arg&& arg1, common::NoArg_t) + : Base1(std::forward(arg1)), + Base2() +{ + // Do nothing +} + +//============================================================================== +template +template +NodeManagerJoinerForBodyNode::NodeManagerJoinerForBodyNode( + common::NoArg_t, Base2Args&&... args2) + : Base1(), + Base2(std::forward(args2)...) +{ + // Do nothing +} + +//============================================================================== +DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(size_t, NodeManagerJoinerForBodyNode, getNumNodes, () const, ()) +DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(T*, NodeManagerJoinerForBodyNode, getNode, (size_t index), (index)) +DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(const T*, NodeManagerJoinerForBodyNode, getNode, (size_t index) const, (index)) + +//============================================================================== +template +template +constexpr bool NodeManagerJoinerForBodyNode::isSpecializedForNode() +{ + return (Base1::template isSpecializedForNode() + || Base2::template isSpecializedForNode()); +} + +//============================================================================== +template +template +NodeManagerJoinerForBodyNode::NodeManagerJoinerForBodyNode( + Args&&... args) + : NodeManagerJoinerForBodyNode>( + std::forward(args)...) +{ + // Do nothing +} + +//============================================================================== +template +template +NodeManagerJoinerForSkeleton::NodeManagerJoinerForSkeleton( + Args&&... args) + : NodeManagerJoinerForBodyNode(std::forward(args)...) +{ + // Do nothing +} + +//============================================================================== +DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(size_t, NodeManagerJoinerForSkeleton, getNumNodes, (size_t treeIndex) const, (treeIndex)) +DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(T*, NodeManagerJoinerForSkeleton, getNode, (size_t treeIndex, size_t nodeIndex), (treeIndex, nodeIndex)) +DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(const T*, NodeManagerJoinerForSkeleton, getNode, (size_t treeIndex, size_t nodeIndex) const, (treeIndex, nodeIndex)) +DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(T*, NodeManagerJoinerForSkeleton, getNode, (const std::string& name), (name)) +DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(const T*, NodeManagerJoinerForSkeleton, getNode, (const std::string& name) const, (name)) + +//============================================================================== +template +template +NodeManagerJoinerForSkeleton::NodeManagerJoinerForSkeleton( + Args&&... args) + : NodeManagerJoinerForSkeleton>( + std::forward(args)...) +{ + // Do nothing +} + +} // namespace dynamics +} // namespace dart + +#endif // DART_DYNAMICS_DETAIL_NODEMANAGERJOINER_H_ diff --git a/dart/dynamics/detail/PlanarJointProperties.cpp b/dart/dynamics/detail/PlanarJointProperties.cpp new file mode 100644 index 0000000000000..8590e14c19482 --- /dev/null +++ b/dart/dynamics/detail/PlanarJointProperties.cpp @@ -0,0 +1,187 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/dynamics/PlanarJoint.h" + +namespace dart { +namespace dynamics { +namespace detail { + +//============================================================================== +PlanarJointUniqueProperties::PlanarJointUniqueProperties(PlaneType _planeType) +{ + switch(_planeType) + { + case PlaneType::ARBITRARY: + case PlaneType::XY: + setXYPlane(); + mPlaneType = _planeType; // In case the PlaneType was supposed to be arbitrary + break; + case PlaneType::YZ: + setYZPlane(); + break; + case PlaneType::ZX: + setZXPlane(); + break; + } +} + +//============================================================================== +PlanarJointUniqueProperties::PlanarJointUniqueProperties( + const Eigen::Vector3d& _transAxis1, + const Eigen::Vector3d& _transAxis2) +{ + setArbitraryPlane(_transAxis1, _transAxis2); +} + +//============================================================================== +PlanarJointUniqueProperties::PlanarJointUniqueProperties( + const PlanarJointUniqueProperties& other) +{ + switch(other.mPlaneType) + { + case PlaneType::ARBITRARY: + setArbitraryPlane(other.mTransAxis1, other.mTransAxis2); + break; + case PlaneType::XY: + setXYPlane(); + break; + case PlaneType::YZ: + setYZPlane(); + break; + case PlaneType::ZX: + setZXPlane(); + break; + } +} + +//============================================================================== +void PlanarJointUniqueProperties::setXYPlane() +{ + mPlaneType = PlaneType::XY; + mRotAxis = Eigen::Vector3d::UnitZ(); + mTransAxis1 = Eigen::Vector3d::UnitX(); + mTransAxis2 = Eigen::Vector3d::UnitY(); +} + +//============================================================================== +void PlanarJointUniqueProperties::setYZPlane() +{ + mPlaneType = PlaneType::YZ; + mRotAxis = Eigen::Vector3d::UnitX(); + mTransAxis1 = Eigen::Vector3d::UnitY(); + mTransAxis2 = Eigen::Vector3d::UnitZ(); +} + +//============================================================================== +void PlanarJointUniqueProperties::setZXPlane() +{ + mPlaneType = PlaneType::ZX; + mRotAxis = Eigen::Vector3d::UnitY(); + mTransAxis1 = Eigen::Vector3d::UnitZ(); + mTransAxis2 = Eigen::Vector3d::UnitX(); +} + +//============================================================================== +void PlanarJointUniqueProperties::setArbitraryPlane( + const Eigen::Vector3d& _transAxis1, + const Eigen::Vector3d& _transAxis2) +{ + // Set plane type as arbitrary plane + mPlaneType = PlaneType::ARBITRARY; + + // First translational axis + mTransAxis1 = _transAxis1.normalized(); + + // Second translational axis + mTransAxis2 = _transAxis2.normalized(); + + // Orthogonalize translational axese + double dotProduct = mTransAxis1.dot(mTransAxis2); + assert(std::abs(dotProduct) < 1.0 - 1e-6); + if (std::abs(dotProduct) > 1e-6) + mTransAxis2 = (mTransAxis2 - dotProduct * mTransAxis1).normalized(); + + // Rotational axis + mRotAxis = (mTransAxis1.cross(mTransAxis2)).normalized(); +} + +//============================================================================== +PlanarJointProperties::PlanarJointProperties( + const MultiDofJoint<3>::Properties& _multiDofProperties, + const PlanarJointUniqueProperties& _planarProperties) + : MultiDofJoint<3>::Properties(_multiDofProperties), + PlanarJointUniqueProperties(_planarProperties) +{ + // Do nothing +} + +//============================================================================== +void PlanarJointAddon::setXYPlane() +{ + mProperties.setXYPlane(); + UpdateProperties(this); + incrementSkeletonVersion(); +} + +//============================================================================== +void PlanarJointAddon::setYZPlane() +{ + mProperties.setYZPlane(); + UpdateProperties(this); + incrementSkeletonVersion(); +} + +//============================================================================== +void PlanarJointAddon::setZXPlane() +{ + mProperties.setZXPlane(); + UpdateProperties(this); + incrementSkeletonVersion(); +} + +//============================================================================== +void PlanarJointAddon::setArbitraryPlane(const Eigen::Vector3d& _axis1, + const Eigen::Vector3d& _axis2) +{ + mProperties.setArbitraryPlane(_axis1, _axis2); + UpdateProperties(this); + incrementSkeletonVersion(); +} + +} // namespace detail +} // namespace dynamics +} // namespace dart diff --git a/dart/dynamics/detail/PlanarJointProperties.h b/dart/dynamics/detail/PlanarJointProperties.h new file mode 100644 index 0000000000000..92c1191e5e931 --- /dev/null +++ b/dart/dynamics/detail/PlanarJointProperties.h @@ -0,0 +1,155 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_DYNAMICS_DETAIL_PLANARJOINTPROPERTIES_H_ +#define DART_DYNAMICS_DETAIL_PLANARJOINTPROPERTIES_H_ + +#include + +#include "dart/dynamics/MultiDofJoint.h" +#include "dart/dynamics/Addon.h" + +namespace dart { +namespace dynamics { + +class PlanarJoint; + +namespace detail { + +//============================================================================== +/// Plane type +enum class PlaneType : int +{ + XY, + YZ, + ZX, + ARBITRARY +}; + +//============================================================================== +/// Properties that are unique to PlanarJoints. Note that the mPlaneType +/// member has greater authority than the mTransAxis1 and mTransAxis2 members. +/// When copying properties into a PlanarJoint, it will first defer to +/// mPlaneType. If mPlaneType is PlaneType::ARBITRARY, then and only then will +/// it use mTransAxis1 and mTransAxis2. mRotAxis has no authority; it will +/// always be recomputed from mTransAxis1 and mTransAxis2 when copying it into a +/// PlanarJoint +struct PlanarJointUniqueProperties +{ + /// Plane type + PlaneType mPlaneType; + + /// First translational axis + Eigen::Vector3d mTransAxis1; + + /// Second translational axis + Eigen::Vector3d mTransAxis2; + + /// Rotational axis + Eigen::Vector3d mRotAxis; + + /// Constructor for pre-defined plane types. Defaults to the XY plane if + /// PlaneType::ARBITRARY is specified. + PlanarJointUniqueProperties(PlaneType _planeType = PlaneType::XY); + + /// Constructor for arbitrary plane types. mPlaneType will be set to + /// PlaneType::ARBITRARY + PlanarJointUniqueProperties(const Eigen::Vector3d& _transAxis1, + const Eigen::Vector3d& _transAxis2); + + /// Copy-constructor, customized for robustness + PlanarJointUniqueProperties(const PlanarJointUniqueProperties& other); + + virtual ~PlanarJointUniqueProperties() = default; + + /// Set plane type as XY-plane + void setXYPlane(); + + /// Set plane type as YZ-plane + void setYZPlane(); + + /// Set plane type as ZX-plane + void setZXPlane(); + + /// Set plane type as arbitrary plane with two orthogonal translational axes + void setArbitraryPlane(const Eigen::Vector3d& _transAxis1, + const Eigen::Vector3d& _transAxis2); +}; + +//============================================================================== +struct PlanarJointProperties : + MultiDofJoint<3>::Properties, + PlanarJointUniqueProperties +{ + PlanarJointProperties( + const MultiDofJoint<3>::Properties& _multiDofProperties = + MultiDofJoint<3>::Properties(), + const PlanarJointUniqueProperties& _planarProperties = + PlanarJointUniqueProperties()); + + virtual ~PlanarJointProperties() = default; +}; + +//============================================================================== +class PlanarJointAddon final : + public AddonWithProtectedPropertiesInSkeleton< + PlanarJointAddon, PlanarJointUniqueProperties, PlanarJoint, + detail::JointPropertyUpdate, false > +{ +public: + DART_DYNAMICS_JOINT_ADDON_CONSTRUCTOR( PlanarJointAddon ) + + void setXYPlane(); + void setYZPlane(); + void setZXPlane(); + void setArbitraryPlane(const Eigen::Vector3d& _axis1, + const Eigen::Vector3d& _axis2); + + DART_DYNAMICS_GET_ADDON_PROPERTY( PlaneType, PlaneType ) + DART_DYNAMICS_GET_ADDON_PROPERTY( Eigen::Vector3d, TransAxis1 ) + DART_DYNAMICS_GET_ADDON_PROPERTY( Eigen::Vector3d, TransAxis2 ) + DART_DYNAMICS_GET_ADDON_PROPERTY( Eigen::Vector3d, RotAxis ) +}; + +//============================================================================== +using PlanarJointBase = common::AddonManagerJoiner< + MultiDofJoint<3>, common::SpecializedAddonManager >; + +} // namespace detail +} // namespace dynamics +} // namespace dart + +#endif // DART_DYNAMICS_DETAIL_PLANARJOINTPROPERTIES_H_ diff --git a/dart/dynamics/detail/PrismaticJointProperties.cpp b/dart/dynamics/detail/PrismaticJointProperties.cpp new file mode 100644 index 0000000000000..2b5640ccb75c1 --- /dev/null +++ b/dart/dynamics/detail/PrismaticJointProperties.cpp @@ -0,0 +1,77 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/dynamics/PrismaticJoint.h" + +namespace dart { +namespace dynamics { +namespace detail { + +//============================================================================== +PrismaticJointUniqueProperties::PrismaticJointUniqueProperties( + const Eigen::Vector3d& _axis) + : mAxis(_axis.normalized()) +{ + // Do nothing +} + +//============================================================================== +PrismaticJointProperties::PrismaticJointProperties( + const SingleDofJoint::Properties& _singleDofProperties, + const PrismaticJointUniqueProperties& _prismaticProperties) + : SingleDofJoint::Properties(_singleDofProperties), + PrismaticJointUniqueProperties(_prismaticProperties) +{ + // Do nothing +} + +//============================================================================== +void PrismaticJointAddon::setAxis(const Eigen::Vector3d& _axis) +{ + mProperties.mAxis = _axis.normalized(); + incrementSkeletonVersion(); + UpdateProperties(this); +} + +//============================================================================== +const Eigen::Vector3d& PrismaticJointAddon::getAxis() const +{ + return mProperties.mAxis; +} + +} // namespace detail +} // namespace dynamics +} // namespace dart diff --git a/dart/dynamics/detail/PrismaticJointProperties.h b/dart/dynamics/detail/PrismaticJointProperties.h new file mode 100644 index 0000000000000..be628399971e3 --- /dev/null +++ b/dart/dynamics/detail/PrismaticJointProperties.h @@ -0,0 +1,100 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_DYNAMICS_DETAIL_PRISMATICJOINTPROPERTIES_H_ +#define DART_DYNAMICS_DETAIL_PRISMATICJOINTPROPERTIES_H_ + +#include + +#include + +#include "dart/dynamics/SingleDofJoint.h" +#include "dart/dynamics/Addon.h" + +namespace dart { +namespace dynamics { + +class PrismaticJoint; + +namespace detail { + +//============================================================================== +struct PrismaticJointUniqueProperties +{ + Eigen::Vector3d mAxis; + + PrismaticJointUniqueProperties( + const Eigen::Vector3d& _axis = Eigen::Vector3d::UnitZ()); + + virtual ~PrismaticJointUniqueProperties() = default; +}; + +//============================================================================== +struct PrismaticJointProperties : + SingleDofJoint::Properties, + PrismaticJointUniqueProperties +{ + PrismaticJointProperties( + const SingleDofJoint::Properties& _singleDofProperties = + SingleDofJoint::Properties(), + const PrismaticJointUniqueProperties& _prismaticProperties = + PrismaticJointUniqueProperties()); + + virtual ~PrismaticJointProperties() = default; +}; + +//============================================================================== +class PrismaticJointAddon final : + public AddonWithProtectedPropertiesInSkeleton< + PrismaticJointAddon, PrismaticJointUniqueProperties, PrismaticJoint, + detail::JointPropertyUpdate, false > +{ +public: + DART_DYNAMICS_JOINT_ADDON_CONSTRUCTOR( PrismaticJointAddon ) + + void setAxis(const Eigen::Vector3d& _axis); + const Eigen::Vector3d& getAxis() const; +}; + +//============================================================================== +using PrismaticJointBase = common::AddonManagerJoiner< + SingleDofJoint, common::SpecializedAddonManager >; + +} // namespace detail +} // namespace dynamics +} // namespace dart + +#endif // DART_DYNAMICS_DETAIL_PRISMATICJOINTPROPERTIES_H_ diff --git a/dart/dynamics/detail/RevoluteJointProperties.cpp b/dart/dynamics/detail/RevoluteJointProperties.cpp new file mode 100644 index 0000000000000..66c0c52bf9713 --- /dev/null +++ b/dart/dynamics/detail/RevoluteJointProperties.cpp @@ -0,0 +1,77 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/dynamics/RevoluteJoint.h" + +namespace dart { +namespace dynamics { +namespace detail { + +//============================================================================== +RevoluteJointUniqueProperties::RevoluteJointUniqueProperties( + const Eigen::Vector3d& _axis) + : mAxis(_axis.normalized()) +{ + // Do nothing +} + +//============================================================================== +RevoluteJointProperties::RevoluteJointProperties( + const SingleDofJoint::Properties& _singleDofJointProperties, + const RevoluteJointUniqueProperties& _revoluteProperties) + : SingleDofJoint::Properties(_singleDofJointProperties), + RevoluteJointUniqueProperties(_revoluteProperties) +{ + // Do nothing +} + +//============================================================================== +void RevoluteJointAddon::setAxis(const Eigen::Vector3d& _axis) +{ + mProperties.mAxis = _axis.normalized(); + incrementSkeletonVersion(); + UpdateProperties(this); +} + +//============================================================================== +const Eigen::Vector3d& RevoluteJointAddon::getAxis() const +{ + return mProperties.mAxis; +} + +} // namespace detail +} // namespace dynamics +} // namespace dart diff --git a/dart/dynamics/detail/RevoluteJointProperties.h b/dart/dynamics/detail/RevoluteJointProperties.h new file mode 100644 index 0000000000000..fbc00024181b7 --- /dev/null +++ b/dart/dynamics/detail/RevoluteJointProperties.h @@ -0,0 +1,102 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_DYNAMICS_DETAIL_REVOLUTEJOINTPROPERTIES_H_ +#define DART_DYNAMICS_DETAIL_REVOLUTEJOINTPROPERTIES_H_ + +#include + +#include + +#include "dart/dynamics/SingleDofJoint.h" +#include "dart/dynamics/Addon.h" + +namespace dart { +namespace dynamics { + +class RevoluteJoint; + +namespace detail { + +//============================================================================== +struct RevoluteJointUniqueProperties +{ + Eigen::Vector3d mAxis; + + RevoluteJointUniqueProperties( + const Eigen::Vector3d& _axis = Eigen::Vector3d::UnitZ()); + + virtual ~RevoluteJointUniqueProperties() = default; +}; + +//============================================================================== +struct RevoluteJointProperties : + SingleDofJoint::Properties, + RevoluteJointUniqueProperties +{ + RevoluteJointProperties( + const SingleDofJoint::Properties& _singleDofJointProperties = + SingleDofJoint::Properties(), + const RevoluteJointUniqueProperties& _revoluteProperties = + RevoluteJointUniqueProperties()); + + virtual ~RevoluteJointProperties() = default; +}; + +//============================================================================== +class RevoluteJointAddon final : + public AddonWithProtectedPropertiesInSkeleton< + RevoluteJointAddon, RevoluteJointUniqueProperties, RevoluteJoint, + detail::JointPropertyUpdate, false > +{ +public: + DART_DYNAMICS_JOINT_ADDON_CONSTRUCTOR( RevoluteJointAddon ) + + void setAxis(const Eigen::Vector3d& _axis); + const Eigen::Vector3d& getAxis() const; +}; + +//============================================================================== +using RevoluteJointBase = common::AddonManagerJoiner< + SingleDofJoint, common::SpecializedAddonManager >; + +} // namespace detail + +} // namespace dynamics +} // namespace dart + + +#endif // DART_DYNAMICS_DETAIL_REVOLUTEJOINTPROPERTIES_H_ diff --git a/dart/dynamics/detail/ScrewJointProperties.cpp b/dart/dynamics/detail/ScrewJointProperties.cpp new file mode 100644 index 0000000000000..42ec703642ef7 --- /dev/null +++ b/dart/dynamics/detail/ScrewJointProperties.cpp @@ -0,0 +1,78 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/dynamics/ScrewJoint.h" + +namespace dart { +namespace dynamics { +namespace detail { + +//============================================================================== +ScrewJointUniqueProperties::ScrewJointUniqueProperties( + const Eigen::Vector3d& _axis, double _pitch) + : mAxis(_axis.normalized()), + mPitch(_pitch) +{ + // Do nothing +} + +//============================================================================== +ScrewJointProperties::ScrewJointProperties( + const SingleDofJoint::Properties& _singleDofProperties, + const ScrewJointUniqueProperties& _screwProperties) + : SingleDofJoint::Properties(_singleDofProperties), + ScrewJointUniqueProperties(_screwProperties) +{ + // Do nothing +} + +//============================================================================== +void ScrewJointAddon::setAxis(const Eigen::Vector3d& _axis) +{ + mProperties.mAxis = _axis.normalized(); + UpdateProperties(this); + incrementSkeletonVersion(); +} + +//============================================================================== +const Eigen::Vector3d& ScrewJointAddon::getAxis() const +{ + return mProperties.mAxis; +} + +} // namespace detail +} // namespace dynamics +} // namespace dart diff --git a/dart/dynamics/detail/ScrewJointProperties.h b/dart/dynamics/detail/ScrewJointProperties.h new file mode 100644 index 0000000000000..12cf0ba729890 --- /dev/null +++ b/dart/dynamics/detail/ScrewJointProperties.h @@ -0,0 +1,106 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_DYNAMICS_DETAIL_SCREWJOINTPROPERTIES_H_ +#define DART_DYNAMICS_DETAIL_SCREWJOINTPROPERTIES_H_ + +#include + +#include + +#include "dart/dynamics/SingleDofJoint.h" +#include "dart/dynamics/Addon.h" + +namespace dart { +namespace dynamics { + +class ScrewJoint; + +namespace detail { + +//============================================================================== +struct ScrewJointUniqueProperties +{ + /// Rotational axis + Eigen::Vector3d mAxis; + + /// Translational pitch + double mPitch; + + ScrewJointUniqueProperties( + const Eigen::Vector3d& _axis = Eigen::Vector3d::UnitZ(), + double _pitch = 0.1); + + virtual ~ScrewJointUniqueProperties() = default; +}; + +//============================================================================== +struct ScrewJointProperties : SingleDofJoint::Properties, + ScrewJointUniqueProperties +{ + ScrewJointProperties( + const SingleDofJoint::Properties& _singleDofProperties = + SingleDofJoint::Properties(), + const ScrewJointUniqueProperties& _screwProperties = + ScrewJointUniqueProperties()); + + virtual ~ScrewJointProperties() = default; +}; + +//============================================================================== +class ScrewJointAddon final : + public AddonWithProtectedPropertiesInSkeleton< + ScrewJointAddon, ScrewJointUniqueProperties, ScrewJoint, + detail::JointPropertyUpdate, false > +{ +public: + DART_DYNAMICS_JOINT_ADDON_CONSTRUCTOR( ScrewJointAddon ) + + void setAxis(const Eigen::Vector3d& _axis); + const Eigen::Vector3d& getAxis() const; + + DART_DYNAMICS_SET_GET_ADDON_PROPERTY(double, Pitch) +}; + +//============================================================================== +using ScrewJointBase = common::AddonManagerJoiner< + SingleDofJoint, common::SpecializedAddonManager >; + +} // namespace detail +} // namespace dynamics +} // namespace dart + +#endif // DART_DYNAMICS_DETAIL_SCREWJOINTPROPERTIES_H_ diff --git a/dart/dynamics/detail/SingleDofJointProperties.cpp b/dart/dynamics/detail/SingleDofJointProperties.cpp new file mode 100644 index 0000000000000..948c3449a62da --- /dev/null +++ b/dart/dynamics/detail/SingleDofJointProperties.cpp @@ -0,0 +1,98 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/dynamics/SingleDofJoint.h" + +namespace dart { +namespace dynamics { +namespace detail { + +//============================================================================== +SingleDofJointUniqueProperties::SingleDofJointUniqueProperties( + double _positionLowerLimit, + double _positionUpperLimit, + double _velocityLowerLimit, + double _velocityUpperLimit, + double _accelerationLowerLimit, + double _accelerationUpperLimit, + double _forceLowerLimit, + double _forceUpperLimit, + double _springStiffness, + double _restPosition, + double _dampingCoefficient, + double _coulombFriction, + bool _preserveDofName, + std::string _dofName) + : mPositionLowerLimit(_positionLowerLimit), + mPositionUpperLimit(_positionUpperLimit), + mInitialPosition(0.0), + mVelocityLowerLimit(_velocityLowerLimit), + mVelocityUpperLimit(_velocityUpperLimit), + mInitialVelocity(0.0), + mAccelerationLowerLimit(_accelerationLowerLimit), + mAccelerationUpperLimit(_accelerationUpperLimit), + mForceLowerLimit(_forceLowerLimit), + mForceUpperLimit(_forceUpperLimit), + mSpringStiffness(_springStiffness), + mRestPosition(_restPosition), + mDampingCoefficient(_dampingCoefficient), + mFriction(_coulombFriction), + mPreserveDofName(_preserveDofName), + mDofName(_dofName) +{ + // Do nothing +} + +//============================================================================== +SingleDofJointProperties::SingleDofJointProperties( + const Joint::Properties& _jointProperties, + const SingleDofJointUniqueProperties& _singleDofProperties) + : Joint::Properties(_jointProperties), + SingleDofJointUniqueProperties(_singleDofProperties) +{ + // Do nothing +} + +//============================================================================== +const std::string& SingleDofJointAddon::setDofName( + const std::string& name, bool preserveName) +{ + return getManager()->setDofName(0, name, preserveName); +} + +} // namespace detail +} // namespace dynamics +} // namespace dart diff --git a/dart/dynamics/detail/SingleDofJointProperties.h b/dart/dynamics/detail/SingleDofJointProperties.h new file mode 100644 index 0000000000000..2f0e0b1c4fc55 --- /dev/null +++ b/dart/dynamics/detail/SingleDofJointProperties.h @@ -0,0 +1,172 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_DYNAMICS_DETAIL_SINGLEDOFJOINTPROPERTIES_H_ +#define DART_DYNAMICS_DETAIL_SINGLEDOFJOINTPROPERTIES_H_ + +#include "dart/dynamics/Addon.h" +#include "dart/dynamics/Joint.h" + +namespace dart { +namespace dynamics { + +// Forward declare the SingleDofJoint class +class SingleDofJoint; + +namespace detail { + +struct SingleDofJointUniqueProperties +{ + /// Lower limit of position + double mPositionLowerLimit; + + /// Upper limit of position + double mPositionUpperLimit; + + /// Initial position + double mInitialPosition; + + /// Lower limit of velocity + double mVelocityLowerLimit; + + /// Upper limit of velocity + double mVelocityUpperLimit; + + /// Initial velocity + double mInitialVelocity; + + /// Lower limit of acceleration + double mAccelerationLowerLimit; + + /// Upper limit of acceleration + double mAccelerationUpperLimit; + + /// Lower limit of force + double mForceLowerLimit; + + /// Upper limit of force + double mForceUpperLimit; + + /// Joint spring stiffness + double mSpringStiffness; + + /// Rest position for joint spring + double mRestPosition; + + /// Joint damping coefficient + double mDampingCoefficient; + + /// Coulomb friction force + double mFriction; + + /// True if the name of this Joint's DOF is not allowed to be overwritten + bool mPreserveDofName; + + /// The name of the DegreeOfFreedom for this Joint + std::string mDofName; + + /// Constructor + SingleDofJointUniqueProperties(double _positionLowerLimit = -DART_DBL_INF, + double _positionUpperLimit = DART_DBL_INF, + double _velocityLowerLimit = -DART_DBL_INF, + double _velocityUpperLimit = DART_DBL_INF, + double _accelerationLowerLimit = -DART_DBL_INF, + double _accelerationUpperLimit = DART_DBL_INF, + double _forceLowerLimit = -DART_DBL_INF, + double _forceUpperLimit = DART_DBL_INF, + double _springStiffness = 0.0, + double _restPosition = 0.0, + double _dampingCoefficient = 0.0, + double _coulombFriction = 0.0, + bool _preserveDofName = false, + std::string _dofName = ""); + // TODO(MXG): In version 6.0, we should add mInitialPosition and + // mInitialVelocity to the constructor arguments. For now we must wait in + // order to avoid breaking the API + + virtual ~SingleDofJointUniqueProperties() = default; +}; + +//============================================================================== +struct SingleDofJointProperties : + Joint::Properties, + SingleDofJointUniqueProperties +{ + SingleDofJointProperties( + const Joint::Properties& _jointProperties = Joint::Properties(), + const SingleDofJointUniqueProperties& _singleDofProperties = + SingleDofJointUniqueProperties()); + + virtual ~SingleDofJointProperties() = default; +}; + +//============================================================================== +class SingleDofJointAddon final : + public AddonWithProtectedPropertiesInSkeleton< + SingleDofJointAddon, SingleDofJointUniqueProperties, + SingleDofJoint, common::detail::NoOp, false> +{ +public: + DART_DYNAMICS_ADDON_PROPERTY_CONSTRUCTOR( SingleDofJointAddon, &common::detail::NoOp ) + + DART_DYNAMICS_SET_GET_ADDON_PROPERTY(double, PositionLowerLimit) + DART_DYNAMICS_SET_GET_ADDON_PROPERTY(double, PositionUpperLimit) + DART_DYNAMICS_SET_GET_ADDON_PROPERTY(double, InitialPosition) + DART_DYNAMICS_SET_GET_ADDON_PROPERTY(double, VelocityLowerLimit) + DART_DYNAMICS_SET_GET_ADDON_PROPERTY(double, VelocityUpperLimit) + DART_DYNAMICS_SET_GET_ADDON_PROPERTY(double, InitialVelocity) + DART_DYNAMICS_SET_GET_ADDON_PROPERTY(double, AccelerationLowerLimit) + DART_DYNAMICS_SET_GET_ADDON_PROPERTY(double, AccelerationUpperLimit) + DART_DYNAMICS_SET_GET_ADDON_PROPERTY(double, ForceLowerLimit) + DART_DYNAMICS_SET_GET_ADDON_PROPERTY(double, ForceUpperLimit) + DART_DYNAMICS_SET_GET_ADDON_PROPERTY(double, SpringStiffness) + DART_DYNAMICS_SET_GET_ADDON_PROPERTY(double, RestPosition) + DART_DYNAMICS_SET_GET_ADDON_PROPERTY(double, DampingCoefficient) + DART_DYNAMICS_SET_GET_ADDON_PROPERTY(double, Friction) + DART_DYNAMICS_SET_GET_ADDON_PROPERTY(bool, PreserveDofName) + + const std::string& setDofName(const std::string& name, + bool preserveName = true); + DART_DYNAMICS_GET_ADDON_PROPERTY(std::string, DofName) + + friend class dart::dynamics::SingleDofJoint; +}; + +} // namespace detail +} // namespace dynamics +} // namespace dart + +#endif // DART_DYNAMICS_DETAIL_SINGLEDOFJOINTPROPERTIES_H_ diff --git a/dart/dynamics/detail/Skeleton.h b/dart/dynamics/detail/Skeleton.h index f70da03776fa8..74db73f04ccd9 100644 --- a/dart/dynamics/detail/Skeleton.h +++ b/dart/dynamics/detail/Skeleton.h @@ -37,6 +37,11 @@ #ifndef DART_DYNAMICS_DETAIL_SKELETON_H_ #define DART_DYNAMICS_DETAIL_SKELETON_H_ +#include "dart/dynamics/Skeleton.h" + +namespace dart { +namespace dynamics { + //============================================================================== template JointType* Skeleton::moveBodyNodeTree( @@ -85,4 +90,7 @@ std::pair Skeleton::createJointAndBodyNodePair( return std::pair(joint, node); } +} // namespace dynamics +} // namespace dart + #endif // DART_DYNAMICS_DETAIL_SKELETON_H_ diff --git a/dart/dynamics/detail/SpecializedNodeManager.h b/dart/dynamics/detail/SpecializedNodeManager.h new file mode 100644 index 0000000000000..a4040940e1587 --- /dev/null +++ b/dart/dynamics/detail/SpecializedNodeManager.h @@ -0,0 +1,301 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_DYNAMICS_DETAIL_SPECIALIZEDNODEMANAGER_H_ +#define DART_DYNAMICS_DETAIL_SPECIALIZEDNODEMANAGER_H_ + +#include "dart/dynamics/SpecializedNodeManager.h" + +namespace dart { +namespace dynamics { + +// This preprocessor token should only be used by the unittest that is +// responsible for checking that the specialized routines are being used to +// access specialized Addons +#ifdef DART_UNITTEST_SPECIALIZED_NODE_ACCESS +bool usedSpecializedNodeAccess; +#endif // DART_UNITTEST_SPECIALIZED_NODE_ACCESS + +//============================================================================== +template +SpecializedNodeManagerForBodyNode::SpecializedNodeManagerForBodyNode() +{ + mNodeMap[typeid( SpecNode )] = std::vector(); + mSpecNodeIterator = mNodeMap.find(typeid( SpecNode )); +} + +//============================================================================== +template +template +size_t SpecializedNodeManagerForBodyNode::getNumNodes() const +{ + return _getNumNodes(type()); +} + +//============================================================================== +template +template +NodeType* SpecializedNodeManagerForBodyNode::getNode(size_t index) +{ + return _getNode(type(), index); +} + +//============================================================================== +template +template +const NodeType* SpecializedNodeManagerForBodyNode::getNode(size_t index) const +{ + return const_cast*>(this)-> + _getNode(type(), index); +} + +//============================================================================== +template +template +constexpr bool SpecializedNodeManagerForBodyNode::isSpecializedForNode() +{ + return _isSpecializedForNode(type()); +} + +//============================================================================== +template +template +size_t SpecializedNodeManagerForBodyNode::_getNumNodes(type) const +{ + return detail::BasicNodeManagerForBodyNode::getNumNodes(); +} + +//============================================================================== +template +size_t SpecializedNodeManagerForBodyNode::_getNumNodes(type) const +{ +#ifdef DART_UNITTEST_SPECIALIZED_NODE_ACCESS + usedSpecializedNodeAccess = true; +#endif // DART_UNITTEST_SPECIALIZED_NODE_ACCESS + + return mSpecNodeIterator->second.size(); +} + +//============================================================================== +template +template +NodeType* SpecializedNodeManagerForBodyNode::_getNode(type, size_t index) +{ + return detail::BasicNodeManagerForBodyNode::getNode(index); +} + +//============================================================================== +template +SpecNode* SpecializedNodeManagerForBodyNode::_getNode(type, size_t index) +{ +#ifdef DART_UNITTEST_SPECIALIZED_NODE_ACCESS + usedSpecializedNodeAccess = true; +#endif // DART_UNITTEST_SPECIALIZED_NODE_ACCESS + + return static_cast( + getVectorObjectIfAvailable(index, mSpecNodeIterator->second)); +} + +//============================================================================== +template +template +constexpr bool SpecializedNodeManagerForBodyNode::_isSpecializedForNode(type) +{ + return false; +} + +//============================================================================== +template +constexpr bool SpecializedNodeManagerForBodyNode::_isSpecializedForNode(type) +{ + return true; +} + +//============================================================================== +template +SpecializedNodeManagerForSkeleton::SpecializedNodeManagerForSkeleton() +{ + mSpecializedTreeNodes[typeid( SpecNode )] = &mTreeSpecNodeIterators; + + mNodeNameMgrMap[typeid( SpecNode )] = common::NameManager(); + mSpecNodeNameMgrIterator = mNodeNameMgrMap.find(typeid( SpecNode )); +} + +//============================================================================== +template +template +size_t SpecializedNodeManagerForSkeleton::getNumNodes( + size_t treeIndex) const +{ + return _getNumNodes(type(), treeIndex); +} + +//============================================================================== +template +template +NodeType* SpecializedNodeManagerForSkeleton::getNode( + size_t treeIndex, size_t nodeIndex) +{ + return _getNode(type(), treeIndex, nodeIndex); +} + +//============================================================================== +template +template +const NodeType* SpecializedNodeManagerForSkeleton::getNode( + size_t treeIndex, size_t nodeIndex) const +{ + return const_cast*>(this)-> + _getNode(type(), treeIndex, nodeIndex); +} + +//============================================================================== +template +template +NodeType* SpecializedNodeManagerForSkeleton::getNode( + const std::string& name) +{ + return _getNode(type(), name); +} + +//============================================================================== +template +template +const NodeType* SpecializedNodeManagerForSkeleton::getNode( + const std::string& name) const +{ + return const_cast*>(this)-> + _getNode(type(), name); +} + +//============================================================================== +template +template +size_t SpecializedNodeManagerForSkeleton::_getNumNodes( + type, size_t treeIndex) const +{ + return detail::BasicNodeManagerForSkeleton::getNumNodes(treeIndex); +} + +//============================================================================== +template +size_t SpecializedNodeManagerForSkeleton::_getNumNodes( + type, size_t treeIndex) const +{ +#ifdef DART_UNITTEST_SPECIALIZED_NODE_ACCESS + usedSpecializedNodeAccess = true; +#endif // DART_UNITTEST_SPECIALIZED_NODE_ACCESS + + if(treeIndex >= mTreeNodeMaps.size()) + { + dterr << "[Skeleton::getNumNodes<" << typeid(SpecNode).name() << ">] " + << "Requested tree index (" << treeIndex << "), but there are only (" + << mTreeNodeMaps.size() << ") trees available\n"; + assert(false); + return 0; + } + + return mTreeSpecNodeIterators[treeIndex]->second.size(); +} + +//============================================================================== +template +template +NodeType* SpecializedNodeManagerForSkeleton::_getNode( + type, size_t treeIndex, size_t nodeIndex) +{ + return detail::BasicNodeManagerForSkeleton::getNode( + treeIndex, nodeIndex); +} + +//============================================================================== +template +SpecNode* SpecializedNodeManagerForSkeleton::_getNode( + type, size_t treeIndex, size_t nodeIndex) +{ +#ifdef DART_UNITTEST_SPECIALIZED_NODE_ACCESS + usedSpecializedNodeAccess = true; +#endif // DART_UNITTEST_SPECIALIZED_NODE_ACCESS + + if(treeIndex >= mTreeNodeMaps.size()) + { + dterr << "[Skeleton::getNode<" << typeid(SpecNode).name() << ">] " + << "Requested tree index (" << treeIndex << "), but there are only (" + << mTreeNodeMaps.size() << ") trees available\n"; + assert(false); + return nullptr; + } + + NodeMap::iterator& it = mTreeSpecNodeIterators[treeIndex]; + + if(nodeIndex >= it->second.size()) + { + dterr << "[Skeleton::getNode<" << typeid(SpecNode).name() << ">] " + << "Requested index (" << nodeIndex << ") within tree (" << treeIndex + << "), but there are only (" << it->second.size() << ") Nodes of the " + << "requested type within that tree\n"; + assert(false); + return nullptr; + } + + return static_cast(it->second[nodeIndex]); +} + +//============================================================================== +template +template +NodeType* SpecializedNodeManagerForSkeleton::_getNode( + type, const std::string& name) +{ + return detail::BasicNodeManagerForSkeleton::getNode(name); +} + +//============================================================================== +template +SpecNode* SpecializedNodeManagerForSkeleton::_getNode( + type, const std::string& name) +{ +#ifdef DART_UNITTEST_SPECIALIZED_NODE_ACCESS + usedSpecializedNodeAccess = true; +#endif // DART_UNITTEST_SPECIALIZED_NODE_ACCESS + + return static_cast(mSpecNodeNameMgrIterator->second.getObject(name)); +} + +} // namespace dynamics +} // namespace dart + +#endif // DART_DYNAMICS_DETAIL_SPECIALIZEDNODEMANAGER_H_ diff --git a/dart/dynamics/detail/TemplatedJacobianNode.h b/dart/dynamics/detail/TemplatedJacobianNode.h index 2588d1889a002..f2cff60f404a6 100644 --- a/dart/dynamics/detail/TemplatedJacobianNode.h +++ b/dart/dynamics/detail/TemplatedJacobianNode.h @@ -40,7 +40,9 @@ namespace dart { namespace dynamics { -// Documentation inherited +#include "dart/dynamics/TemplatedJacobianNode.h" + +//============================================================================== template math::Jacobian TemplatedJacobianNode::getJacobian( @@ -55,7 +57,7 @@ TemplatedJacobianNode::getJacobian( static_cast(this)->getJacobian()); } -// Documentation inherited +//============================================================================== template math::Jacobian TemplatedJacobianNode::getJacobian( @@ -67,7 +69,7 @@ TemplatedJacobianNode::getJacobian( return J; } -// Documentation inherited +//============================================================================== template math::Jacobian TemplatedJacobianNode::getJacobian( @@ -85,7 +87,7 @@ TemplatedJacobianNode::getJacobian( return math::AdTJac(T, static_cast(this)->getJacobian()); } -// Documentation inherited +//============================================================================== template math::Jacobian TemplatedJacobianNode::getWorldJacobian( @@ -98,7 +100,7 @@ TemplatedJacobianNode::getWorldJacobian( return J; } -// Documentation inherited +//============================================================================== template math::LinearJacobian TemplatedJacobianNode::getLinearJacobian( @@ -125,7 +127,7 @@ TemplatedJacobianNode::getLinearJacobian( return getTransform(_inCoordinatesOf).linear() * J.bottomRows<3>(); } -// Documentation inherited +//============================================================================== template math::LinearJacobian TemplatedJacobianNode::getLinearJacobian( @@ -144,7 +146,7 @@ TemplatedJacobianNode::getLinearJacobian( return getTransform(_inCoordinatesOf).linear() * JLinear; } -// Documentation inherited +//============================================================================== template math::AngularJacobian TemplatedJacobianNode::getAngularJacobian( @@ -169,7 +171,7 @@ TemplatedJacobianNode::getAngularJacobian( return getTransform(_inCoordinatesOf).linear() * J.topRows<3>(); } -// Documentation inherited +//============================================================================== template math::Jacobian TemplatedJacobianNode::getJacobianSpatialDeriv( @@ -182,7 +184,7 @@ TemplatedJacobianNode::getJacobianSpatialDeriv( static_cast(this)->getJacobianSpatialDeriv()); } -// Documentation inherited +//============================================================================== template math::Jacobian TemplatedJacobianNode::getJacobianSpatialDeriv( @@ -196,7 +198,7 @@ TemplatedJacobianNode::getJacobianSpatialDeriv( return J_d; } -// Documentation inherited +//============================================================================== template math::Jacobian TemplatedJacobianNode::getJacobianSpatialDeriv( @@ -213,7 +215,7 @@ TemplatedJacobianNode::getJacobianSpatialDeriv( T, static_cast(this)->getJacobianSpatialDeriv()); } -// Documentation inherited +//============================================================================== template math::Jacobian TemplatedJacobianNode::getJacobianClassicDeriv( @@ -226,7 +228,7 @@ TemplatedJacobianNode::getJacobianClassicDeriv( static_cast(this)->getJacobianClassicDeriv()); } -// Documentation inherited +//============================================================================== template math::Jacobian TemplatedJacobianNode::getJacobianClassicDeriv( @@ -251,7 +253,7 @@ TemplatedJacobianNode::getJacobianClassicDeriv( return math::AdRInvJac(_inCoordinatesOf->getWorldTransform(), J_d); } -// Documentation inherited +//============================================================================== template math::LinearJacobian TemplatedJacobianNode::getLinearJacobianDeriv( @@ -267,7 +269,7 @@ TemplatedJacobianNode::getLinearJacobianDeriv( * J_d.bottomRows<3>(); } -// Documentation inherited +//============================================================================== template math::LinearJacobian TemplatedJacobianNode::getLinearJacobianDeriv( @@ -292,7 +294,7 @@ TemplatedJacobianNode::getLinearJacobianDeriv( + J.topRows<3>().colwise().cross(w.cross(p))); } -// Documentation inherited +//============================================================================== template math::AngularJacobian TemplatedJacobianNode::getAngularJacobianDeriv( @@ -308,13 +310,12 @@ TemplatedJacobianNode::getAngularJacobianDeriv( * J_d.topRows<3>(); } -/// Default constructor. This is only a formality, because Entity and Frame -/// do not offer default constructors. +//============================================================================== template -TemplatedJacobianNode::TemplatedJacobianNode() +TemplatedJacobianNode::TemplatedJacobianNode(BodyNode* bn) : Entity(Entity::ConstructAbstract), - Frame(nullptr, ""), - Node(Node::ConstructAbstract) + Frame(Frame::ConstructAbstract), + JacobianNode(bn) { // Do nothing } diff --git a/dart/dynamics/detail/UniversalJointProperties.cpp b/dart/dynamics/detail/UniversalJointProperties.cpp new file mode 100644 index 0000000000000..598511146f930 --- /dev/null +++ b/dart/dynamics/detail/UniversalJointProperties.cpp @@ -0,0 +1,91 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/dynamics/UniversalJoint.h" + +namespace dart { +namespace dynamics { +namespace detail { + +//============================================================================== +UniversalJointUniqueProperties::UniversalJointUniqueProperties( + const Eigen::Vector3d& _axis1, const Eigen::Vector3d& _axis2) + : mAxis({_axis1.normalized(), _axis2.normalized()}) +{ + // Do nothing +} + +//============================================================================== +UniversalJointProperties::UniversalJointProperties( + const MultiDofJoint<2>::Properties& _multiDofProperties, + const UniversalJointUniqueProperties& _universalProperties) + : MultiDofJoint<2>::Properties(_multiDofProperties), + UniversalJointUniqueProperties(_universalProperties) +{ + // Do nothing +} + +//============================================================================== +void UniversalJointAddon::setAxis1(const Eigen::Vector3d& _axis) +{ + mProperties.mAxis[0] = _axis.normalized(); + UpdateProperties(this); + incrementSkeletonVersion(); +} + +//============================================================================== +const Eigen::Vector3d& UniversalJointAddon::getAxis1() const +{ + return mProperties.mAxis[0]; +} + +//============================================================================== +void UniversalJointAddon::setAxis2(const Eigen::Vector3d& _axis) +{ + mProperties.mAxis[1] = _axis.normalized(); + UpdateProperties(this); + incrementSkeletonVersion(); +} + +//============================================================================== +const Eigen::Vector3d& UniversalJointAddon::getAxis2() const +{ + return mProperties.mAxis[1]; +} + +} // namespace detail +} // namespace dynamics +} // namespace dart diff --git a/dart/dynamics/detail/UniversalJointProperties.h b/dart/dynamics/detail/UniversalJointProperties.h new file mode 100644 index 0000000000000..1245a1f294738 --- /dev/null +++ b/dart/dynamics/detail/UniversalJointProperties.h @@ -0,0 +1,102 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_DYNAMICS_DETAIL_UNIVERSALJOINTPROPERTIES_H_ +#define DART_DYNAMICS_DETAIL_UNIVERSALJOINTPROPERTIES_H_ + +#include + +#include + +#include "dart/dynamics/MultiDofJoint.h" +#include "dart/dynamics/Addon.h" + +namespace dart { +namespace dynamics { + +class UniversalJoint; + +namespace detail { + +//============================================================================== +struct UniversalJointUniqueProperties +{ + std::array mAxis; + + UniversalJointUniqueProperties( + const Eigen::Vector3d& _axis1 = Eigen::Vector3d::UnitX(), + const Eigen::Vector3d& _axis2 = Eigen::Vector3d::UnitY()); + + virtual ~UniversalJointUniqueProperties() = default; +}; + +//============================================================================== +struct UniversalJointProperties : + MultiDofJoint<2>::Properties, + UniversalJointUniqueProperties +{ + UniversalJointProperties( + const MultiDofJoint<2>::Properties& _multiDofProperties = + MultiDofJoint<2>::Properties(), + const UniversalJointUniqueProperties& _universalProperties = + UniversalJointUniqueProperties()); + + virtual ~UniversalJointProperties() = default; +}; + +//============================================================================== +class UniversalJointAddon final : + public AddonWithProtectedPropertiesInSkeleton< + UniversalJointAddon, UniversalJointUniqueProperties, UniversalJoint, + detail::JointPropertyUpdate, false > +{ +public: + DART_DYNAMICS_JOINT_ADDON_CONSTRUCTOR( UniversalJointAddon ) + void setAxis1(const Eigen::Vector3d& _axis); + const Eigen::Vector3d& getAxis1() const; + void setAxis2(const Eigen::Vector3d& _axis); + const Eigen::Vector3d& getAxis2() const; +}; + +//============================================================================== +using UniversalJointBase = common::AddonManagerJoiner< + MultiDofJoint<2>, common::SpecializedAddonManager >; + +} // namespace detail +} // namespace dynamics +} // namespace dart + +#endif // DART_DYNAMICS_DETAIL_UNIVERSALJOINTPROPERTIES_H_ diff --git a/dart/math/Helpers.h b/dart/math/Helpers.h index 5409d7c8cf0ba..0ceaa896b53c6 100644 --- a/dart/math/Helpers.h +++ b/dart/math/Helpers.h @@ -66,34 +66,19 @@ inline int delta(int _i, int _j) { return 0; } -#if defined(_MSC_VER) -// TODO: Change to constexpr once Visual Studio supports it -template inline -#else template inline constexpr -#endif int sign(T x, std::false_type) { return static_cast(0) < x; } -#if defined(_MSC_VER) -// TODO: Change to constexpr once Visual Studio supports it -template inline -#else template inline constexpr -#endif int sign(T x, std::true_type) { return (static_cast(0) < x) - (x < static_cast(0)); } -#if defined(_MSC_VER) -// TODO: Change to constexpr once Visual Studio supports it -template inline -#else template inline constexpr -#endif int sign(T x) { return sign(x, std::is_signed()); diff --git a/dart/optimizer/GradientDescentSolver.cpp b/dart/optimizer/GradientDescentSolver.cpp index 81e73d866f978..746b833fe1be7 100644 --- a/dart/optimizer/GradientDescentSolver.cpp +++ b/dart/optimizer/GradientDescentSolver.cpp @@ -181,11 +181,14 @@ bool GradientDescentSolver::solve() satisfied = false; } + dx.setZero(); Eigen::Map dxMap(dx.data(), dim); Eigen::Map gradMap(grad.data(), dim); // Compute the gradient of the objective, combined with the weighted // gradients of the softened constraints - problem->getObjective()->evalGradient(x, dxMap); + const FunctionPtr& objective = problem->getObjective(); + if(objective) + objective->evalGradient(x, dxMap); for(int i=0; i < static_cast(problem->getNumEqConstraints()); ++i) { if(std::abs(mEqConstraintCostCache[i]) < tol) @@ -274,7 +277,10 @@ bool GradientDescentSolver::solve() mLastConfig = x; problem->setOptimalSolution(x); - problem->setOptimumValue(problem->getObjective()->eval(x)); + if(problem->getObjective()) + problem->setOptimumValue(problem->getObjective()->eval(x)); + else + problem->setOptimumValue(0.0); return minimized && satisfied; } diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index 97977ec225a17..19881df178d99 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -1789,11 +1789,11 @@ SkelParser::JointPropPtr SkelParser::readEulerJoint( std::string order = getValueString(_jointElement, "axis_order"); if (order == "xyz") { - properties.mAxisOrder = dynamics::EulerJoint::AO_XYZ; + properties.mAxisOrder = dynamics::EulerJoint::AxisOrder::XYZ; } else if (order == "zyx") { - properties.mAxisOrder = dynamics::EulerJoint::AO_ZYX; + properties.mAxisOrder = dynamics::EulerJoint::AxisOrder::ZYX; } else { @@ -1889,19 +1889,19 @@ SkelParser::JointPropPtr SkelParser::readPlanarJoint( if (type == "xy") { - properties.mPlaneType = dynamics::PlanarJoint::PT_XY; + properties.mPlaneType = dynamics::PlanarJoint::PlaneType::XY; } else if (type == "yz") { - properties.mPlaneType = dynamics::PlanarJoint::PT_YZ; + properties.mPlaneType = dynamics::PlanarJoint::PlaneType::YZ; } else if (type == "zx") { - properties.mPlaneType = dynamics::PlanarJoint::PT_ZX; + properties.mPlaneType = dynamics::PlanarJoint::PlaneType::ZX; } else if (type == "arbitrary") { - properties.mPlaneType = dynamics::PlanarJoint::PT_ARBITRARY; + properties.mPlaneType = dynamics::PlanarJoint::PlaneType::ARBITRARY; tinyxml2::XMLElement* transAxis1Element = getElement(planeElement, "translation_axis1"); diff --git a/osgDart/Viewer.cpp b/osgDart/Viewer.cpp index c905a733a3510..26d3a69f2c71f 100644 --- a/osgDart/Viewer.cpp +++ b/osgDart/Viewer.cpp @@ -196,6 +196,7 @@ void ViewerAttachment::attach(Viewer* newViewer) Viewer::Viewer(const osg::Vec4& clearColor) : mImageSequenceNum(0), mImageDigits(0), + mRecording(false), mRootGroup(new osg::Group), mLightGroup(new osg::Group), mLight1(new osg::Light), diff --git a/osgDart/Viewer.h b/osgDart/Viewer.h index e59479c0db875..497beb7126639 100644 --- a/osgDart/Viewer.h +++ b/osgDart/Viewer.h @@ -134,6 +134,8 @@ class Viewer : public osgViewer::Viewer, public dart::common::Subject /// It is recommended that you end the filename with ".png". That is the type /// of file that will be produced, but the extension will not be added on /// automatically. + // TODO(MXG): Add a bool argument that instructs the viewer to append a date + // and time to the name of the file. void captureScreen(const std::string& filename); /// As the screen refreshes, save screen capture images to the specified diff --git a/osgDart/examples/osgHuboPuppet.cpp b/osgDart/examples/osgHuboPuppet.cpp index 1bbd795bb4901..34f549721a153 100644 --- a/osgDart/examples/osgHuboPuppet.cpp +++ b/osgDart/examples/osgHuboPuppet.cpp @@ -178,28 +178,18 @@ class HuboArmIK : public InverseKinematics::Analytical { public: - HuboArmIK(InverseKinematics* _ik, BodyNode* baseLink, + HuboArmIK(InverseKinematics* _ik, const std::string& baseLinkName, const Analytical::Properties& properties = Analytical::Properties()) - : Analytical(_ik, "HuboArmIK_"+baseLink->getName(), properties), - mBaseLink(BodyNodePtr(baseLink)) + : Analytical(_ik, "HuboArmIK_"+baseLinkName, properties), + configured(false), + mBaseLinkName(baseLinkName) { - configure(); + // Do nothing } std::unique_ptr clone(InverseKinematics* _newIK) const override { - BodyNode* base = mBaseLink.lock(); - if(nullptr == base) - { - dterr << "[HuboArmIK::clone] Could not clone IK because its base link " - << "can no longer be referenced\n"; - assert(false); - return nullptr; - } - - return std::unique_ptr( - new HuboArmIK(_newIK, _newIK->getNode()->getSkeleton()->getBodyNode( - base->getName()), getAnalyticalProperties())); + return std::unique_ptr(new HuboArmIK(_newIK, mBaseLinkName, getAnalyticalProperties())); } const std::vector& computeSolutions( @@ -210,17 +200,30 @@ class HuboArmIK : public InverseKinematics::Analytical if(!configured) { - dtwarn << "[HuboArmIK::computeSolutions] This analytical IK was not able " - << "to configure properly, so it will not be able to compute " - << "solutions\n"; - return mSolutions; + configure(); + + if(!configured) + { + dtwarn << "[HuboArmIK::computeSolutions] This analytical IK was not able " + << "to configure properly, so it will not be able to compute " + << "solutions\n"; + return mSolutions; + } } const BodyNodePtr& base = mBaseLink.lock(); - if(nullptr == base || nullptr == mWristEnd) + if(nullptr == base) { dterr << "[HuboArmIK::computeSolutions] Attempting to perform an IK on a " - << "limb that no longer exists!\n"; + << "limb that no longer exists [" << getMethodName() << "]!\n"; + assert(false); + return mSolutions; + } + + if(nullptr == mWristEnd) + { + dterr << "[HuboArmIK::computeSolutions] Attempting to perform IK without " + << "a wrist!\n"; assert(false); return mSolutions; } @@ -349,6 +352,9 @@ class HuboArmIK : public InverseKinematics::Analytical const std::vector& getDofs() const override { + if(!configured) + configure(); + return mDofs; } @@ -356,15 +362,16 @@ class HuboArmIK : public InverseKinematics::Analytical protected: - void configure() + void configure() const { configured = false; - const BodyNodePtr& base = mBaseLink.lock(); + mBaseLink = mIK->getNode()->getSkeleton()->getBodyNode(mBaseLinkName); + + BodyNode* base = mBaseLink.lock(); if(nullptr == base) { - dterr << "[HuboArmIK::configure] Cannot configure with a nullptr base " - << "link\n"; + dterr << "[HuboArmIK::configure] base link is a nullptr\n"; assert(false); return; } @@ -438,19 +445,21 @@ class HuboArmIK : public InverseKinematics::Analytical configured = true; } - bool configured; + mutable bool configured; - Eigen::Isometry3d shoulderTf; - Eigen::Isometry3d wristTfInv; - Eigen::Isometry3d mNodeOffsetTfInv; - double L3, L4, L5; + mutable Eigen::Isometry3d shoulderTf; + mutable Eigen::Isometry3d wristTfInv; + mutable Eigen::Isometry3d mNodeOffsetTfInv; + mutable double L3, L4, L5; - Eigen::Matrix alterantives; + mutable Eigen::Matrix alterantives; - std::vector mDofs; + mutable std::vector mDofs; - WeakBodyNodePtr mBaseLink; - JacobianNode* mWristEnd; + std::string mBaseLinkName; + mutable WeakBodyNodePtr mBaseLink; + + mutable JacobianNode* mWristEnd; }; class HuboLegIK : public InverseKinematics::Analytical @@ -458,28 +467,18 @@ class HuboLegIK : public InverseKinematics::Analytical public: /// baseLink should be Body_LHY or Body_RHY - HuboLegIK(InverseKinematics* _ik, BodyNode* baseLink, + HuboLegIK(InverseKinematics* _ik, const std::string& baseLinkName, const Analytical::Properties& properties = Analytical::Properties()) - : Analytical(_ik, "HuboLegIK_"+baseLink->getName(), properties), - mBaseLink(BodyNodePtr(baseLink)) + : Analytical(_ik, "HuboLegIK_"+baseLinkName, properties), + configured(false), + mBaseLinkName(baseLinkName) { - configure(); + // Do nothing } std::unique_ptr clone(InverseKinematics* _newIK) const override { - BodyNode* base = mBaseLink.lock(); - if(nullptr == base) - { - dterr << "[HuboLegIK::clone] Could not clone IK because its base link " - << "can no longer be referenced\n"; - assert(false); - return nullptr; - } - - return std::unique_ptr( - new HuboLegIK(_newIK, _newIK->getNode()->getSkeleton()->getBodyNode( - base->getName()), getAnalyticalProperties())); + return std::unique_ptr(new HuboLegIK(_newIK, mBaseLinkName, getAnalyticalProperties())); } const std::vector& computeSolutions( @@ -490,10 +489,15 @@ class HuboLegIK : public InverseKinematics::Analytical if(!configured) { - dtwarn << "[HuboLegIK::computeSolutions] This analytical IK was not able " - << "to configure properly, so it will not be able to compute " - << "solutions\n"; - return mSolutions; + configure(); + + if(!configured) + { + dtwarn << "[HuboLegIK::computeSolutions] This analytical IK was not able " + << "to configure properly, so it will not be able to compute " + << "solutions\n"; + return mSolutions; + } } const BodyNodePtr& base = mBaseLink.lock(); @@ -585,6 +589,9 @@ class HuboLegIK : public InverseKinematics::Analytical const std::vector& getDofs() const override { + if(!configured) + configure(); + return mDofs; } @@ -592,10 +599,12 @@ class HuboLegIK : public InverseKinematics::Analytical protected: - void configure() + void configure() const { configured = false; + mBaseLink = mIK->getNode()->getSkeleton()->getBodyNode(mBaseLinkName); + BodyNode* base = mBaseLink.lock(); if(nullptr == base) { @@ -679,17 +688,19 @@ class HuboLegIK : public InverseKinematics::Analytical configured = true; } - double L4, L5, L6; - Eigen::Isometry3d waist; - Eigen::Isometry3d hipRotation; - Eigen::Isometry3d footTfInv; - Eigen::Matrix alternatives; + mutable double L4, L5, L6; + mutable Eigen::Isometry3d waist; + mutable Eigen::Isometry3d hipRotation; + mutable Eigen::Isometry3d footTfInv; + mutable Eigen::Matrix alternatives; - std::vector mDofs; + mutable std::vector mDofs; - bool configured; + mutable bool configured; - WeakBodyNodePtr mBaseLink; + std::string mBaseLinkName; + + mutable WeakBodyNodePtr mBaseLink; }; @@ -1157,8 +1168,7 @@ void setupEndEffectors(const SkeletonPtr& hubo) l_hand->getIK(true)->setTarget(lh_target); l_hand->getIK()->useWholeBody(); - l_hand->getIK()->setGradientMethod( - hubo->getBodyNode("Body_LSP")); + l_hand->getIK()->setGradientMethod("Body_LSP"); l_hand->getIK()->getAnalytical()->setExtraDofUtilization( IK::Analytical::PRE_ANALYTICAL); @@ -1184,8 +1194,7 @@ void setupEndEffectors(const SkeletonPtr& hubo) r_hand->getIK(true)->setTarget(rh_target); r_hand->getIK()->useWholeBody(); - r_hand->getIK()->setGradientMethod( - hubo->getBodyNode("Body_RSP")); + r_hand->getIK()->setGradientMethod("Body_RSP"); r_hand->getIK()->getAnalytical()->setExtraDofUtilization( IK::Analytical::PRE_ANALYTICAL); @@ -1233,8 +1242,7 @@ void setupEndEffectors(const SkeletonPtr& hubo) l_foot->getIK()->getErrorMethod().setAngularBounds( -angularBounds, angularBounds); - l_foot->getIK()->setGradientMethod( - hubo->getBodyNode("Body_LHY")); + l_foot->getIK()->setGradientMethod("Body_LHY"); l_foot->getSupport(true)->setGeometry(foot_support); l_foot->getSupport()->setActive(); @@ -1256,8 +1264,7 @@ void setupEndEffectors(const SkeletonPtr& hubo) r_foot->getIK()->getErrorMethod().setAngularBounds( -angularBounds, angularBounds); - r_foot->getIK()->setGradientMethod( - hubo->getBodyNode("Body_RHY")); + r_foot->getIK()->setGradientMethod("Body_RHY"); r_foot->getSupport(true)->setGeometry(foot_support); r_foot->getSupport()->setActive(); @@ -1280,8 +1287,7 @@ void setupEndEffectors(const SkeletonPtr& hubo) l_peg->getIK(true)->setTarget(lp_target); - l_peg->getIK()->setGradientMethod( - hubo->getBodyNode("Body_LSP")); + l_peg->getIK()->setGradientMethod("Body_LSP"); l_peg->getIK()->getErrorMethod().setLinearBounds( -linearBounds, linearBounds); @@ -1298,8 +1304,7 @@ void setupEndEffectors(const SkeletonPtr& hubo) r_peg->getIK(true)->setTarget(rp_target); - r_peg->getIK()->setGradientMethod( - hubo->getBodyNode("Body_RSP")); + r_peg->getIK()->setGradientMethod("Body_RSP"); r_peg->getIK()->getErrorMethod().setLinearBounds( -linearBounds, linearBounds); @@ -1379,10 +1384,14 @@ int main() { dart::simulation::WorldPtr world(new dart::simulation::World); - const SkeletonPtr& hubo = createHubo(); + SkeletonPtr hubo = createHubo(); setStartupConfiguration(hubo); setupEndEffectors(hubo); + Eigen::VectorXd positions = hubo->getPositions(); + hubo = hubo->clone("hubo_copy"); + hubo->setPositions(positions); + world->addSkeleton(hubo); world->addSkeleton(createGround()); diff --git a/osgDart/render/BoxShapeNode.cpp b/osgDart/render/BoxShapeNode.cpp index faebd69fabd97..8431aa3eb1a02 100644 --- a/osgDart/render/BoxShapeNode.cpp +++ b/osgDart/render/BoxShapeNode.cpp @@ -138,7 +138,6 @@ BoxShapeGeode::BoxShapeGeode(dart::dynamics::BoxShape* shape, mDrawable(nullptr) { getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON); - getOrCreateStateSet()->setRenderingHint(osg::StateSet::TRANSPARENT_BIN); extractData(); } diff --git a/osgDart/render/CylinderShapeNode.cpp b/osgDart/render/CylinderShapeNode.cpp index 2fbeebf660e5c..469156353b3e4 100644 --- a/osgDart/render/CylinderShapeNode.cpp +++ b/osgDart/render/CylinderShapeNode.cpp @@ -141,7 +141,6 @@ CylinderShapeGeode::CylinderShapeGeode(dart::dynamics::CylinderShape* shape, mDrawable(nullptr) { getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON); - getOrCreateStateSet()->setRenderingHint(osg::StateSet::TRANSPARENT_BIN); extractData(); } diff --git a/osgDart/render/EllipsoidShapeNode.cpp b/osgDart/render/EllipsoidShapeNode.cpp index 923ad9ffb464f..fdc6e19dc2415 100644 --- a/osgDart/render/EllipsoidShapeNode.cpp +++ b/osgDart/render/EllipsoidShapeNode.cpp @@ -158,7 +158,6 @@ EllipsoidShapeGeode::EllipsoidShapeGeode(dart::dynamics::EllipsoidShape* shape, mDrawable(nullptr) { getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON); - getOrCreateStateSet()->setRenderingHint(osg::StateSet::TRANSPARENT_BIN); extractData(); } diff --git a/osgDart/render/LineSegmentShapeNode.cpp b/osgDart/render/LineSegmentShapeNode.cpp index c7ad4a71529fd..8171daa9291b3 100644 --- a/osgDart/render/LineSegmentShapeNode.cpp +++ b/osgDart/render/LineSegmentShapeNode.cpp @@ -143,7 +143,6 @@ LineSegmentShapeGeode::LineSegmentShapeGeode( mLineWidth(new osg::LineWidth) { getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON); - getOrCreateStateSet()->setRenderingHint(osg::StateSet::TRANSPARENT_BIN); getOrCreateStateSet()->setMode(GL_LIGHTING, osg::StateAttribute::OFF); extractData(true); } diff --git a/osgDart/render/MeshShapeNode.cpp b/osgDart/render/MeshShapeNode.cpp index d155d6f8ed1eb..33dc52748c643 100644 --- a/osgDart/render/MeshShapeNode.cpp +++ b/osgDart/render/MeshShapeNode.cpp @@ -385,7 +385,6 @@ MeshShapeGeode::MeshShapeGeode(dart::dynamics::MeshShape* shape, mMainNode(parentNode) { getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON); - getOrCreateStateSet()->setRenderingHint(osg::StateSet::TRANSPARENT_BIN); getOrCreateStateSet()->setAttributeAndModes(new osg::CullFace(osg::CullFace::BACK)); extractData(true); } diff --git a/osgDart/render/PlaneShapeNode.cpp b/osgDart/render/PlaneShapeNode.cpp index 008aa5be24485..7f6553cea335f 100644 --- a/osgDart/render/PlaneShapeNode.cpp +++ b/osgDart/render/PlaneShapeNode.cpp @@ -140,7 +140,6 @@ PlaneShapeGeode::PlaneShapeGeode(dart::dynamics::PlaneShape* shape, mDrawable(nullptr) { getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON); - getOrCreateStateSet()->setRenderingHint(osg::StateSet::TRANSPARENT_BIN); extractData(); } diff --git a/osgDart/render/SoftMeshShapeNode.cpp b/osgDart/render/SoftMeshShapeNode.cpp index d4af46b625e1b..47b3fc19bd43b 100644 --- a/osgDart/render/SoftMeshShapeNode.cpp +++ b/osgDart/render/SoftMeshShapeNode.cpp @@ -148,7 +148,6 @@ SoftMeshShapeGeode::SoftMeshShapeGeode( mDrawable(nullptr) { getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON); - getOrCreateStateSet()->setRenderingHint(osg::StateSet::TRANSPARENT_BIN); extractData(); } diff --git a/unittests/testAddon.cpp b/unittests/testAddon.cpp new file mode 100644 index 0000000000000..d4b1158681311 --- /dev/null +++ b/unittests/testAddon.cpp @@ -0,0 +1,509 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +// Use this preprocessor token to allow us to test that the specialized versions +// of Addon functions are being used appropriately. This preprocessor token +// should NOT be used anywhere outside of this file (testAddon.cpp). +#define DART_UNITTEST_SPECIALIZED_ADDON_ACCESS + +#include + +#include "TestHelpers.h" + +#include "dart/common/Subject.h" +#include "dart/common/sub_ptr.h" +#include "dart/common/AddonManager.h" +#include "dart/common/SpecializedAddonManager.h" + +using namespace dart::common; + +class GenericAddon : public Addon, public Subject +{ +public: + + GenericAddon(AddonManager* manager) + : Addon(manager) + { + // Do nothing + } + + std::unique_ptr cloneAddon(AddonManager* newManager) const override final + { + return std::unique_ptr(new GenericAddon(newManager)); + } + +}; + +class SpecializedAddon : public Addon, public Subject +{ +public: + + SpecializedAddon(AddonManager* manager) + : Addon(manager) + { + // Do nothing + } + + std::unique_ptr cloneAddon(AddonManager* newManager) const override final + { + return std::unique_ptr(new SpecializedAddon(newManager)); + } +}; + +template +class StatefulAddon : public Addon, public Subject +{ +public: + + class State : public Addon::State + { + public: + + State() : val(static_cast(dart::math::random(0, 100))) { } + + State(const State& other) : val(other.val) { } + + State(const T& otherVal) : val(otherVal) { } + + T val; + + std::unique_ptr clone() const override + { + return std::unique_ptr(new State(*this)); + } + + void copy(const Addon::State& anotherState) override + { + val = static_cast(anotherState).val; + } + }; + + class Properties : public Addon::Properties + { + public: + + Properties() : val(static_cast(dart::math::random(0, 100))) { } + + Properties(const Properties& other) : val(other.val) { } + + Properties(const T& otherVal) : val(otherVal) { } + + T val; + + std::unique_ptr clone() const override + { + return std::unique_ptr(new Properties(*this)); + } + + void copy(const Addon::Properties& otherProperties) override + { + val = static_cast(otherProperties).val; + } + }; + + StatefulAddon(AddonManager* mgr) + : Addon(mgr) + { + // Do nothing + } + + StatefulAddon(AddonManager* mgr, const StatefulAddon& other) + : Addon(mgr), + mState(other.mState), mProperties(other.mProperties) + { + // Do nothing + } + + StatefulAddon(AddonManager* mgr, const T& state) + : Addon(mgr), mState(state) + { + // Do nothing + } + + StatefulAddon(AddonManager* mgr, const T& state, const T& properties) + : Addon(mgr), + mState(state), mProperties(properties) + { + // Do nothing + } + + std::unique_ptr cloneAddon(AddonManager* newManager) const override final + { + return std::unique_ptr(new StatefulAddon(newManager, *this)); + } + + void setAddonState(const Addon::State& otherState) override + { + mState.copy(otherState); + } + + const Addon::State* getAddonState() const override + { + return &mState; + } + + void setAddonProperties(const Addon::Properties& someProperties) override + { + mProperties.copy(someProperties); + } + + const Addon::Properties* getAddonProperties() const override + { + return &mProperties; + } + + void randomize() + { + mState.val = static_cast(dart::math::random(0, 100)); + mProperties.val = static_cast(dart::math::random(0, 100)); + } + + State mState; + + Properties mProperties; + +}; + +typedef StatefulAddon DoubleAddon; +typedef StatefulAddon FloatAddon; +typedef StatefulAddon CharAddon; +typedef StatefulAddon IntAddon; + +class CustomSpecializedManager : public SpecializedAddonManager { }; + +TEST(Addon, Generic) +{ + AddonManager mgr; + + EXPECT_TRUE( mgr.get() == nullptr ); + + sub_ptr addon = mgr.create(); + GenericAddon* rawAddon = addon; + EXPECT_FALSE( nullptr == addon ); + EXPECT_TRUE( mgr.get() == addon ); + + GenericAddon* newAddon = mgr.create(); + EXPECT_FALSE( nullptr == newAddon ); + EXPECT_TRUE( nullptr == addon ); + EXPECT_FALSE( rawAddon == newAddon ); +} + +TEST(Addon, Specialized) +{ + usedSpecializedAddonAccess = false; + CustomSpecializedManager mgr; + + EXPECT_TRUE( mgr.get() == nullptr ); + EXPECT_TRUE( usedSpecializedAddonAccess ); usedSpecializedAddonAccess = false; +// EXPECT_TRUE( mgr.getSpecializedAddon() == nullptr ); + EXPECT_TRUE( mgr.get() == nullptr ); + EXPECT_FALSE( usedSpecializedAddonAccess ); usedSpecializedAddonAccess = false; + + sub_ptr spec = mgr.create(); + EXPECT_TRUE( usedSpecializedAddonAccess ); usedSpecializedAddonAccess = false; + SpecializedAddon* rawSpec = spec; + + sub_ptr generic = mgr.create(); + EXPECT_FALSE( usedSpecializedAddonAccess ); usedSpecializedAddonAccess = false; + GenericAddon* rawGeneric = generic; + + EXPECT_TRUE( mgr.get() == spec ); + EXPECT_TRUE( usedSpecializedAddonAccess ); usedSpecializedAddonAccess = false; +// EXPECT_TRUE( mgr.getSpecializedAddon() == spec ); + + EXPECT_TRUE( mgr.get() == generic ); + EXPECT_FALSE( usedSpecializedAddonAccess ); usedSpecializedAddonAccess = false; + +// SpecializedAddon* newSpec = mgr.createSpecializedAddon(); + SpecializedAddon* newSpec = mgr.create(); + EXPECT_TRUE( usedSpecializedAddonAccess ); usedSpecializedAddonAccess = false; + + GenericAddon* newGeneric = mgr.create(); + EXPECT_FALSE( usedSpecializedAddonAccess ); usedSpecializedAddonAccess = false; + + EXPECT_TRUE( nullptr == spec ); + EXPECT_TRUE( nullptr == generic ); + + EXPECT_FALSE( mgr.get() == rawSpec ); + EXPECT_TRUE( usedSpecializedAddonAccess ); usedSpecializedAddonAccess = false; +// EXPECT_FALSE( mgr.getSpecializedAddon() == rawSpec ); + + EXPECT_FALSE( mgr.get() == rawGeneric ); + EXPECT_FALSE( usedSpecializedAddonAccess ); usedSpecializedAddonAccess = false; + + EXPECT_TRUE( mgr.get() == newSpec ); + EXPECT_TRUE( usedSpecializedAddonAccess ); usedSpecializedAddonAccess = false; +// EXPECT_TRUE( mgr.getSpecializedAddon() == newSpec ); + + EXPECT_TRUE( mgr.get() == newGeneric ); + EXPECT_FALSE( usedSpecializedAddonAccess ); usedSpecializedAddonAccess = false; +} + +TEST(Addon, Releasing) +{ + AddonManager sender; + CustomSpecializedManager receiver; + + // ---- Test generic releases ---- + { + EXPECT_TRUE( sender.get() == nullptr ); + EXPECT_TRUE( receiver.get() == nullptr ); + + sub_ptr addon = sender.create(); + + EXPECT_TRUE( sender.get() == addon ); + EXPECT_TRUE( receiver.get() == nullptr ); + + receiver.set(sender.release()); + + EXPECT_FALSE( nullptr == addon ); + + EXPECT_TRUE( sender.get() == nullptr ); + EXPECT_TRUE( receiver.get() == addon ); + + sender.set(receiver.release()); + + EXPECT_FALSE( nullptr == addon ); + + EXPECT_TRUE( sender.get() == addon ); + EXPECT_TRUE( receiver.get() == nullptr ); + + sender.release(); + + EXPECT_TRUE( nullptr == addon ); + EXPECT_TRUE( sender.get() == nullptr ); + EXPECT_TRUE( receiver.get() == nullptr ); + } + + // ---- Test specialized releases ---- + { + EXPECT_TRUE( sender.get() == nullptr ); +// EXPECT_TRUE( receiver.getSpecializedAddon() == nullptr ); + + sub_ptr spec = sender.create(); + + EXPECT_TRUE( sender.get() == spec ); +// EXPECT_TRUE( receiver.getSpecializedAddon() == nullptr ); + +// receiver.setSpecializedAddon(sender.release()); + receiver.set(sender.release()); + + EXPECT_FALSE( nullptr == spec ); + + EXPECT_TRUE( sender.get() == nullptr ); +// EXPECT_TRUE( receiver.getSpecializedAddon() == spec ); + +// sender.set(receiver.releaseSpecializedAddon()); + sender.set(receiver.release()); + + EXPECT_FALSE( nullptr == spec ); + + EXPECT_TRUE( sender.get() == spec ); +// EXPECT_TRUE( receiver.getSpecializedAddon() == nullptr ); + + sender.release(); + + EXPECT_TRUE( nullptr == spec ); + EXPECT_TRUE( sender.get() == nullptr ); +// EXPECT_TRUE( receiver.getSpecializedAddon() == nullptr ); + } + + + // ---- Test non-moving set method ---- + { + // The set() methods being used in this block of code will make clones of + // the addons that are being passed in instead of transferring their + // ownership like the previous blocks of code were. + sub_ptr addon = sender.create(); + + // This should create a copy of the GenericAddon without taking the addon + // away from 'sender' + receiver.set(sender.get()); + + EXPECT_FALSE( nullptr == addon ); + EXPECT_FALSE( receiver.get() == addon ); + EXPECT_FALSE( receiver.get() == nullptr ); + EXPECT_TRUE( sender.get() == addon ); + + sub_ptr rec_addon = receiver.get(); + EXPECT_FALSE( nullptr == rec_addon ); + + // This should replace the first GenericAddon that was created in 'sender' + sender.set(receiver.get()); + + EXPECT_TRUE( nullptr == addon ); + EXPECT_FALSE( nullptr == rec_addon ); + EXPECT_FALSE( sender.get() == receiver.get() ); + + sub_ptr addon2 = sender.get(); + EXPECT_FALSE( nullptr == addon2 ); + + sender.set(receiver.release()); + EXPECT_TRUE( nullptr == addon2 ); + EXPECT_FALSE( nullptr == rec_addon ); + EXPECT_TRUE( receiver.get() == nullptr ); + EXPECT_TRUE( sender.get() == rec_addon ); + } +} + +template +void makeStatesDifferent(AddonT* addon, const AddonT* differentFrom) +{ + size_t counter = 0; + while( addon->mState.val == differentFrom->mState.val ) + { + addon->randomize(); + ++counter; + if(counter > 10) + { + dtwarn << "[testAddon::makeStatesDifferent] Randomization failed to make " + << "the states different after " << counter << " attempts!\n"; + break; + } + } +} + +template +void makePropertiesDifferent(AddonT* addon, const AddonT* differentFrom) +{ + size_t counter = 0; + while( addon->mProperties.val == differentFrom->mProperties.val ) + { + addon->randomize(); + ++counter; + if(counter > 10) + { + dtwarn << "[testAddon::makePropertiesDifferent] Randomization failed to " + << "make the states different after " << counter << " attempts!\n"; + break; + } + } +} + +TEST(Addon, StateAndProperties) +{ + AddonManager mgr1; + mgr1.create(); + mgr1.create(); + mgr1.create(); + mgr1.create(); + + AddonManager mgr2; + mgr2.create(); + mgr2.create(); + + // ---- Test state transfer ---- + + mgr2.setAddonStates(mgr1.getAddonStates()); + + EXPECT_EQ( mgr1.get()->mState.val, + mgr2.get()->mState.val ); + + EXPECT_EQ( mgr1.get()->mState.val, + mgr2.get()->mState.val ); + + makeStatesDifferent( mgr2.get(), mgr1.get() ); + makeStatesDifferent( mgr2.get(), mgr1.get() ); + + EXPECT_NE( mgr1.get()->mState.val, + mgr2.get()->mState.val ); + + EXPECT_NE( mgr1.get()->mState.val, + mgr2.get()->mState.val ); + + mgr1.setAddonStates( mgr2.getAddonStates() ); + + EXPECT_EQ( mgr1.get()->mState.val, + mgr2.get()->mState.val ); + + EXPECT_EQ( mgr1.get()->mState.val, + mgr2.get()->mState.val ); + + EXPECT_TRUE( nullptr == mgr2.get() ); + EXPECT_TRUE( nullptr == mgr2.get() ); + + + // ---- Test property transfer ---- + + mgr2.setAddonProperties(mgr1.getAddonProperties()); + + EXPECT_EQ( mgr1.get()->mProperties.val, + mgr2.get()->mProperties.val ); + + EXPECT_EQ( mgr1.get()->mProperties.val, + mgr2.get()->mProperties.val ); + + makePropertiesDifferent( mgr2.get(), mgr1.get() ); + makePropertiesDifferent( mgr2.get(), mgr1.get() ); + + EXPECT_NE( mgr1.get()->mProperties.val, + mgr2.get()->mProperties.val ); + + EXPECT_NE( mgr1.get()->mProperties.val, + mgr2.get()->mProperties.val ); + + mgr1.setAddonProperties( mgr2.getAddonProperties() ); + + EXPECT_EQ( mgr1.get()->mProperties.val, + mgr2.get()->mProperties.val ); + + EXPECT_EQ( mgr1.get()->mProperties.val, + mgr2.get()->mProperties.val ); + + EXPECT_TRUE( nullptr == mgr2.get() ); + EXPECT_TRUE( nullptr == mgr2.get() ); +} + +TEST(Addon, Construction) +{ + AddonManager mgr; + + mgr.create(); + + double s = dart::math::random(0, 100); + mgr.create(s); + EXPECT_EQ(mgr.get()->mState.val, s); + + double p = dart::math::random(0, 100); + mgr.create(dart::math::random(0, 100), p); + EXPECT_EQ(mgr.get()->mProperties.val, p); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/unittests/testJoints.cpp b/unittests/testJoints.cpp index 62056e3ce9e34..96e2c19889eb8 100644 --- a/unittests/testJoints.cpp +++ b/unittests/testJoints.cpp @@ -323,10 +323,10 @@ TEST_F(JOINTS, EULER_JOINT) { EulerJoint::Properties properties; - properties.mAxisOrder = EulerJoint::AO_XYZ; + properties.mAxisOrder = EulerJoint::AxisOrder::XYZ; kinematicsTest(properties); - properties.mAxisOrder = EulerJoint::AO_ZYX; + properties.mAxisOrder = EulerJoint::AxisOrder::ZYX; kinematicsTest(properties); } diff --git a/unittests/testSkelParser.cpp b/unittests/testSkelParser.cpp index 0bb8c38671f70..ec0fb73c07e08 100644 --- a/unittests/testSkelParser.cpp +++ b/unittests/testSkelParser.cpp @@ -216,10 +216,10 @@ TEST(SkelParser, PlanarJoint) EXPECT_TRUE(planarJoint3 != nullptr); EXPECT_TRUE(planarJoint4 != nullptr); - EXPECT_EQ(planarJoint1->getPlaneType(), PlanarJoint::PT_XY); - EXPECT_EQ(planarJoint2->getPlaneType(), PlanarJoint::PT_YZ); - EXPECT_EQ(planarJoint3->getPlaneType(), PlanarJoint::PT_ZX); - EXPECT_EQ(planarJoint4->getPlaneType(), PlanarJoint::PT_ARBITRARY); + EXPECT_EQ(planarJoint1->getPlaneType(), PlanarJoint::PlaneType::XY); + EXPECT_EQ(planarJoint2->getPlaneType(), PlanarJoint::PlaneType::YZ); + EXPECT_EQ(planarJoint3->getPlaneType(), PlanarJoint::PlaneType::ZX); + EXPECT_EQ(planarJoint4->getPlaneType(), PlanarJoint::PlaneType::ARBITRARY); EXPECT_EQ(planarJoint1->getTranslationalAxis1(), Eigen::Vector3d::UnitX()); EXPECT_EQ(planarJoint2->getTranslationalAxis1(), Eigen::Vector3d::UnitY()); diff --git a/unittests/testSkeleton.cpp b/unittests/testSkeleton.cpp index c3c0c9cc13f25..76c3b3e7535db 100644 --- a/unittests/testSkeleton.cpp +++ b/unittests/testSkeleton.cpp @@ -560,41 +560,75 @@ TEST(Skeleton, Persistence) EXPECT_TRUE(weakSkel.lock() == nullptr); } +class GenericNode final : public dart::dynamics::Node, + public AccessoryNode +{ +public: + + GenericNode(BodyNode* bn, const std::string& name) + : Node(bn), mName(name) { } + + const std::string& setName(const std::string& newName) override + { + mName = registerNameChange(newName); + return mName; + } + + const std::string& getName() const override + { + return mName; + } + +protected: + + Node* cloneNode(BodyNode* bn) const override + { + return new GenericNode(bn, mName); + } + + std::string mName; +}; + TEST(Skeleton, NodePersistence) { SkeletonPtr skel = Skeleton::create(); skel->createJointAndBodyNodePair(nullptr); + //-------------------------------------------------------------------------- + // Testing EndEffector, which is a specialized Node type + //-------------------------------------------------------------------------- { - EndEffector* manip = - skel->getBodyNode(0)->createEndEffector(Entity::Properties("manip")); + EndEffector* manip = skel->getBodyNode(0)->createEndEffector("manip"); - EXPECT_TRUE(skel->getEndEffector("manip") == manip); - EXPECT_TRUE(skel->getEndEffector(0) == manip); - EXPECT_TRUE(skel->getBodyNode(0)->getEndEffector(0) == manip); + // Test both methods of adding a Support to an EndEffector + manip->create(); + manip->createSupport(); + + EXPECT_EQ(skel->getEndEffector("manip"), manip); + EXPECT_EQ(skel->getEndEffector(0), manip); +// EXPECT_EQ(skel->getBodyNode(0)->getEndEffector(0), manip); WeakEndEffectorPtr weakManip = manip; - EXPECT_FALSE(weakManip.lock() == nullptr); + EXPECT_NE(weakManip.lock(), nullptr); manip->remove(); // The Node has been removed, and no strong reference to it exists, so it // should be gone from the Skeleton - EXPECT_TRUE(skel->getEndEffector("manip") == nullptr); - EXPECT_TRUE(skel->getNumEndEffectors() == 0); - EXPECT_TRUE(skel->getBodyNode(0)->getNumEndEffectors() == 0); + EXPECT_EQ(skel->getEndEffector("manip"), nullptr); + EXPECT_EQ(skel->getNumEndEffectors(), 0u); +// EXPECT_EQ(skel->getBodyNode(0)->getNumEndEffectors(), 0u); - EXPECT_TRUE(weakManip.lock() == nullptr); + EXPECT_EQ(weakManip.lock(), nullptr); } { - EndEffector* manip = - skel->getBodyNode(0)->createEndEffector(Entity::Properties("manip")); + EndEffector* manip = skel->getBodyNode(0)->createEndEffector("manip"); - EXPECT_TRUE(skel->getEndEffector("manip") == manip); - EXPECT_TRUE(skel->getEndEffector(0) == manip); - EXPECT_TRUE(skel->getBodyNode(0)->getEndEffector(0) == manip); + EXPECT_EQ(skel->getEndEffector("manip"), manip); + EXPECT_EQ(skel->getEndEffector(0), manip); +// EXPECT_EQ(skel->getBodyNode(0)->getEndEffector(0), manip); EndEffectorPtr strongManip = manip; WeakEndEffectorPtr weakManip = strongManip; @@ -603,23 +637,139 @@ TEST(Skeleton, NodePersistence) manip->remove(); - // The Node has been removed, but a strong reference to it still exists, so - // it will remain in the Skeleton for now - EXPECT_TRUE(skel->getEndEffector("manip") == manip); - EXPECT_TRUE(skel->getEndEffector(0) == manip); - EXPECT_TRUE(skel->getBodyNode(0)->getEndEffector(0) == manip); + // The Node has been removed, so no reference to it will exist in the + // Skeleton +#ifdef NDEBUG // Release Mode + EXPECT_NE(skel->getEndEffector("manip"), manip); + EXPECT_EQ(skel->getEndEffector("manip"), nullptr); - EXPECT_FALSE(weakManip.lock() == nullptr); + EXPECT_NE(skel->getEndEffector(0), manip); + EXPECT_EQ(skel->getEndEffector(0), nullptr); +#endif // Release Mode + +#ifdef NDEBUG // Release Mode + // But it will not remain in the BodyNode's indexing. + // Note: We should only run this test in release mode, because otherwise it + // will trigger an assertion. +// EXPECT_NE(skel->getBodyNode(0)->getEndEffector(0), manip); +// EXPECT_EQ(skel->getBodyNode(0)->getEndEffector(0), nullptr); +#endif // Release Mode + + EXPECT_NE(weakManip.lock(), nullptr); strongManip = nullptr; // The Node has been removed, and no strong reference to it exists any // longer, so it should be gone from the Skeleton - EXPECT_TRUE(skel->getEndEffector("manip") == nullptr); - EXPECT_TRUE(skel->getNumEndEffectors() == 0); - EXPECT_TRUE(skel->getBodyNode(0)->getNumEndEffectors() == 0); + EXPECT_EQ(skel->getEndEffector("manip"), nullptr); + EXPECT_EQ(skel->getNumEndEffectors(), 0u); +// EXPECT_EQ(skel->getBodyNode(0)->getNumEndEffectors(), 0u); + + EXPECT_EQ(weakManip.lock(), nullptr); + } + + using GenericNodePtr = TemplateNodePtr; + using WeakGenericNodePtr = TemplateWeakNodePtr; + //-------------------------------------------------------------------------- + // Testing GenericNode, which is NOT a specialized Node type + //-------------------------------------------------------------------------- + { + GenericNode* node = + skel->getBodyNode(0)->createNode("node"); + + EXPECT_EQ(skel->getNode("node"), node); + EXPECT_EQ(skel->getNode(0), node); + EXPECT_EQ(skel->getBodyNode(0)->getNode(0), node); + + WeakGenericNodePtr weakNode = node; + + EXPECT_NE(weakNode.lock(), nullptr); + + node->remove(); - EXPECT_TRUE(weakManip.lock() == nullptr); + // The Node has been removed, and no strong reference to it exists, so it + // should be gone from the Skeleton + EXPECT_EQ(skel->getNode("node"), nullptr); + EXPECT_EQ(skel->getNumNodes(), 0u); + EXPECT_EQ(skel->getBodyNode(0)->getNumNodes(), 0u); + + EXPECT_EQ(weakNode.lock(), nullptr); + } + + { + GenericNode* node = + skel->getBodyNode(0)->createNode("node"); + + EXPECT_EQ(skel->getNode("node"), node); + EXPECT_EQ(skel->getNode(0), node); + EXPECT_EQ(skel->getBodyNode(0)->getNode(0), node); + + GenericNodePtr strongNode = node; + WeakGenericNodePtr weakNode = strongNode; + + EXPECT_FALSE(weakNode.lock() == nullptr); + + node->remove(); + + // The Node has been removed, so no reference to it will exist in the + // Skeleton +#ifdef NDEBUG // Release Mode + EXPECT_NE(skel->getNode("node"), node); + EXPECT_EQ(skel->getNode("node"), nullptr); + + EXPECT_NE(skel->getNode(0), node); + EXPECT_EQ(skel->getNode(0), nullptr); +#endif // Release Mode + +#ifdef NDEBUG // Release Mode + // But it will not remain in the BodyNode's indexing. + // Note: We should only run this test in release mode, because otherwise it + // will trigger an assertion. + EXPECT_NE(skel->getBodyNode(0)->getNode(0), node); + EXPECT_EQ(skel->getBodyNode(0)->getNode(0), nullptr); +#endif // Release Mode + + EXPECT_NE(weakNode.lock(), nullptr); + + strongNode = nullptr; + + // The Node has been removed, and no strong reference to it exists any + // longer, so it should be gone from the Skeleton + EXPECT_EQ(skel->getNode("node"), nullptr); + EXPECT_EQ(skel->getNumNodes(), 0u); + EXPECT_EQ(skel->getBodyNode(0)->getNumNodes(), 0u); + + EXPECT_EQ(weakNode.lock(), nullptr); + } +} + +TEST(Skeleton, CloneNodeOrdering) +{ + // This test checks that the ordering of Nodes in a cloned Skeleton will match + // the ordering of Nodes in the original that was copied. + + SkeletonPtr skel = Skeleton::create(); + skel->createJointAndBodyNodePair(nullptr); + skel->createJointAndBodyNodePair(nullptr); + skel->createJointAndBodyNodePair(nullptr); + + // Add Nodes in the reverse order, so that their indexing is different from + // the BodyNodes they are attached to + for(int i=skel->getNumBodyNodes()-1; i > 0; --i) + { + skel->getBodyNode(i)->createEndEffector("manip_"+std::to_string(i)); + } + + skel->getBodyNode(1)->createEndEffector("other_manip"); + skel->getBodyNode(0)->createEndEffector("another_manip"); + skel->getBodyNode(2)->createEndEffector("yet_another_manip"); + + SkeletonPtr clone = skel->clone(); + + for(size_t i=0; i < skel->getNumEndEffectors(); ++i) + { + EXPECT_EQ(skel->getEndEffector(i)->getName(), + clone->getEndEffector(i)->getName()); } } @@ -1044,6 +1194,35 @@ TEST(Skeleton, Group) EXPECT_EQ(group->getDof(i), dofs[i]); } +TEST(Skeleton, Configurations) +{ + SkeletonPtr twoLink = createTwoLinkRobot(Vector3d::Ones(), DOF_YAW, + Vector3d::Ones(), DOF_ROLL); + + SkeletonPtr threeLink = createThreeLinkRobot(Vector3d::Ones(), DOF_PITCH, + Vector3d::Ones(), DOF_ROLL, + Vector3d::Ones(), DOF_YAW); + + Skeleton::Configuration c2 = twoLink->getConfiguration(); + Skeleton::Configuration c3 = threeLink->getConfiguration(); + + EXPECT_FALSE(c2 == c3); + EXPECT_TRUE(c2 == c2); + EXPECT_TRUE(c3 == c3); + EXPECT_TRUE(c2 != c3); + + twoLink->setPosition(0, 1.0); + EXPECT_FALSE(c2 == twoLink->getConfiguration()); + + threeLink->setPosition(1, 2.0); + EXPECT_FALSE(c3 == twoLink->getConfiguration()); + + c2 = twoLink->getConfiguration(Skeleton::CONFIG_VELOCITIES); + EXPECT_TRUE(c2.mPositions.size() == 0); + EXPECT_TRUE(c2.mVelocities.size() == static_cast(twoLink->getNumDofs())); + EXPECT_TRUE(c2.mAccelerations.size() == 0); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv);