diff --git a/extended_robot_hw_tests/CMakeLists.txt b/extended_robot_hw_tests/CMakeLists.txt new file mode 100644 index 0000000..d758901 --- /dev/null +++ b/extended_robot_hw_tests/CMakeLists.txt @@ -0,0 +1,67 @@ +cmake_minimum_required(VERSION 2.8.3) +project(extended_robot_hw_tests) + +find_package(catkin REQUIRED + COMPONENTS + cmake_modules + pluginlib + transmission_interface) + +find_package(TinyXML REQUIRED) + +catkin_package( + LIBRARIES + ${PROJECT_NAME}_loader_plugins + INCLUDE_DIRS + include + CATKIN_DEPENDS + pluginlib + DEPENDS + TinyXML +) + + +########### +## Build ## +########### + +include_directories(include) +include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${TinyXML_INCLUDE_DIRS}) + +add_library(${PROJECT_NAME}_loader_plugins + src/extended_simple_transmission_loader.cpp + src/foo_joint_interface_provider.cpp) +# src/bar_joint_interface_provider.cpp) + +target_link_libraries(${PROJECT_NAME}_loader_plugins ${catkin_LIBRARIES}) + + + +install(FILES extended_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +############# +## Testing ## +############# + +if(CATKIN_ENABLE_TESTING) + + find_package(catkin REQUIRED + COMPONENTS + hardware_interface + resource_retriever + transmission_interface + urdf + ) + + find_package(rostest REQUIRED) + + include_directories(SYSTEM ${catkin_INCLUDE_DIRS}) + + catkin_add_gtest(extended_robot_hw_test test/extended_robot_hw_test.cpp) + + target_link_libraries(extended_robot_hw_test + ${catkin_LIBRARIES}) + +endif() + diff --git a/extended_robot_hw_tests/extended_plugins.xml b/extended_robot_hw_tests/extended_plugins.xml new file mode 100644 index 0000000..2edfa7e --- /dev/null +++ b/extended_robot_hw_tests/extended_plugins.xml @@ -0,0 +1,11 @@ + + + + + Load from a URDF description the configuration of a extended simple transmission. + + + + diff --git a/extended_robot_hw_tests/include/extended_robot_hw_tests/extended_joint_interface.h b/extended_robot_hw_tests/include/extended_robot_hw_tests/extended_joint_interface.h new file mode 100644 index 0000000..000a03c --- /dev/null +++ b/extended_robot_hw_tests/include/extended_robot_hw_tests/extended_joint_interface.h @@ -0,0 +1,49 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2019, PAL Robotics S.L. +// +// 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. +// * Neither the name of hiDOF, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// 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 OWNER 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. +////////////////////////////////////////////////////////////////////////////// + +/// \author Jordán Palacios + +#ifndef HARDWARE_INTERFACE_EXTENDED_JOINT_INTERFACE_H +#define HARDWARE_INTERFACE_EXTENDED_JOINT_INTERFACE_H + +#include + +namespace hardware_interface +{ + +// joint foo bar command interfaces +class FooJointInterface : public JointCommandInterface +{ +}; + +class BarJointInterface : public JointCommandInterface +{ +}; + +} // namespace hardware_interface + +#endif diff --git a/extended_robot_hw_tests/include/extended_robot_hw_tests/extended_simple_transmission.h b/extended_robot_hw_tests/include/extended_robot_hw_tests/extended_simple_transmission.h new file mode 100644 index 0000000..e10838c --- /dev/null +++ b/extended_robot_hw_tests/include/extended_robot_hw_tests/extended_simple_transmission.h @@ -0,0 +1,111 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2019, PAL Robotics S.L. +// +// 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. +// * Neither the name of PAL Robotics S.L. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// 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 OWNER 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. +////////////////////////////////////////////////////////////////////////////// + +/// \author Jordán Palacios + +#ifndef TRANSMISSION_INTERFACE_EXTENDED_SIMPLE_TRANSMISSION_H +#define TRANSMISSION_INTERFACE_EXTENDED_SIMPLE_TRANSMISSION_H + +#include +#include +#include + +#include + +namespace transmission_interface +{ +struct FooActuatorData +{ + std::vector foo; +}; + +struct BarActuatorData +{ + std::vector bar; +}; + +struct FooJointData +{ + std::vector foo; +}; + +struct BarJointData +{ + std::vector bar; +}; + +class ExtendedSimpleTransmission : public SimpleTransmission +{ +public: + ExtendedSimpleTransmission(const double reduction, const double joint_offset = 0.0) + : SimpleTransmission(reduction, joint_offset), reduction_(reduction) + { + } + + virtual ~ExtendedSimpleTransmission() + { + } + + // bring base functions into scope + using SimpleTransmission::actuatorToJoint; + using SimpleTransmission::jointToActuator; + + void actuatorToJoint(const FooActuatorData& act_data, FooJointData& jnt_data) + { + assert(numActuators() == act_data.foo.size() && numJoints() == jnt_data.foo.size()); + assert(act_data.foo[0] && jnt_data.foo[0]); + *jnt_data.foo[0] = *act_data.foo[0] * reduction_; + } + + void actuatorToJoint(const BarActuatorData& act_data, BarJointData& jnt_data) + { + assert(numActuators() == act_data.bar.size() && numJoints() == jnt_data.bar.size()); + assert(act_data.bar[0] && jnt_data.bar[0]); + *jnt_data.bar[0] = *act_data.bar[0] * reduction_; + } + + void jointToActuator(const FooJointData& jnt_data, FooActuatorData& act_data) + { + assert(numActuators() == act_data.foo.size() && numJoints() == jnt_data.foo.size()); + assert(act_data.foo[0] && jnt_data.foo[0]); + *act_data.foo[0] = *jnt_data.foo[0] * reduction_; + } + + void jointToActuator(const BarJointData& jnt_data, BarActuatorData& act_data) + { + assert(numActuators() == act_data.bar.size() && numJoints() == jnt_data.bar.size()); + assert(act_data.bar[0] && jnt_data.bar[0]); + *act_data.bar[0] = *jnt_data.bar[0] * reduction_; + } + +private: + double reduction_; +}; + +} // namespace transmission_interface + +#endif // TRANSMISSION_INTERFACE_EXTENDED_SIMPLE_TRANSMISSION_H diff --git a/extended_robot_hw_tests/include/extended_robot_hw_tests/extended_simple_transmission_loader.h b/extended_robot_hw_tests/include/extended_robot_hw_tests/extended_simple_transmission_loader.h new file mode 100644 index 0000000..6cfc83e --- /dev/null +++ b/extended_robot_hw_tests/include/extended_robot_hw_tests/extended_simple_transmission_loader.h @@ -0,0 +1,45 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2019, PAL Robotics S.L. +// +// 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. +// * Neither the name of PAL Robotics S.L. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// 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 OWNER 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. +////////////////////////////////////////////////////////////////////////////// + +/// \author Jordán Palacios + +#ifndef TRANSMISSION_INTERFACE_EXTENDED_SIMPLE_TRANSMISSION_LOADER_H +#define TRANSMISSION_INTERFACE_EXTENDED_SIMPLE_TRANSMISSION_LOADER_H + +#include + +namespace transmission_interface +{ +class ExtendedSimpleTransmissionLoader : public TransmissionLoader +{ +public: + TransmissionSharedPtr load(const TransmissionInfo& transmission_info); +}; + +} // namespace transmission_interface + +#endif diff --git a/extended_robot_hw_tests/include/extended_robot_hw_tests/foo_joint_interface_provider.h b/extended_robot_hw_tests/include/extended_robot_hw_tests/foo_joint_interface_provider.h new file mode 100644 index 0000000..119542d --- /dev/null +++ b/extended_robot_hw_tests/include/extended_robot_hw_tests/foo_joint_interface_provider.h @@ -0,0 +1,61 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2019, PAL Robotics S.L. +// +// 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. +// * Neither the name of PAL Robotics S.L. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// 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 OWNER 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. +////////////////////////////////////////////////////////////////////////////// + +/// \author Jordán Palacios + +#ifndef TRANSMISSION_INTERFACE_FOO_JOINT_INTERFACE_PROVIDER_H +#define TRANSMISSION_INTERFACE_FOO_JOINT_INTERFACE_PROVIDER_H + +// ros_control +#include +#include +#include + +namespace transmission_interface +{ +class FooJointInterfaceProvider : public JointStateInterfaceProvider +{ +public: + bool updateJointInterfaces(const TransmissionInfo& transmission_info, + hardware_interface::RobotHW* robot_hw, + JointInterfaces& joint_interfaces, + RawJointDataMap& raw_joint_data_map); + +protected: + bool getJointCommandData(const TransmissionInfo& transmission_info, + const RawJointDataMap& raw_joint_data_map, JointData& jnt_cmd_data); + + bool getActuatorCommandData(const TransmissionInfo& transmission_info, + hardware_interface::RobotHW* robot_hw, ActuatorData& act_cmd_data); + + bool registerTransmission(TransmissionLoaderData& loader_data, + TransmissionHandleData& handle_data); +}; + +} // namespace transmission_interface + +#endif diff --git a/extended_robot_hw_tests/package.xml b/extended_robot_hw_tests/package.xml new file mode 100644 index 0000000..c3b6ecc --- /dev/null +++ b/extended_robot_hw_tests/package.xml @@ -0,0 +1,27 @@ + + + extended_robot_hw_tests + 0.15.1 + Robot HW Extension tests + Bence Magyar + + BSD + + catkin + + pluginlib + transmission_interface + + pluginlib + + hardware_interface + resource_retriever + rostest + rosunit + urdf + + + + + + diff --git a/extended_robot_hw_tests/src/extended_simple_transmission_loader.cpp b/extended_robot_hw_tests/src/extended_simple_transmission_loader.cpp new file mode 100644 index 0000000..2230326 --- /dev/null +++ b/extended_robot_hw_tests/src/extended_simple_transmission_loader.cpp @@ -0,0 +1,99 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2019, PAL Robotics S.L. +// +// 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. +// * Neither the name of PAL Robotics S.L. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// 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 OWNER 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. +////////////////////////////////////////////////////////////////////////////// + +/// \author Jordán Palacios + +#include +#include +#include + +#include + +#include + +namespace transmission_interface +{ +TransmissionSharedPtr ExtendedSimpleTransmissionLoader::load(const TransmissionInfo& transmission_info) +{ + // Transmission should contain only one actuator/joint + if (!checkActuatorDimension(transmission_info, 1)) + { + return TransmissionSharedPtr(); + } + if (!checkJointDimension(transmission_info, 1)) + { + return TransmissionSharedPtr(); + } + + // Parse actuator and joint xml elements + TiXmlElement actuator_el = loadXmlElement(transmission_info.actuators_.front().xml_element_); + TiXmlElement joint_el = loadXmlElement(transmission_info.joints_.front().xml_element_); + + // Parse required mechanical reduction + double reduction = 0.0; + const ParseStatus reduction_status = getActuatorReduction( + actuator_el, transmission_info.actuators_.front().name_, transmission_info.name_, + true, // Required + reduction); + if (reduction_status != SUCCESS) + { + return TransmissionSharedPtr(); + } + + // Parse optional joint offset. Even though it's optional --and to avoid surprises-- we + // fail if the element is specified but is of the wrong type + double joint_offset = 0.0; + const ParseStatus joint_offset_status = + getJointOffset(joint_el, transmission_info.joints_.front().name_, transmission_info.name_, + false, // Optional + joint_offset); + if (joint_offset_status == BAD_TYPE) + { + return TransmissionSharedPtr(); + } + + // Transmission instance + try + { + TransmissionSharedPtr transmission(new ExtendedSimpleTransmission(reduction, joint_offset)); + return transmission; + } + catch (const TransmissionInterfaceException& ex) + { + using hardware_interface::internal::demangledTypeName; + ROS_ERROR_STREAM_NAMED("parser", "Failed to construct transmission '" + << transmission_info.name_ << "' of type '" + << demangledTypeName() + << "'. " << ex.what()); + return TransmissionSharedPtr(); + } +} + +} // namespace transmission_interface + +PLUGINLIB_EXPORT_CLASS(transmission_interface::ExtendedSimpleTransmissionLoader, + transmission_interface::TransmissionLoader) diff --git a/extended_robot_hw_tests/src/foo_joint_interface_provider.cpp b/extended_robot_hw_tests/src/foo_joint_interface_provider.cpp new file mode 100644 index 0000000..e1ff471 --- /dev/null +++ b/extended_robot_hw_tests/src/foo_joint_interface_provider.cpp @@ -0,0 +1,149 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2019, PAL Robotics S.L. +// +// 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. +// * Neither the name of PAL Robotics S.L. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// 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 OWNER 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. +////////////////////////////////////////////////////////////////////////////// + +/// \author Jordán Palacios + +#include +#include + +#include + +namespace transmission_interface +{ + +bool FooJointInterfaceProvider::updateJointInterfaces(const TransmissionInfo& transmission_info, + hardware_interface::RobotHW* robot_hw, + JointInterfaces& joint_interfaces, + RawJointDataMap& raw_joint_data_map) +{ + // Setup joint state interface first + if (!JointStateInterfaceProvider::updateJointInterfaces(transmission_info, + robot_hw, + joint_interfaces, + raw_joint_data_map)) {return false;} + + // If interface does not yet exist in robot hardware abstraction, add it and use internal data structures + using hardware_interface::FooJointInterface; + if (!robot_hw->get()) + { + robot_hw->registerInterface(&joint_interfaces.foo_joint_interface); + } + FooJointInterface& interface = *(robot_hw->get()); + + // Register joints on the hardware interface + for (const JointInfo& joint_info : transmission_info.joints_) + { + const std::string& name = joint_info.name_; + + // Do nothing if joint already exists on the hardware interface + if (hasResource(name, interface)) {continue;} + + // Update hardware interface + using hardware_interface::JointHandle; + RawJointData& raw_joint_data = raw_joint_data_map[name]; + JointHandle handle(joint_interfaces.joint_state_interface.getHandle(joint_info.name_), + &raw_joint_data.foo_cmd); + interface.registerHandle(handle); + } + return true; +} + +bool FooJointInterfaceProvider::getJointCommandData(const TransmissionInfo& transmission_info, + const RawJointDataMap& raw_joint_data_map, + JointData& jnt_cmd_data) +{ + const unsigned int dim = transmission_info.joints_.size(); + jnt_cmd_data.foo.resize(dim); + + for (unsigned int i = 0; i < dim; ++i) + { + const std::string& joint_name = transmission_info.joints_[i].name_; + RawJointDataMap::const_iterator raw_joint_data_it = raw_joint_data_map.find(joint_name); + if (raw_joint_data_it == raw_joint_data_map.end()) {return false;} // Joint name not found! + const RawJointData& raw_joint_data = raw_joint_data_it->second; + + // TODO: Get rid of these const casts! + jnt_cmd_data.foo[i] = const_cast(&(raw_joint_data.foo_cmd)); + } + + return true; +} + +bool FooJointInterfaceProvider::getActuatorCommandData(const TransmissionInfo& transmission_info, + hardware_interface::RobotHW* robot_hw, + ActuatorData& act_cmd_data) +{ + using hardware_interface::FooActuatorInterface; + using hardware_interface::ActuatorHandle; + + // Get handles to all required actuators + std::vector handles; + if (!this->getActuatorHandles(transmission_info.actuators_, + robot_hw, + handles)) {return false;} + + // Populate actuator data + const unsigned int dim = transmission_info.actuators_.size(); + act_cmd_data.foo.resize(dim); + + for (unsigned int i = 0; i < dim; ++i) + { + // TODO: Get rid of these const casts! + act_cmd_data.foo[i] = const_cast(handles[i].getCommandPtr()); + } + return true; +} + +bool FooJointInterfaceProvider::registerTransmission(TransmissionLoaderData& loader_data, + TransmissionHandleData& handle_data) +{ + // Setup joint state interface first (if not yet done) + if (!hasResource(handle_data.name, loader_data.transmission_interfaces.act_to_jnt_state)) + { + if (!JointStateInterfaceProvider::registerTransmission(loader_data, handle_data)) {return false;} + } + + // If command interface does not yet exist in the robot transmissions, add it and use internal data structures + if (!loader_data.robot_transmissions->get()) + { + loader_data.robot_transmissions->registerInterface(&loader_data.transmission_interfaces.jnt_to_act_eff_cmd); + } + JointToActuatorFooInterface& interface = *(loader_data.robot_transmissions->get()); + + // Setup command interface + JointToActuatorFooHandle handle(handle_data.name, + handle_data.transmission.get(), + handle_data.act_cmd_data, + handle_data.jnt_cmd_data); + interface.registerHandle(handle); + return true; +} + +} // namespace + +PLUGINLIB_EXPORT_CLASS(transmission_interface::FooJointInterfaceProvider, + transmission_interface::RequisiteProvider) diff --git a/extended_robot_hw_tests/test/extended_actuator_interface.h b/extended_robot_hw_tests/test/extended_actuator_interface.h new file mode 100644 index 0000000..26f446c --- /dev/null +++ b/extended_robot_hw_tests/test/extended_actuator_interface.h @@ -0,0 +1,123 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2019, PAL Robotics S.L. +// +// 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. +// * Neither the name of hiDOF, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// 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 OWNER 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. +////////////////////////////////////////////////////////////////////////////// + +/// \author Jordán Palacios + +#ifndef HARDWARE_INTERFACE_EXTENDED_ACTUATOR_INTERFACE_H +#define HARDWARE_INTERFACE_EXTENDED_ACTUATOR_INTERFACE_H + +#include +#include + +namespace hardware_interface +{ + +// foo handle +class FooActuatorStateHandle +{ +public: + FooActuatorStateHandle() = default; + + FooActuatorStateHandle(const double* foo) : foo_(foo) + { + } + + virtual ~FooActuatorStateHandle() + { + } + + virtual double getFoo() const + { + assert(foo_); + return *foo_; + } + +protected: + const double* foo_ = nullptr; +}; + +// bar handle +class BarActuatorStateHandle +{ +public: + BarActuatorStateHandle() = default; + + BarActuatorStateHandle(const double* bar) : bar_(bar) + { + } + + virtual ~BarActuatorStateHandle() + { + } + + virtual double getBar() const + { + assert(bar_); + return *bar_; + } + +protected: + const double* bar_ = nullptr; +}; + +// actuator state handle + foo + bar +class ExtendedActuatorStateHandle : public ActuatorStateHandle, + public FooActuatorStateHandle, + public BarActuatorStateHandle +{ +public: + ExtendedActuatorStateHandle() = default; + + ExtendedActuatorStateHandle(const std::string& name, const double* pos, const double* vel, + const double* eff, const double* foo, const double* bar) + : ActuatorStateHandle(name, pos, vel, eff) + , FooActuatorStateHandle(foo) + , BarActuatorStateHandle(bar) + { + } + + std::string getName() const + { + return ActuatorStateHandle::getName(); + } +}; + +class ExtendedActuatorStateInterface : public HardwareResourceManager +{ +}; + +// actuator foo bar command interfaces +class FooActuatorInterface : public ActuatorCommandInterface +{ +}; +class BarActuatorInterface : public ActuatorCommandInterface +{ +}; + +} // namespace hardware_interface + +#endif diff --git a/extended_robot_hw_tests/test/extended_robot_hw_test.cpp b/extended_robot_hw_tests/test/extended_robot_hw_test.cpp new file mode 100644 index 0000000..de4dd34 --- /dev/null +++ b/extended_robot_hw_tests/test/extended_robot_hw_test.cpp @@ -0,0 +1,212 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2019, PAL Robotics S.L. +// +// 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. +// * Neither the name of hiDOF, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// 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 OWNER 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. +////////////////////////////////////////////////////////////////////////////// + +/// \author Jordán Palacios + +#include "extended_actuator_interface.h" +#include "extended_joint_interface.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include + +struct ExtendedActuatorData +{ + ExtendedActuatorData() + : pos_data(0.0) + , vel_data(0.0) + , eff_data(0.0) + , foo_data(0.0) + , bar_data(0.0) + , pos_cmd(0.0) + , vel_cmd(0.0) + , eff_cmd(0.0) + , foo_cmd(0.0) + , bar_cmd(0.0) + { + } + + double pos_data; + double vel_data; + double eff_data; + double foo_data; + double bar_data; + + double pos_cmd; + double vel_cmd; + double eff_cmd; + double foo_cmd; + double bar_cmd; +}; + +class ExtendedRobotHW : public hardware_interface::RobotHW +{ +public: + ExtendedRobotHW() + : hardware_interface::RobotHW(), act_name_("extended_actuator") + { + } + + virtual ~ExtendedRobotHW() + { + } + + bool init_acts(ExtendedActuatorData* act_data) + { + // for simplicity, robot with one actuator + + // actuator state interface + act_state_.registerHandle(hardware_interface::ExtendedActuatorStateHandle( + act_name_, &act_data->pos_data, &act_data->vel_data, &act_data->eff_data, + &act_data->foo_data, &act_data->bar_data)); + + /// @note should we use a new 'ExtendedActuatorHandle' here? + + // actuator command interfaces + act_pos_cmd_.registerHandle(hardware_interface::ActuatorHandle( + act_state_.getHandle(act_name_), &act_data->pos_cmd)); + act_vel_cmd_.registerHandle(hardware_interface::ActuatorHandle( + act_state_.getHandle(act_name_), &act_data->vel_cmd)); + act_eff_cmd_.registerHandle(hardware_interface::ActuatorHandle( + act_state_.getHandle(act_name_), &act_data->eff_cmd)); + act_foo_cmd_.registerHandle(hardware_interface::ActuatorHandle( + act_state_.getHandle(act_name_), &act_data->foo_cmd)); + act_bar_cmd_.registerHandle(hardware_interface::ActuatorHandle( + act_state_.getHandle(act_name_), &act_data->bar_cmd)); + + // register all actuator interfaces + registerInterface(&act_state_); + registerInterface(&act_pos_cmd_); + registerInterface(&act_vel_cmd_); + registerInterface(&act_eff_cmd_); + registerInterface(&act_foo_cmd_); + registerInterface(&act_bar_cmd_); + + /// @todo add some checks + return true; + } + + bool init_trans() + { + std::string urdf; + if(!readURDF("test/urdf/extended_simple_transmission_loader.urdf", urdf)) + { + return false; + } + + transmission_interface::TransmissionParser parser; + if (!parser.parse(urdf, transmission_infos_)) + { + return false; + } + + transmission_interface::TransmissionInterfaceLoader loader(this, &robot_transmissions_); + if(!loader.load(transmission_infos_)) + { + return false; + } + + return true; + } + + + void init_joints() + { + } + +private: + bool readURDF(const std::string& filename, std::string& contents) + { + resource_retriever::Retriever retriever; + resource_retriever::MemoryResource resource; + try + { + resource = retriever.get("package://extended_robot_hw_tests/" + filename); + } + catch (resource_retriever::Exception& e) + { + ROS_ERROR_STREAM("Failed to retrieve file: " << e.what()); + return false; + } + contents.assign(resource.data.get(), resource.data.get() + resource.size); + return true; + } + +private: + // actuators + hardware_interface::ExtendedActuatorStateInterface act_state_; + hardware_interface::PositionActuatorInterface act_pos_cmd_; + hardware_interface::VelocityActuatorInterface act_vel_cmd_; + hardware_interface::EffortActuatorInterface act_eff_cmd_; + hardware_interface::FooActuatorInterface act_foo_cmd_; + hardware_interface::BarActuatorInterface act_bar_cmd_; + + // transmissions + + + // joints + hardware_interface::PositionJointInterface jnt_pos_cmd_; + hardware_interface::VelocityJointInterface jnt_vel_cmd_; + hardware_interface::EffortJointInterface jnt_eff_cmd_; + hardware_interface::FooJointInterface jnt_foo_cmd_; + hardware_interface::BarJointInterface jnt_bar_cmd_; + + std::string act_name_; + + // ros::NodeHandle node_handle_; + std::vector transmission_infos_; + transmission_interface::RobotTransmissions robot_transmissions_; +}; + +TEST(RobotHWExtensionTest, ExtensionTest) +{ + ExtendedActuatorData data; + + ExtendedRobotHW robot_hw; + + ASSERT_TRUE(robot_hw.init_acts(&data)); + ASSERT_TRUE(robot_hw.init_trans()); + + /// @todo test write actuator data, check equals joint data + /// @todo test write joint command, check equals actuator command +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/extended_robot_hw_tests/test/extended_robot_hw_test.test b/extended_robot_hw_tests/test/extended_robot_hw_test.test new file mode 100644 index 0000000..13f7626 --- /dev/null +++ b/extended_robot_hw_tests/test/extended_robot_hw_test.test @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/extended_robot_hw_tests/test/urdf/extended_simple_transmission_loader.urdf b/extended_robot_hw_tests/test/urdf/extended_simple_transmission_loader.urdf new file mode 100644 index 0000000..eee5e15 --- /dev/null +++ b/extended_robot_hw_tests/test/urdf/extended_simple_transmission_loader.urdf @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + transmission_interface/ExtendedSimpleTransmission + + 0.0 + hardware_interface/PositionJointInterface + hardware_interface/VelocityJointInterface + hardware_interface/EffortJointInterface + hardware_interface/FooJointInterface + hardware_interface/BarJointInterface + + + + 50 + + + + diff --git a/hardware_interface/include/hardware_interface/actuator_state_interface.h b/hardware_interface/include/hardware_interface/actuator_state_interface.h index 9c23808..722f2b1 100644 --- a/hardware_interface/include/hardware_interface/actuator_state_interface.h +++ b/hardware_interface/include/hardware_interface/actuator_state_interface.h @@ -36,50 +36,126 @@ namespace hardware_interface { +class PositionActuatorStateHandle +{ + public: + PositionActuatorStateHandle() = default; + + PositionActuatorStateHandle(const double* pos) : pos_(pos) + { + if (!pos) + { + throw HardwareInterfaceException("Position data pointer is null."); + } + } + + virtual double getPosition() const + { + assert(pos_); + return *pos_; + } + + virtual const double* getPositionPtr() const + { + return pos_; + } + + virtual std::string getName() const = 0; + + protected: + const double* pos_ = nullptr; +}; + +class VelocityActuatorStateHandle +{ + public: + VelocityActuatorStateHandle() = default; + + VelocityActuatorStateHandle(const double* vel) : vel_(vel) + { + if (!vel) + { + throw HardwareInterfaceException("Velocity data pointer is null."); + } + } + + virtual double getVelocity() const + { + assert(vel_); + return *vel_; + } + + virtual const double* getVelocityPtr() const + { + return vel_; + } + + virtual std::string getName() const = 0; + + protected: + const double* vel_ = nullptr; +}; + +class EffortActuatorStateHandle +{ + public: + EffortActuatorStateHandle() = default; + + EffortActuatorStateHandle(const double* eff) : eff_(eff) + { + if (!eff) + { + throw HardwareInterfaceException("Effort data pointer is null."); + } + } + + virtual double getEffort() const + { + assert(eff_); + return *eff_; + } + + virtual const double* getEffortPtr() const + { + return eff_; + } + + virtual std::string getName() const = 0; + + protected: + const double* eff_ = nullptr; +}; /** A handle used to read the state of a single actuator. */ -class ActuatorStateHandle +class ActuatorStateHandle : public PositionActuatorStateHandle, + public VelocityActuatorStateHandle, + public EffortActuatorStateHandle { -public: - ActuatorStateHandle() : name_(), pos_(0), vel_(0), eff_(0) {} - - /** - * \param name The name of the actuator - * \param pos A pointer to the storage for this actuator's position - * \param vel A pointer to the storage for this actuator's velocity - * \param eff A pointer to the storage for this actuator's effort (force or torque) - */ - ActuatorStateHandle(const std::string& name, const double* pos, const double* vel, const double* eff) - : name_(name), pos_(pos), vel_(vel), eff_(eff) - { - if (!pos) + public: + ActuatorStateHandle() = default; + + /** + * \param name The name of the actuator + * \param pos A pointer to the storage for this actuator's position + * \param vel A pointer to the storage for this actuator's velocity + * \param eff A pointer to the storage for this actuator's effort (force or torque) + */ + ActuatorStateHandle(const std::string& name, const double* pos, const double* vel, const double* eff) + try : PositionActuatorStateHandle(pos), VelocityActuatorStateHandle(vel), EffortActuatorStateHandle(eff), name_(name) { - throw HardwareInterfaceException("Cannot create handle '" + name + "'. Position data pointer is null."); } - if (!vel) + catch(const HardwareInterfaceException& ex) { - throw HardwareInterfaceException("Cannot create handle '" + name + "'. Velocity data pointer is null."); + throw HardwareInterfaceException("Cannot create handle '" + name + "'. " + ex.what()); } - if (!eff) + + std::string getName() const { - throw HardwareInterfaceException("Cannot create handle '" + name + "'. Effort data pointer is null."); + return name_; } - } - - std::string getName() const {return name_;} - double getPosition() const {assert(pos_); return *pos_;} - double getVelocity() const {assert(vel_); return *vel_;} - double getEffort() const {assert(eff_); return *eff_;} - - const double* getPositionPtr() const {return pos_;} - const double* getVelocityPtr() const {return vel_;} - const double* getEffortPtr() const {return eff_;} - -private: - std::string name_; - const double* pos_; - const double* vel_; - const double* eff_; + + private: + std::string name_ = ""; }; /** \brief Hardware interface to support reading the state of an array of actuators @@ -89,8 +165,9 @@ class ActuatorStateHandle * torque). * */ -class ActuatorStateInterface : public HardwareResourceManager {}; - +class ActuatorStateInterface : public HardwareResourceManager +{ +}; } #endif diff --git a/hardware_interface/include/hardware_interface/joint_state_interface.h b/hardware_interface/include/hardware_interface/joint_state_interface.h index 3b6da08..46bbab0 100644 --- a/hardware_interface/include/hardware_interface/joint_state_interface.h +++ b/hardware_interface/include/hardware_interface/joint_state_interface.h @@ -37,11 +37,88 @@ namespace hardware_interface { +class PositionJointStateHandle +{ + public: + PositionJointStateHandle() = default; + + PositionJointStateHandle(const double* pos) : pos_(pos) + { + if (!pos) + { + throw HardwareInterfaceException("Position data pointer is null."); + } + } + + virtual double getPosition() const + { + assert(pos_); + return *pos_; + } + + virtual std::string getName() const = 0; + + protected: + const double* pos_ = nullptr; +}; + +class VelocityJointStateHandle +{ + public: + VelocityJointStateHandle() = default; + + VelocityJointStateHandle(const double* vel) : vel_(vel) + { + if (!vel) + { + throw HardwareInterfaceException("Velocity data pointer is null."); + } + } + + virtual double getVelocity() const + { + assert(vel_); + return *vel_; + } + + virtual std::string getName() const = 0; + + protected: + const double* vel_ = nullptr; +}; + +class EffortJointStateHandle +{ + public: + EffortJointStateHandle() = default; + + EffortJointStateHandle(const double* eff) : eff_(eff) + { + if (!eff) + { + throw HardwareInterfaceException("Effort data pointer is null."); + } + } + + virtual double getEffort() const + { + assert(eff_); + return *eff_; + } + + virtual std::string getName() const = 0; + + protected: + const double* eff_ = nullptr; +}; + /** A handle used to read the state of a single joint. */ -class JointStateHandle +class JointStateHandle : public PositionJointStateHandle, + public VelocityJointStateHandle, + public EffortJointStateHandle { public: - JointStateHandle() : name_(), pos_(0), vel_(0), eff_(0) {} + JointStateHandle() = default; /** * \param name The name of the joint @@ -50,32 +127,19 @@ class JointStateHandle * \param eff A pointer to the storage for this joint's effort (force or torque) */ JointStateHandle(const std::string& name, const double* pos, const double* vel, const double* eff) - : name_(name), pos_(pos), vel_(vel), eff_(eff) - { - if (!pos) - { - throw HardwareInterfaceException("Cannot create handle '" + name + "'. Position data pointer is null."); - } - if (!vel) + try : PositionJointStateHandle(pos), VelocityJointStateHandle(vel), EffortJointStateHandle(eff), name_(name) { - throw HardwareInterfaceException("Cannot create handle '" + name + "'. Velocity data pointer is null."); } - if (!eff) + catch(const HardwareInterfaceException& ex) { - throw HardwareInterfaceException("Cannot create handle '" + name + "'. Effort data pointer is null."); + throw HardwareInterfaceException("Cannot create handle '" + name + "'. " + ex.what()); } - } + std::string getName() const {return name_;} - double getPosition() const {assert(pos_); return *pos_;} - double getVelocity() const {assert(vel_); return *vel_;} - double getEffort() const {assert(eff_); return *eff_;} private: - std::string name_; - const double* pos_; - const double* vel_; - const double* eff_; + std::string name_ = ""; }; /** \brief Hardware interface to support reading the state of an array of joints diff --git a/hardware_interface/test/actuator_state_interface_test.cpp b/hardware_interface/test/actuator_state_interface_test.cpp index b02a1ca..b1284de 100644 --- a/hardware_interface/test/actuator_state_interface_test.cpp +++ b/hardware_interface/test/actuator_state_interface_test.cpp @@ -125,6 +125,82 @@ TEST_F(ActuatorStateInterfaceTest, ExcerciseApi) catch(const HardwareInterfaceException& e) {ROS_ERROR_STREAM(e.what());} } +namespace hardware_interface { + +class CustomActuatorStateHandle +{ +public: + CustomActuatorStateHandle() = default; + + CustomActuatorStateHandle(const double* custom) : custom_(custom) + { + if (!custom) + { + throw HardwareInterfaceException("Custom data pointer is null."); + } + } + + virtual ~CustomActuatorStateHandle() + { + } + + virtual double getCustom() const + { + assert(custom_); + return *custom_; + } + + virtual const double* getCustomPtr() const + { + return custom_; + } + + virtual std::string getName() const = 0; + +protected: + const double* custom_ = nullptr; +}; + +class ExtendedActuatorStateHandle : public ActuatorStateHandle, + public CustomActuatorStateHandle +{ +public: + ExtendedActuatorStateHandle() = default; + + ExtendedActuatorStateHandle(const std::string& name, const double* pos, + const double* vel, const double* eff, const double* custom) + : ActuatorStateHandle(name, pos, vel, eff), CustomActuatorStateHandle(custom) + { + } + + std::string getName() const + { + return ActuatorStateHandle::getName(); + } +}; + +} + +TEST_F(ActuatorStateInterfaceTest, ExtensionTest) +{ + const double POSITION = 1.0; + const double VELOCITY = 2.0; + const double EFFORT = 3.0; + const double CUSTOM = 4.0; + + double pos = POSITION; + double vel = VELOCITY; + double eff = EFFORT; + double custom = CUSTOM; + + ExtendedActuatorStateHandle act("ExtendedHandle", &pos, &vel, &eff, &custom); + + ASSERT_EQ(act.getPosition(), POSITION); + ASSERT_EQ(act.getVelocity(), VELOCITY); + ASSERT_EQ(act.getEffort(), EFFORT); + ASSERT_EQ(act.getCustom(), CUSTOM); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/transmission_interface/CMakeLists.txt b/transmission_interface/CMakeLists.txt index 3072479..59f9e3b 100644 --- a/transmission_interface/CMakeLists.txt +++ b/transmission_interface/CMakeLists.txt @@ -105,6 +105,9 @@ if(CATKIN_ENABLE_TESTING) catkin_add_gtest(transmission_interface_test test/transmission_interface_test.cpp) target_link_libraries(transmission_interface_test ${TinyXML_LIBRARIES} ${catkin_LIBRARIES}) + catkin_add_gtest(transmission_interface_variable_test test/transmission_interface_variable_test.cpp) + target_link_libraries(transmission_interface_variable_test ${TinyXML_LIBRARIES} ${catkin_LIBRARIES}) + catkin_add_gtest(transmission_parser_test test/transmission_parser_test.cpp) target_link_libraries(transmission_parser_test ${PROJECT_NAME}_parser ${catkin_LIBRARIES}) diff --git a/transmission_interface/include/transmission_interface/actuator_data.h b/transmission_interface/include/transmission_interface/actuator_data.h new file mode 100644 index 0000000..c599579 --- /dev/null +++ b/transmission_interface/include/transmission_interface/actuator_data.h @@ -0,0 +1,90 @@ +#pragma once + +#include +#include +#include + +namespace transmission_interface +{ + +struct PositionActuatorData +{ + std::vector position; +}; + +struct VelocityActuatorData +{ + std::vector velocity; +}; + +struct EffortActuatorData +{ + std::vector effort; +}; + +class ActuatorDataBase +{ +public: +virtual bool empty() const = 0; +virtual bool hasSize(std::size_t size) const = 0; +virtual bool valid() const = 0; + +inline bool hasValidPointers(const std::vector& data) const +{ +return std::all_of(data.cbegin(), data.cend(), [](const double* ptr){ return ptr;}); +} +}; + +template +struct ActuatorDataContainer : public ActuatorDataBase, public Interfaces... +{ + public: + ActuatorDataContainer(){} + ActuatorDataContainer(Interfaces... ifaces) : Interfaces(ifaces)... {} + virtual bool empty() const = 0; + virtual bool hasSize(std::size_t size) const = 0; + virtual bool valid() const = 0; +}; + +/** + * \brief Contains pointers to raw data representing the position, velocity and acceleration of a transmission's + * actuators. + */ +class ActuatorData : public ActuatorDataContainer +{ +public: + bool empty() const override + { + return position.empty() && velocity.empty() && effort.empty(); + } + + bool hasSize(std::size_t size) const override + { + if(not position.empty() and position.size() != size ) + { + std::cout << "Positions vector has wrong size" << std::endl; + return false; + } + + if(not velocity.empty() and velocity.size() != size ) + { + std::cout << "velocity vector has wrong size" << std::endl; + return false; + } + + if(not effort.empty() and effort.size() != size ) + { + std::cout << "effort vector has wrong size" << std::endl; + return false; + } + + return true; + } + + bool valid() const override + { + return hasValidPointers(position) and hasValidPointers(velocity) and hasValidPointers(effort); + } +}; + +} // namespace transmission_interface diff --git a/transmission_interface/include/transmission_interface/differential_transmission.h b/transmission_interface/include/transmission_interface/differential_transmission.h index b992bc3..376f234 100644 --- a/transmission_interface/include/transmission_interface/differential_transmission.h +++ b/transmission_interface/include/transmission_interface/differential_transmission.h @@ -39,7 +39,6 @@ namespace transmission_interface { - /** * \brief Implementation of a differential transmission. * @@ -116,195 +115,198 @@ namespace transmission_interface */ class DifferentialTransmission : public Transmission { -public: - /** - * \param actuator_reduction Reduction ratio of actuators. - * \param joint_reduction Reduction ratio of joints. - * \param joint_offset Joint position offset used in the position mappings. - * \pre Nonzero actuator and joint reduction values. - */ - DifferentialTransmission(const std::vector& actuator_reduction, - const std::vector& joint_reduction, - const std::vector& joint_offset = std::vector(2, 0.0)); - - /** - * \brief Transform \e effort variables from actuator to joint space. - * \param[in] act_data Actuator-space variables. - * \param[out] jnt_data Joint-space variables. - * \pre Actuator and joint effort vectors must have size 2 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. - */ - void actuatorToJointEffort(const ActuatorData& act_data, - JointData& jnt_data); - - /** - * \brief Transform \e velocity variables from actuator to joint space. - * \param[in] act_data Actuator-space variables. - * \param[out] jnt_data Joint-space variables. - * \pre Actuator and joint velocity vectors must have size 2 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. - */ - void actuatorToJointVelocity(const ActuatorData& act_data, - JointData& jnt_data); - - /** - * \brief Transform \e position variables from actuator to joint space. - * \param[in] act_data Actuator-space variables. - * \param[out] jnt_data Joint-space variables. - * \pre Actuator and joint position vectors must have size 2 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. - */ - void actuatorToJointPosition(const ActuatorData& act_data, - JointData& jnt_data); - - /** - * \brief Transform \e effort variables from joint to actuator space. - * \param[in] jnt_data Joint-space variables. - * \param[out] act_data Actuator-space variables. - * \pre Actuator and joint effort vectors must have size 2 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. - */ - void jointToActuatorEffort(const JointData& jnt_data, - ActuatorData& act_data); - - /** - * \brief Transform \e velocity variables from joint to actuator space. - * \param[in] jnt_data Joint-space variables. - * \param[out] act_data Actuator-space variables. - * \pre Actuator and joint velocity vectors must have size 2 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. - */ - void jointToActuatorVelocity(const JointData& jnt_data, - ActuatorData& act_data); - - /** - * \brief Transform \e position variables from joint to actuator space. - * \param[in] jnt_data Joint-space variables. - * \param[out] act_data Actuator-space variables. - * \pre Actuator and joint position vectors must have size 2 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. - */ - void jointToActuatorPosition(const JointData& jnt_data, - ActuatorData& act_data); - - std::size_t numActuators() const {return 2;} - std::size_t numJoints() const {return 2;} - - const std::vector& getActuatorReduction() const {return act_reduction_;} - const std::vector& getJointReduction() const {return jnt_reduction_;} - const std::vector& getJointOffset() const {return jnt_offset_;} - -protected: - std::vector act_reduction_; - std::vector jnt_reduction_; - std::vector jnt_offset_; + public: + /** + * \param actuator_reduction Reduction ratio of actuators. + * \param joint_reduction Reduction ratio of joints. + * \param joint_offset Joint position offset used in the position mappings. + * \pre Nonzero actuator and joint reduction values. + */ + DifferentialTransmission(const std::vector& actuator_reduction, const std::vector& joint_reduction, + const std::vector& joint_offset = std::vector(2, 0.0)); + + /** + * \brief Transform \e effort variables from actuator to joint space. + * \param[in] act_data Actuator-space variables. + * \param[out] jnt_data Joint-space variables. + * \pre Actuator and joint effort vectors must have size 2 and point to valid data. + * To call this method it is not required that all other data vectors contain valid data, and can even remain + * empty. + */ + void actuatorToJoint(const EffortActuatorData& act_data, EffortJointData& jnt_data); + + /** + * \brief Transform \e velocity variables from actuator to joint space. + * \param[in] act_data Actuator-space variables. + * \param[out] jnt_data Joint-space variables. + * \pre Actuator and joint velocity vectors must have size 2 and point to valid data. + * To call this method it is not required that all other data vectors contain valid data, and can even remain + * empty. + */ + void actuatorToJoint(const VelocityActuatorData& act_data, VelocityJointData& jnt_data); + + /** + * \brief Transform \e position variables from actuator to joint space. + * \param[in] act_data Actuator-space variables. + * \param[out] jnt_data Joint-space variables. + * \pre Actuator and joint position vectors must have size 2 and point to valid data. + * To call this method it is not required that all other data vectors contain valid data, and can even remain + * empty. + */ + void actuatorToJoint(const PositionActuatorData& act_data, PositionJointData& jnt_data); + + /** + * \brief Transform \e effort variables from joint to actuator space. + * \param[in] jnt_data Joint-space variables. + * \param[out] act_data Actuator-space variables. + * \pre Actuator and joint effort vectors must have size 2 and point to valid data. + * To call this method it is not required that all other data vectors contain valid data, and can even remain + * empty. + */ + void jointToActuator(const EffortJointData& jnt_data, EffortActuatorData& act_data); + + /** + * \brief Transform \e velocity variables from joint to actuator space. + * \param[in] jnt_data Joint-space variables. + * \param[out] act_data Actuator-space variables. + * \pre Actuator and joint velocity vectors must have size 2 and point to valid data. + * To call this method it is not required that all other data vectors contain valid data, and can even remain + * empty. + */ + void jointToActuator(const VelocityJointData& jnt_data, VelocityActuatorData& act_data); + + /** + * \brief Transform \e position variables from joint to actuator space. + * \param[in] jnt_data Joint-space variables. + * \param[out] act_data Actuator-space variables. + * \pre Actuator and joint position vectors must have size 2 and point to valid data. + * To call this method it is not required that all other data vectors contain valid data, and can even remain + * empty. + */ + void jointToActuator(const PositionJointData& jnt_data, PositionActuatorData& act_data); + + std::size_t numActuators() const + { + return 2; + } + std::size_t numJoints() const + { + return 2; + } + + const std::vector& getActuatorReduction() const + { + return act_reduction_; + } + const std::vector& getJointReduction() const + { + return jnt_reduction_; + } + const std::vector& getJointOffset() const + { + return jnt_offset_; + } + + protected: + std::vector act_reduction_; + std::vector jnt_reduction_; + std::vector jnt_offset_; }; inline DifferentialTransmission::DifferentialTransmission(const std::vector& actuator_reduction, const std::vector& joint_reduction, const std::vector& joint_offset) - : Transmission(), - act_reduction_(actuator_reduction), - jnt_reduction_(joint_reduction), - jnt_offset_(joint_offset) + : Transmission(), act_reduction_(actuator_reduction), jnt_reduction_(joint_reduction), jnt_offset_(joint_offset) { - if (numActuators() != act_reduction_.size() || - numJoints() != jnt_reduction_.size() || - numJoints() != jnt_offset_.size()) - { - throw TransmissionInterfaceException("Reduction and offset vectors of a differential transmission must have size 2."); - } - - if (0.0 == act_reduction_[0] || - 0.0 == act_reduction_[1] || - 0.0 == jnt_reduction_[0] || - 0.0 == jnt_reduction_[1] - ) - { - throw TransmissionInterfaceException("Transmission reduction ratios cannot be zero."); - } + if (numActuators() != act_reduction_.size() || numJoints() != jnt_reduction_.size() || + numJoints() != jnt_offset_.size()) + { + throw TransmissionInterfaceException("Reduction and offset vectors of a differential transmission must have " + "size 2."); + } + + if (0.0 == act_reduction_[0] || 0.0 == act_reduction_[1] || 0.0 == jnt_reduction_[0] || 0.0 == jnt_reduction_[1]) + { + throw TransmissionInterfaceException("Transmission reduction ratios cannot be zero."); + } } -inline void DifferentialTransmission::actuatorToJointEffort(const ActuatorData& act_data, - JointData& jnt_data) +inline void DifferentialTransmission::actuatorToJoint(const EffortActuatorData& act_data, EffortJointData& jnt_data) { - assert(numActuators() == act_data.effort.size() && numJoints() == jnt_data.effort.size()); - assert(act_data.effort[0] && act_data.effort[1] && jnt_data.effort[0] && jnt_data.effort[1]); + assert(numActuators() == act_data.effort.size() && numJoints() == jnt_data.effort.size()); + assert(act_data.effort[0] && act_data.effort[1] && jnt_data.effort[0] && jnt_data.effort[1]); - std::vector& ar = act_reduction_; - std::vector& jr = jnt_reduction_; + std::vector& ar = act_reduction_; + std::vector& jr = jnt_reduction_; - *jnt_data.effort[0] = jr[0] * (*act_data.effort[0] * ar[0] + *act_data.effort[1] * ar[1]); - *jnt_data.effort[1] = jr[1] * (*act_data.effort[0] * ar[0] - *act_data.effort[1] * ar[1]); + *jnt_data.effort[0] = jr[0] * (*act_data.effort[0] * ar[0] + *act_data.effort[1] * ar[1]); + *jnt_data.effort[1] = jr[1] * (*act_data.effort[0] * ar[0] - *act_data.effort[1] * ar[1]); } -inline void DifferentialTransmission::actuatorToJointVelocity(const ActuatorData& act_data, - JointData& jnt_data) +inline void DifferentialTransmission::actuatorToJoint(const VelocityActuatorData& act_data, VelocityJointData& jnt_data) { - assert(numActuators() == act_data.velocity.size() && numJoints() == jnt_data.velocity.size()); - assert(act_data.velocity[0] && act_data.velocity[1] && jnt_data.velocity[0] && jnt_data.velocity[1]); + assert(numActuators() == act_data.velocity.size() && numJoints() == jnt_data.velocity.size()); + assert(act_data.velocity[0] && act_data.velocity[1] && jnt_data.velocity[0] && jnt_data.velocity[1]); - std::vector& ar = act_reduction_; - std::vector& jr = jnt_reduction_; + std::vector& ar = act_reduction_; + std::vector& jr = jnt_reduction_; - *jnt_data.velocity[0] = (*act_data.velocity[0] / ar[0] + *act_data.velocity[1] / ar[1]) / (2.0 * jr[0]); - *jnt_data.velocity[1] = (*act_data.velocity[0] / ar[0] - *act_data.velocity[1] / ar[1]) / (2.0 * jr[1]); + *jnt_data.velocity[0] = (*act_data.velocity[0] / ar[0] + *act_data.velocity[1] / ar[1]) / (2.0 * jr[0]); + *jnt_data.velocity[1] = (*act_data.velocity[0] / ar[0] - *act_data.velocity[1] / ar[1]) / (2.0 * jr[1]); } -inline void DifferentialTransmission::actuatorToJointPosition(const ActuatorData& act_data, - JointData& jnt_data) +inline void DifferentialTransmission::actuatorToJoint(const PositionActuatorData& act_data, PositionJointData& jnt_data) { - assert(numActuators() == act_data.position.size() && numJoints() == jnt_data.position.size()); - assert(act_data.position[0] && act_data.position[1] && jnt_data.position[0] && jnt_data.position[1]); + assert(numActuators() == act_data.position.size() && numJoints() == jnt_data.position.size()); + assert(act_data.position[0] && act_data.position[1] && jnt_data.position[0] && jnt_data.position[1]); - std::vector& ar = act_reduction_; - std::vector& jr = jnt_reduction_; + std::vector& ar = act_reduction_; + std::vector& jr = jnt_reduction_; - *jnt_data.position[0] = (*act_data.position[0] / ar[0] + *act_data.position[1] / ar[1]) / (2.0 * jr[0]) + jnt_offset_[0]; - *jnt_data.position[1] = (*act_data.position[0] / ar[0] - *act_data.position[1] / ar[1]) / (2.0 * jr[1]) + jnt_offset_[1]; + *jnt_data.position[0] = + (*act_data.position[0] / ar[0] + *act_data.position[1] / ar[1]) / (2.0 * jr[0]) + jnt_offset_[0]; + *jnt_data.position[1] = + (*act_data.position[0] / ar[0] - *act_data.position[1] / ar[1]) / (2.0 * jr[1]) + jnt_offset_[1]; } -inline void DifferentialTransmission::jointToActuatorEffort(const JointData& jnt_data, - ActuatorData& act_data) +inline void DifferentialTransmission::jointToActuator(const EffortJointData& jnt_data, EffortActuatorData& act_data) { - assert(numActuators() == act_data.effort.size() && numJoints() == jnt_data.effort.size()); - assert(act_data.effort[0] && act_data.effort[1] && jnt_data.effort[0] && jnt_data.effort[1]); + assert(numActuators() == act_data.effort.size() && numJoints() == jnt_data.effort.size()); + assert(act_data.effort[0] && act_data.effort[1] && jnt_data.effort[0] && jnt_data.effort[1]); - std::vector& ar = act_reduction_; - std::vector& jr = jnt_reduction_; + std::vector& ar = act_reduction_; + std::vector& jr = jnt_reduction_; - *act_data.effort[0] = (*jnt_data.effort[0] / jr[0] + *jnt_data.effort[1] / jr[1]) / (2.0 * ar[0]); - *act_data.effort[1] = (*jnt_data.effort[0] / jr[0] - *jnt_data.effort[1] / jr[1]) / (2.0 * ar[1]); + *act_data.effort[0] = (*jnt_data.effort[0] / jr[0] + *jnt_data.effort[1] / jr[1]) / (2.0 * ar[0]); + *act_data.effort[1] = (*jnt_data.effort[0] / jr[0] - *jnt_data.effort[1] / jr[1]) / (2.0 * ar[1]); } -inline void DifferentialTransmission::jointToActuatorVelocity(const JointData& jnt_data, - ActuatorData& act_data) +inline void DifferentialTransmission::jointToActuator(const VelocityJointData& jnt_data, VelocityActuatorData& act_data) { - assert(numActuators() == act_data.velocity.size() && numJoints() == jnt_data.velocity.size()); - assert(act_data.velocity[0] && act_data.velocity[1] && jnt_data.velocity[0] && jnt_data.velocity[1]); + assert(numActuators() == act_data.velocity.size() && numJoints() == jnt_data.velocity.size()); + assert(act_data.velocity[0] && act_data.velocity[1] && jnt_data.velocity[0] && jnt_data.velocity[1]); - std::vector& ar = act_reduction_; - std::vector& jr = jnt_reduction_; + std::vector& ar = act_reduction_; + std::vector& jr = jnt_reduction_; - *act_data.velocity[0] = (*jnt_data.velocity[0] * jr[0] + *jnt_data.velocity[1] * jr[1]) * ar[0]; - *act_data.velocity[1] = (*jnt_data.velocity[0] * jr[0] - *jnt_data.velocity[1] * jr[1]) * ar[1]; + *act_data.velocity[0] = (*jnt_data.velocity[0] * jr[0] + *jnt_data.velocity[1] * jr[1]) * ar[0]; + *act_data.velocity[1] = (*jnt_data.velocity[0] * jr[0] - *jnt_data.velocity[1] * jr[1]) * ar[1]; } -inline void DifferentialTransmission::jointToActuatorPosition(const JointData& jnt_data, - ActuatorData& act_data) +inline void DifferentialTransmission::jointToActuator(const PositionJointData& jnt_data, PositionActuatorData& act_data) { - assert(numActuators() == act_data.position.size() && numJoints() == jnt_data.position.size()); - assert(act_data.position[0] && act_data.position[1] && jnt_data.position[0] && jnt_data.position[1]); + assert(numActuators() == act_data.position.size() && numJoints() == jnt_data.position.size()); + assert(act_data.position[0] && act_data.position[1] && jnt_data.position[0] && jnt_data.position[1]); - std::vector& ar = act_reduction_; - std::vector& jr = jnt_reduction_; + std::vector& ar = act_reduction_; + std::vector& jr = jnt_reduction_; - double jnt_pos_off[2] = {*jnt_data.position[0] - jnt_offset_[0], *jnt_data.position[1] - jnt_offset_[1]}; + double jnt_pos_off[2] = { *jnt_data.position[0] - jnt_offset_[0], *jnt_data.position[1] - jnt_offset_[1] }; - *act_data.position[0] = (jnt_pos_off[0] * jr[0] + jnt_pos_off[1] * jr[1]) * ar[0]; - *act_data.position[1] = (jnt_pos_off[0] * jr[0] - jnt_pos_off[1] * jr[1]) * ar[1]; + *act_data.position[0] = (jnt_pos_off[0] * jr[0] + jnt_pos_off[1] * jr[1]) * ar[0]; + *act_data.position[1] = (jnt_pos_off[0] * jr[0] - jnt_pos_off[1] * jr[1]) * ar[1]; } -} // transmission_interface +} // transmission_interface -#endif // TRANSMISSION_INTERFACE_DIFFERENTIAL_TRANSMISSION_H +#endif // TRANSMISSION_INTERFACE_DIFFERENTIAL_TRANSMISSION_H diff --git a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.h b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.h index 97f7938..c8771cf 100644 --- a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.h +++ b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.h @@ -40,270 +40,275 @@ namespace transmission_interface { - - /** - * \brief Implementation of a four-bar-linkage transmission. - * - * This transmission relates two actuators and two joints through a mechanism in which the state of the - * first joint only depends on the first actuator, while the second joint depends on both actuators, as - * illustrated below. - * Although the class name makes specific reference to the four-bar-linkage, there are other mechanical layouts - * that yield the same behavior, such as the remote actuation example also depicted below. - * \image html four_bar_linkage_transmission.png - * - *
- * - * - * - * - * - * - * - * - * - * - *
Effort
Velocity
Position
- * Actuator to joint - * - * \f{eqnarray*}{ - * \tau_{j_1} & = & n_{j_1} n_{a_1} \tau_{a_1} \\ - * \tau_{j_2} & = & n_{j_2} (n_{a_2} \tau_{a_2} - n_{j_1} n_{a_1} \tau_{a_1}) - * \f} - * - * \f{eqnarray*}{ - * \dot{x}_{j_1} & = & \frac{ \dot{x}_{a_1} }{ n_{j_1} n_{a_1} } \\ - * \dot{x}_{j_2} & = & \frac{ \dot{x}_{a_2} / n_{a_2} - \dot{x}_{a_1} / (n_{j_1} n_{a_1}) }{ n_{j_2} } - * \f} - * - * \f{eqnarray*}{ - * x_{j_1} & = & \frac{ x_{a_1} }{ n_{j_1} n_{a_1} } + x_{off_1} \\ - * x_{j_2} & = & \frac{ x_{a_2} / n_{a_2} - x_{a_1} / (n_{j_1} n_{a_1}) }{ n_{j_2} } + x_{off_2} - * \f} - *
- * Joint to actuator - * - * \f{eqnarray*}{ - * \tau_{a_1} & = & \tau_{j_1} / (n_{j_1} n_{a_1}) \\ - * \tau_{a_2} & = & \frac{ \tau_{j_1} + \tau_{j_2} / n_{j_2} }{ n_{a_2} } - * \f} - * - * \f{eqnarray*}{ - * \dot{x}_{a_1} & = & n_{j_1} n_{a_1} \dot{x}_{j_1} \\ - * \dot{x}_{a_2} & = & n_{a_2} (\dot{x}_{j_1} + n_{j_2} \dot{x}_{j_2}) - * \f} - * - * \f{eqnarray*}{ - * x_{a_1} & = & n_{j_1} n_{a_1} (x_{j_1} - x_{off_1}) \\ - * x_{a_2} & = & n_{a_2} \left[(x_{j_1} - x_{off_1}) + n_{j_2} (x_{j_2} - x_{off_2})\right] - * \f} - *
- *
- * - * where: - * - \f$ x \f$, \f$ \dot{x} \f$ and \f$ \tau \f$ are position, velocity and effort variables, respectively. - * - Subindices \f$ _a \f$ and \f$ _j \f$ are used to represent actuator-space and joint-space variables, respectively. - * - \f$ x_{off}\f$ represents the offset between motor and joint zeros, expressed in joint position coordinates. - * (cf. SimpleTransmission class documentation for a more detailed description of this variable). - * - \f$ n \f$ represents a transmission ratio (reducers/amplifiers are depicted as timing belts in the figure). - * A transmission ratio can take any real value \e except zero. In particular: - * - If its absolute value is greater than one, it's a velocity reducer / effort amplifier, while if its absolute - * value lies in \f$ (0, 1) \f$ it's a velocity amplifier / effort reducer. - * - Negative values represent a direction flip, ie. input and output move in opposite directions. - * - Important: Use transmission ratio signs to match this class' convention of positive actuator/joint - * directions with a given mechanical design, as they will in general not match. - * - * \ingroup transmission_types - */ +/** +*\brief Implementation of a four-bar-linkage transmission. +* +*This transmission relates two actuators and two joints through a mechanism in which the state of the +*first joint only depends on the first actuator, while the second joint depends on both actuators, as +*illustrated below. +*Although the class name makes specific reference to the four-bar-linkage, there are other mechanical layouts +*that yield the same behavior, such as the remote actuation example also depicted below. +*\image html four_bar_linkage_transmission.png +* +*
+* +* +* +* +* +* +* +* +* +* +*
Effort
Velocity
Position
+* Actuator to joint +* +*\f{eqnarray*}{ +*\tau_{j_1} & = & n_{j_1} n_{a_1} \tau_{a_1} \\ +*\tau_{j_2} & = & n_{j_2} (n_{a_2} \tau_{a_2} - n_{j_1} n_{a_1} \tau_{a_1}) +*\f} +* +*\f{eqnarray*}{ +*\dot{x}_{j_1} & = & \frac{ \dot{x}_{a_1} }{ n_{j_1} n_{a_1} } \\ +*\dot{x}_{j_2} & = & \frac{ \dot{x}_{a_2} / n_{a_2} - \dot{x}_{a_1} / (n_{j_1} n_{a_1}) }{ n_{j_2} } +*\f} +* +*\f{eqnarray*}{ +*x_{j_1} & = & \frac{ x_{a_1} }{ n_{j_1} n_{a_1} } + x_{off_1} \\ +*x_{j_2} & = & \frac{ x_{a_2} / n_{a_2} - x_{a_1} / (n_{j_1} n_{a_1}) }{ n_{j_2} } + x_{off_2} +*\f} +*
+* Joint to actuator +* +*\f{eqnarray*}{ +*\tau_{a_1} & = & \tau_{j_1} / (n_{j_1} n_{a_1}) \\ +*\tau_{a_2} & = & \frac{ \tau_{j_1} + \tau_{j_2} / n_{j_2} }{ n_{a_2} } +*\f} +* +*\f{eqnarray*}{ +*\dot{x}_{a_1} & = & n_{j_1} n_{a_1} \dot{x}_{j_1} \\ +*\dot{x}_{a_2} & = & n_{a_2} (\dot{x}_{j_1} + n_{j_2} \dot{x}_{j_2}) +*\f} +* +*\f{eqnarray*}{ +*x_{a_1} & = & n_{j_1} n_{a_1} (x_{j_1} - x_{off_1}) \\ +*x_{a_2} & = & n_{a_2} \left[(x_{j_1} - x_{off_1}) + n_{j_2} (x_{j_2} - x_{off_2})\right] +*\f} +*
+*
+* +*where: +*- \f$ x \f$, \f$ \dot{x} \f$ and \f$ \tau \f$ are position, velocity and effort variables, respectively. +*- Subindices \f$ _a \f$ and \f$ _j \f$ are used to represent actuator-space and joint-space variables, respectively. +*- \f$ x_{off}\f$ represents the offset between motor and joint zeros, expressed in joint position coordinates. +* (cf. SimpleTransmission class documentation for a more detailed description of this variable). +*- \f$ n \f$ represents a transmission ratio (reducers/amplifiers are depicted as timing belts in the figure). +* A transmission ratio can take any real value \e except zero. In particular: +* - If its absolute value is greater than one, it's a velocity reducer / effort amplifier, while if its absolute +* value lies in \f$ (0, 1) \f$ it's a velocity amplifier / effort reducer. +* - Negative values represent a direction flip, ie. input and output move in opposite directions. +* - Important: Use transmission ratio signs to match this class' convention of positive actuator/joint +* directions with a given mechanical design, as they will in general not match. +* +*\ingroup transmission_types +*/ class FourBarLinkageTransmission : public Transmission { -public: - /** - * \param actuator_reduction Reduction ratio of actuators. - * \param joint_reduction Reduction ratio of joints. - * \param joint_offset Joint position offset used in the position mappings. - * \pre Nonzero actuator reduction values. - */ - FourBarLinkageTransmission(const std::vector& actuator_reduction, - const std::vector& joint_reduction, - const std::vector& joint_offset = std::vector(2, 0.0)); - - /** - * \brief Transform \e effort variables from actuator to joint space. - * \param[in] act_data Actuator-space variables. - * \param[out] jnt_data Joint-space variables. - * \pre Actuator and joint effort vectors must have size 2 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. - */ - void actuatorToJointEffort(const ActuatorData& act_data, - JointData& jnt_data); - - /** - * \brief Transform \e velocity variables from actuator to joint space. - * \param[in] act_data Actuator-space variables. - * \param[out] jnt_data Joint-space variables. - * \pre Actuator and joint velocity vectors must have size 2 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. - */ - void actuatorToJointVelocity(const ActuatorData& act_data, - JointData& jnt_data); - - /** - * \brief Transform \e position variables from actuator to joint space. - * \param[in] act_data Actuator-space variables. - * \param[out] jnt_data Joint-space variables. - * \pre Actuator and joint position vectors must have size 2 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. - */ - void actuatorToJointPosition(const ActuatorData& act_data, - JointData& jnt_data); - - /** - * \brief Transform \e effort variables from joint to actuator space. - * \param[in] jnt_data Joint-space variables. - * \param[out] act_data Actuator-space variables. - * \pre Actuator and joint effort vectors must have size 2 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. - */ - void jointToActuatorEffort(const JointData& jnt_data, - ActuatorData& act_data); - - /** - * \brief Transform \e velocity variables from joint to actuator space. - * \param[in] jnt_data Joint-space variables. - * \param[out] act_data Actuator-space variables. - * \pre Actuator and joint velocity vectors must have size 2 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. - */ - void jointToActuatorVelocity(const JointData& jnt_data, - ActuatorData& act_data); - - /** - * \brief Transform \e position variables from joint to actuator space. - * \param[in] jnt_data Joint-space variables. - * \param[out] act_data Actuator-space variables. - * \pre Actuator and joint position vectors must have size 2 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. - */ - void jointToActuatorPosition(const JointData& jnt_data, - ActuatorData& act_data); - - std::size_t numActuators() const {return 2;} - std::size_t numJoints() const {return 2;} - - const std::vector& getActuatorReduction() const {return act_reduction_;} - const std::vector& getJointReduction() const {return jnt_reduction_;} - const std::vector& getJointOffset() const {return jnt_offset_;} - -protected: - std::vector act_reduction_; - std::vector jnt_reduction_; - std::vector jnt_offset_; + public: + /** + * \param actuator_reduction Reduction ratio of actuators. + * \param joint_reduction Reduction ratio of joints. + * \param joint_offset Joint position offset used in the position mappings. + * \pre Nonzero actuator reduction values. + */ + FourBarLinkageTransmission(const std::vector& actuator_reduction, + const std::vector& joint_reduction, + const std::vector& joint_offset = std::vector(2, 0.0)); + + /** + * \brief Transform \e effort variables from actuator to joint space. + * \param[in] act_data Actuator-space variables. + * \param[out] jnt_data Joint-space variables. + * \pre Actuator and joint effort vectors must have size 2 and point to valid data. + * To call this method it is not required that all other data vectors contain valid data, and can even remain + * empty. + */ + void actuatorToJoint(const EffortActuatorData& act_data, EffortJointData& jnt_data); + + /** + * \brief Transform \e velocity variables from actuator to joint space. + * \param[in] act_data Actuator-space variables. + * \param[out] jnt_data Joint-space variables. + * \pre Actuator and joint velocity vectors must have size 2 and point to valid data. + * To call this method it is not required that all other data vectors contain valid data, and can even remain + * empty. + */ + void actuatorToJoint(const VelocityActuatorData& act_data, VelocityJointData& jnt_data); + + /** + * \brief Transform \e position variables from actuator to joint space. + * \param[in] act_data Actuator-space variables. + * \param[out] jnt_data Joint-space variables. + * \pre Actuator and joint position vectors must have size 2 and point to valid data. + * To call this method it is not required that all other data vectors contain valid data, and can even remain + * empty. + */ + void actuatorToJoint(const PositionActuatorData& act_data, PositionJointData& jnt_data); + + /** + * \brief Transform \e effort variables from joint to actuator space. + * \param[in] jnt_data Joint-space variables. + * \param[out] act_data Actuator-space variables. + * \pre Actuator and joint effort vectors must have size 2 and point to valid data. + * To call this method it is not required that all other data vectors contain valid data, and can even remain + * empty. + */ + void jointToActuator(const EffortJointData& jnt_data, EffortActuatorData& act_data); + + /** + * \brief Transform \e velocity variables from joint to actuator space. + * \param[in] jnt_data Joint-space variables. + * \param[out] act_data Actuator-space variables. + * \pre Actuator and joint velocity vectors must have size 2 and point to valid data. + * To call this method it is not required that all other data vectors contain valid data, and can even remain + * empty. + */ + void jointToActuator(const VelocityJointData& jnt_data, VelocityActuatorData& act_data); + + /** + * \brief Transform \e position variables from joint to actuator space. + * \param[in] jnt_data Joint-space variables. + * \param[out] act_data Actuator-space variables. + * \pre Actuator and joint position vectors must have size 2 and point to valid data. + * To call this method it is not required that all other data vectors contain valid data, and can even remain + * empty. + */ + void jointToActuator(const PositionJointData& jnt_data, PositionActuatorData& act_data); + + std::size_t numActuators() const + { + return 2; + } + std::size_t numJoints() const + { + return 2; + } + + const std::vector& getActuatorReduction() const + { + return act_reduction_; + } + const std::vector& getJointReduction() const + { + return jnt_reduction_; + } + const std::vector& getJointOffset() const + { + return jnt_offset_; + } + + protected: + std::vector act_reduction_; + std::vector jnt_reduction_; + std::vector jnt_offset_; }; inline FourBarLinkageTransmission::FourBarLinkageTransmission(const std::vector& actuator_reduction, const std::vector& joint_reduction, const std::vector& joint_offset) - : Transmission(), - act_reduction_(actuator_reduction), - jnt_reduction_(joint_reduction), - jnt_offset_(joint_offset) + : Transmission(), act_reduction_(actuator_reduction), jnt_reduction_(joint_reduction), jnt_offset_(joint_offset) { - if (numActuators() != act_reduction_.size() || - numJoints() != jnt_reduction_.size() || - numJoints() != jnt_offset_.size()) - { - throw TransmissionInterfaceException("Reduction and offset vectors of a four-bar linkage transmission must have size 2."); - } - if (0.0 == act_reduction_[0] || - 0.0 == act_reduction_[1] || - 0.0 == jnt_reduction_[0] || - 0.0 == jnt_reduction_[1]) - { - throw TransmissionInterfaceException("Transmission reduction ratios cannot be zero."); - } + if (numActuators() != act_reduction_.size() || numJoints() != jnt_reduction_.size() || + numJoints() != jnt_offset_.size()) + { + throw TransmissionInterfaceException("Reduction and offset vectors of a four-bar linkage transmission must " + "have size 2."); + } + if (0.0 == act_reduction_[0] || 0.0 == act_reduction_[1] || 0.0 == jnt_reduction_[0] || 0.0 == jnt_reduction_[1]) + { + throw TransmissionInterfaceException("Transmission reduction ratios cannot be zero."); + } } -inline void FourBarLinkageTransmission::actuatorToJointEffort(const ActuatorData& act_data, - JointData& jnt_data) +inline void FourBarLinkageTransmission::actuatorToJoint(const EffortActuatorData& act_data, EffortJointData& jnt_data) { - assert(numActuators() == act_data.effort.size() && numJoints() == jnt_data.effort.size()); - assert(act_data.effort[0] && act_data.effort[1] && jnt_data.effort[0] && jnt_data.effort[1]); + assert(numActuators() == act_data.effort.size() && numJoints() == jnt_data.effort.size()); + assert(act_data.effort[0] && act_data.effort[1] && jnt_data.effort[0] && jnt_data.effort[1]); - std::vector& ar = act_reduction_; - std::vector& jr = jnt_reduction_; + std::vector& ar = act_reduction_; + std::vector& jr = jnt_reduction_; - *jnt_data.effort[0] = jr[0] * (*act_data.effort[0] * ar[0]); - *jnt_data.effort[1] = jr[1] * (*act_data.effort[1] * ar[1] - *act_data.effort[0] * ar[0] * jr[0]); + *jnt_data.effort[0] = jr[0] * (*act_data.effort[0] * ar[0]); + *jnt_data.effort[1] = jr[1] * (*act_data.effort[1] * ar[1] - *act_data.effort[0] * ar[0] * jr[0]); } -inline void FourBarLinkageTransmission::actuatorToJointVelocity(const ActuatorData& act_data, - JointData& jnt_data) +inline void FourBarLinkageTransmission::actuatorToJoint(const VelocityActuatorData& act_data, VelocityJointData& jnt_data) { - assert(numActuators() == act_data.velocity.size() && numJoints() == jnt_data.velocity.size()); - assert(act_data.velocity[0] && act_data.velocity[1] && jnt_data.velocity[0] && jnt_data.velocity[1]); + assert(numActuators() == act_data.velocity.size() && numJoints() == jnt_data.velocity.size()); + assert(act_data.velocity[0] && act_data.velocity[1] && jnt_data.velocity[0] && jnt_data.velocity[1]); - std::vector& ar = act_reduction_; - std::vector& jr = jnt_reduction_; + std::vector& ar = act_reduction_; + std::vector& jr = jnt_reduction_; - *jnt_data.velocity[0] = *act_data.velocity[0] / (jr[0] * ar[0]); - *jnt_data.velocity[1] = (*act_data.velocity[1] / ar[1] - *act_data.velocity[0] / (jr[0] * ar[0])) / jr[1]; + *jnt_data.velocity[0] = *act_data.velocity[0] / (jr[0] * ar[0]); + *jnt_data.velocity[1] = (*act_data.velocity[1] / ar[1] - *act_data.velocity[0] / (jr[0] * ar[0])) / jr[1]; } -inline void FourBarLinkageTransmission::actuatorToJointPosition(const ActuatorData& act_data, - JointData& jnt_data) +inline void FourBarLinkageTransmission::actuatorToJoint(const PositionActuatorData& act_data, + PositionJointData& jnt_data) { - assert(numActuators() == act_data.position.size() && numJoints() == jnt_data.position.size()); - assert(act_data.position[0] && act_data.position[1] && jnt_data.position[0] && jnt_data.position[1]); + assert(numActuators() == act_data.position.size() && numJoints() == jnt_data.position.size()); + assert(act_data.position[0] && act_data.position[1] && jnt_data.position[0] && jnt_data.position[1]); - std::vector& ar = act_reduction_; - std::vector& jr = jnt_reduction_; + std::vector& ar = act_reduction_; + std::vector& jr = jnt_reduction_; - *jnt_data.position[0] = *act_data.position[0] /(jr[0] * ar[0]) + jnt_offset_[0]; - *jnt_data.position[1] = (*act_data.position[1] / ar[1] - *act_data.position[0] / (jr[0] * ar[0])) / jr[1] - + jnt_offset_[1]; + *jnt_data.position[0] = *act_data.position[0] / (jr[0] * ar[0]) + jnt_offset_[0]; + *jnt_data.position[1] = + (*act_data.position[1] / ar[1] - *act_data.position[0] / (jr[0] * ar[0])) / jr[1] + jnt_offset_[1]; } -inline void FourBarLinkageTransmission::jointToActuatorEffort(const JointData& jnt_data, - ActuatorData& act_data) +inline void FourBarLinkageTransmission::jointToActuator(const EffortJointData& jnt_data, EffortActuatorData& act_data) { - assert(numActuators() == act_data.effort.size() && numJoints() == jnt_data.effort.size()); - assert(act_data.effort[0] && act_data.effort[1] && jnt_data.effort[0] && jnt_data.effort[1]); + assert(numActuators() == act_data.effort.size() && numJoints() == jnt_data.effort.size()); + assert(act_data.effort[0] && act_data.effort[1] && jnt_data.effort[0] && jnt_data.effort[1]); - std::vector& ar = act_reduction_; - std::vector& jr = jnt_reduction_; + std::vector& ar = act_reduction_; + std::vector& jr = jnt_reduction_; - *act_data.effort[0] = *jnt_data.effort[0] / (ar[0] * jr[0]); - *act_data.effort[1] = (*jnt_data.effort[0] + *jnt_data.effort[1] / jr[1]) / ar[1]; + *act_data.effort[0] = *jnt_data.effort[0] / (ar[0] * jr[0]); + *act_data.effort[1] = (*jnt_data.effort[0] + *jnt_data.effort[1] / jr[1]) / ar[1]; } -inline void FourBarLinkageTransmission::jointToActuatorVelocity(const JointData& jnt_data, - ActuatorData& act_data) +inline void FourBarLinkageTransmission::jointToActuator(const VelocityJointData& jnt_data, + VelocityActuatorData& act_data) { - assert(numActuators() == act_data.velocity.size() && numJoints() == jnt_data.velocity.size()); - assert(act_data.velocity[0] && act_data.velocity[1] && jnt_data.velocity[0] && jnt_data.velocity[1]); + assert(numActuators() == act_data.velocity.size() && numJoints() == jnt_data.velocity.size()); + assert(act_data.velocity[0] && act_data.velocity[1] && jnt_data.velocity[0] && jnt_data.velocity[1]); - std::vector& ar = act_reduction_; - std::vector& jr = jnt_reduction_; + std::vector& ar = act_reduction_; + std::vector& jr = jnt_reduction_; - *act_data.velocity[0] = *jnt_data.velocity[0] * jr[0] * ar[0]; - *act_data.velocity[1] = (*jnt_data.velocity[0] + *jnt_data.velocity[1] * jr[1]) * ar[1]; + *act_data.velocity[0] = *jnt_data.velocity[0] * jr[0] * ar[0]; + *act_data.velocity[1] = (*jnt_data.velocity[0] + *jnt_data.velocity[1] * jr[1]) * ar[1]; } -inline void FourBarLinkageTransmission::jointToActuatorPosition(const JointData& jnt_data, - ActuatorData& act_data) +inline void FourBarLinkageTransmission::jointToActuator(const PositionJointData& jnt_data, + PositionActuatorData& act_data) { - assert(numActuators() == act_data.position.size() && numJoints() == jnt_data.position.size()); - assert(act_data.position[0] && act_data.position[1] && jnt_data.position[0] && jnt_data.position[1]); + assert(numActuators() == act_data.position.size() && numJoints() == jnt_data.position.size()); + assert(act_data.position[0] && act_data.position[1] && jnt_data.position[0] && jnt_data.position[1]); - std::vector& ar = act_reduction_; - std::vector& jr = jnt_reduction_; + std::vector& ar = act_reduction_; + std::vector& jr = jnt_reduction_; - double jnt_pos_off[2] = {*jnt_data.position[0] - jnt_offset_[0], *jnt_data.position[1] - jnt_offset_[1]}; + double jnt_pos_off[2] = { *jnt_data.position[0] - jnt_offset_[0], *jnt_data.position[1] - jnt_offset_[1] }; - *act_data.position[0] = jnt_pos_off[0] * jr[0] * ar[0]; - *act_data.position[1] = (jnt_pos_off[0] + jnt_pos_off[1] * jr[1]) * ar[1]; + *act_data.position[0] = jnt_pos_off[0] * jr[0] * ar[0]; + *act_data.position[1] = (jnt_pos_off[0] + jnt_pos_off[1] * jr[1]) * ar[1]; } -} // transmission_interface +} // transmission_interface -#endif // TRANSMISSION_INTERFACE_FOUR_BAR_LINKAGE_TRANSMISSION_H +#endif // TRANSMISSION_INTERFACE_FOUR_BAR_LINKAGE_TRANSMISSION_H diff --git a/transmission_interface/include/transmission_interface/joint_data.h b/transmission_interface/include/transmission_interface/joint_data.h new file mode 100644 index 0000000..a42d320 --- /dev/null +++ b/transmission_interface/include/transmission_interface/joint_data.h @@ -0,0 +1,89 @@ +#pragma once + +#include +#include + +namespace transmission_interface +{ +struct PositionJointData +{ + std::vector position; +}; + +struct VelocityJointData +{ + std::vector velocity; +}; + +struct EffortJointData +{ + std::vector effort; +}; + +class JointDataBase +{ + public: + virtual bool empty() const = 0; + virtual bool hasSize(std::size_t size) const = 0; + virtual bool valid() const = 0; + + inline bool hasValidPointers(const std::vector& data) const + { + return std::all_of(data.cbegin(), data.cend(), [](const double* ptr) { return ptr; }); + } +}; + +template +struct JointDataContainer : public JointDataBase, public Interfaces... +{ + public: + JointDataContainer() + { + } + JointDataContainer(Interfaces... ifaces) : Interfaces(ifaces)... + { + } + virtual bool empty() const = 0; + virtual bool hasSize(std::size_t size) const = 0; + virtual bool valid() const = 0; +}; + +/** + * \brief Contains pointers to raw data representing the position, velocity and acceleration of a transmission's + * joints. + */ +class JointData : public JointDataContainer +{ + public: + bool empty() const override + { + return position.empty() && velocity.empty() && effort.empty(); + } + + bool hasSize(std::size_t size) const override + { + if (not position.empty() and position.size() != size) + { + return false; + } + + if (not velocity.empty() and velocity.size() != size) + { + return false; + } + + if (not effort.empty() and effort.size() != size) + { + return false; + } + + return true; + } + + bool valid() const override + { + return hasValidPointers(position) and hasValidPointers(velocity) and hasValidPointers(effort); + } +}; + +} // namespace transmission_interface \ No newline at end of file diff --git a/transmission_interface/include/transmission_interface/simple_transmission.h b/transmission_interface/include/transmission_interface/simple_transmission.h index 0fec011..83fceef 100644 --- a/transmission_interface/include/transmission_interface/simple_transmission.h +++ b/transmission_interface/include/transmission_interface/simple_transmission.h @@ -39,7 +39,6 @@ namespace transmission_interface { - /** * \brief Implementation of a simple reducer transmission. * @@ -94,152 +93,154 @@ namespace transmission_interface */ class SimpleTransmission : public Transmission { -public: - /** - * \param reduction Reduction ratio. - * \param joint_offset Joint position offset used in the position mappings. - * \pre Nonzero reduction value. - */ - SimpleTransmission(const double reduction, - const double joint_offset = 0.0); - - /** - * \brief Transform \e effort variables from actuator to joint space. - * \param[in] act_data Actuator-space variables. - * \param[out] jnt_data Joint-space variables. - * \pre Actuator and joint effort vectors must have size 1 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. - */ - void actuatorToJointEffort(const ActuatorData& act_data, - JointData& jnt_data); - - /** - * \brief Transform \e velocity variables from actuator to joint space. - * \param[in] act_data Actuator-space variables. - * \param[out] jnt_data Joint-space variables. - * \pre Actuator and joint velocity vectors must have size 1 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. - */ - void actuatorToJointVelocity(const ActuatorData& act_data, - JointData& jnt_data); - - /** - * \brief Transform \e position variables from actuator to joint space. - * \param[in] act_data Actuator-space variables. - * \param[out] jnt_data Joint-space variables. - * \pre Actuator and joint position vectors must have size 1 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. - */ - void actuatorToJointPosition(const ActuatorData& act_data, - JointData& jnt_data); - - /** - * \brief Transform \e effort variables from joint to actuator space. - * \param[in] jnt_data Joint-space variables. - * \param[out] act_data Actuator-space variables. - * \pre Actuator and joint effort vectors must have size 1 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. - */ - void jointToActuatorEffort(const JointData& jnt_data, - ActuatorData& act_data); - - /** - * \brief Transform \e velocity variables from joint to actuator space. - * \param[in] jnt_data Joint-space variables. - * \param[out] act_data Actuator-space variables. - * \pre Actuator and joint velocity vectors must have size 1 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. - */ - void jointToActuatorVelocity(const JointData& jnt_data, - ActuatorData& act_data); - - /** - * \brief Transform \e position variables from joint to actuator space. - * \param[in] jnt_data Joint-space variables. - * \param[out] act_data Actuator-space variables. - * \pre Actuator and joint position vectors must have size 1 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. - */ - void jointToActuatorPosition(const JointData& jnt_data, - ActuatorData& act_data); - - std::size_t numActuators() const {return 1;} - std::size_t numJoints() const {return 1;} - - double getActuatorReduction() const {return reduction_;} - double getJointOffset() const {return jnt_offset_;} - -private: - double reduction_; - double jnt_offset_; + public: + /** + * \param reduction Reduction ratio. + * \param joint_offset Joint position offset used in the position mappings. + * \pre Nonzero reduction value. + */ + SimpleTransmission(const double reduction, const double joint_offset = 0.0); + + /** + * \brief Transform \e effort variables from actuator to joint space. + * \param[in] act_data Actuator-space variables. + * \param[out] jnt_data Joint-space variables. + * \pre Actuator and joint effort vectors must have size 1 and point to valid data. + * To call this method it is not required that all other data vectors contain valid data, and can even remain + * empty. + */ + void actuatorToJoint(const EffortActuatorData& act_data, EffortJointData& jnt_data); + + /** + * \brief Transform \e velocity variables from actuator to joint space. + * \param[in] act_data Actuator-space variables. + * \param[out] jnt_data Joint-space variables. + * \pre Actuator and joint velocity vectors must have size 1 and point to valid data. + * To call this method it is not required that all other data vectors contain valid data, and can even remain + * empty. + */ + void actuatorToJoint(const VelocityActuatorData& act_data, VelocityJointData& jnt_data); + + /** + * \brief Transform \e position variables from actuator to joint space. + * \param[in] act_data Actuator-space variables. + * \param[out] jnt_data Joint-space variables. + * \pre Actuator and joint position vectors must have size 1 and point to valid data. + * To call this method it is not required that all other data vectors contain valid data, and can even remain + * empty. + */ + void actuatorToJoint(const PositionActuatorData& act_data, PositionJointData& jnt_data); + + /** + * \brief Transform \e effort variables from joint to actuator space. + * \param[in] jnt_data Joint-space variables. + * \param[out] act_data Actuator-space variables. + * \pre Actuator and joint effort vectors must have size 1 and point to valid data. + * To call this method it is not required that all other data vectors contain valid data, and can even remain + * empty. + */ + void jointToActuator(const EffortJointData& jnt_data, EffortActuatorData& act_data); + + /** + * \brief Transform \e velocity variables from joint to actuator space. + * \param[in] jnt_data Joint-space variables. + * \param[out] act_data Actuator-space variables. + * \pre Actuator and joint velocity vectors must have size 1 and point to valid data. + * To call this method it is not required that all other data vectors contain valid data, and can even remain + * empty. + */ + void jointToActuator(const VelocityJointData& jnt_data, VelocityActuatorData& act_data); + + /** + * \brief Transform \e position variables from joint to actuator space. + * \param[in] jnt_data Joint-space variables. + * \param[out] act_data Actuator-space variables. + * \pre Actuator and joint position vectors must have size 1 and point to valid data. + * To call this method it is not required that all other data vectors contain valid data, and can even remain + * empty. + */ + void jointToActuator(const PositionJointData& jnt_data, PositionActuatorData& act_data); + + std::size_t numActuators() const + { + return 1; + } + std::size_t numJoints() const + { + return 1; + } + + double getActuatorReduction() const + { + return reduction_; + } + double getJointOffset() const + { + return jnt_offset_; + } + + private: + double reduction_; + double jnt_offset_; }; -inline SimpleTransmission::SimpleTransmission(const double reduction, - const double joint_offset) - : Transmission(), - reduction_(reduction), - jnt_offset_(joint_offset) +inline SimpleTransmission::SimpleTransmission(const double reduction, const double joint_offset) + : Transmission(), reduction_(reduction), jnt_offset_(joint_offset) { - if (0.0 == reduction_) - { - throw TransmissionInterfaceException("Transmission reduction ratio cannot be zero."); - } + if (0.0 == reduction_) + { + throw TransmissionInterfaceException("Transmission reduction ratio cannot be zero."); + } } -inline void SimpleTransmission::actuatorToJointEffort(const ActuatorData& act_data, - JointData& jnt_data) +inline void SimpleTransmission::actuatorToJoint(const EffortActuatorData& act_data, EffortJointData& jnt_data) { - assert(numActuators() == act_data.effort.size() && numJoints() == jnt_data.effort.size()); - assert(act_data.effort[0] && jnt_data.effort[0]); + assert(numActuators() == act_data.effort.size() && numJoints() == jnt_data.effort.size()); + assert(act_data.effort[0] && jnt_data.effort[0]); - *jnt_data.effort[0] = *act_data.effort[0] * reduction_; + *jnt_data.effort[0] = *act_data.effort[0] * reduction_; } -inline void SimpleTransmission::actuatorToJointVelocity(const ActuatorData& act_data, - JointData& jnt_data) +inline void SimpleTransmission::actuatorToJoint(const VelocityActuatorData& act_data, VelocityJointData& jnt_data) { - assert(numActuators() == act_data.velocity.size() && numJoints() == jnt_data.velocity.size()); - assert(act_data.velocity[0] && jnt_data.velocity[0]); + assert(numActuators() == act_data.velocity.size() && numJoints() == jnt_data.velocity.size()); + assert(act_data.velocity[0] && jnt_data.velocity[0]); - *jnt_data.velocity[0] = *act_data.velocity[0] / reduction_; + *jnt_data.velocity[0] = *act_data.velocity[0] / reduction_; } -inline void SimpleTransmission::actuatorToJointPosition(const ActuatorData& act_data, - JointData& jnt_data) +inline void SimpleTransmission::actuatorToJoint(const PositionActuatorData& act_data, PositionJointData& jnt_data) { - assert(numActuators() == act_data.position.size() && numJoints() == jnt_data.position.size()); - assert(act_data.position[0] && jnt_data.position[0]); + assert(numActuators() == act_data.position.size() && numJoints() == jnt_data.position.size()); + assert(act_data.position[0] && jnt_data.position[0]); - *jnt_data.position[0] = *act_data.position[0] / reduction_ + jnt_offset_; + *jnt_data.position[0] = *act_data.position[0] / reduction_ + jnt_offset_; } -inline void SimpleTransmission::jointToActuatorEffort(const JointData& jnt_data, - ActuatorData& act_data) +inline void SimpleTransmission::jointToActuator(const EffortJointData& jnt_data, EffortActuatorData& act_data) { - assert(numActuators() == act_data.effort.size() && numJoints() == jnt_data.effort.size()); - assert(act_data.effort[0] && jnt_data.effort[0]); + assert(numActuators() == act_data.effort.size() && numJoints() == jnt_data.effort.size()); + assert(act_data.effort[0] && jnt_data.effort[0]); - *act_data.effort[0] = *jnt_data.effort[0] / reduction_; + *act_data.effort[0] = *jnt_data.effort[0] / reduction_; } -inline void SimpleTransmission::jointToActuatorVelocity(const JointData& jnt_data, - ActuatorData& act_data) +inline void SimpleTransmission::jointToActuator(const VelocityJointData& jnt_data, VelocityActuatorData& act_data) { - assert(numActuators() == act_data.velocity.size() && numJoints() == jnt_data.velocity.size()); - assert(act_data.velocity[0] && jnt_data.velocity[0]); + assert(numActuators() == act_data.velocity.size() && numJoints() == jnt_data.velocity.size()); + assert(act_data.velocity[0] && jnt_data.velocity[0]); - *act_data.velocity[0] = *jnt_data.velocity[0] * reduction_; + *act_data.velocity[0] = *jnt_data.velocity[0] * reduction_; } -inline void SimpleTransmission::jointToActuatorPosition(const JointData& jnt_data, - ActuatorData& act_data) +inline void SimpleTransmission::jointToActuator(const PositionJointData& jnt_data, PositionActuatorData& act_data) { - assert(numActuators() == act_data.position.size() && numJoints() == jnt_data.position.size()); - assert(act_data.position[0] && jnt_data.position[0]); + assert(numActuators() == act_data.position.size() && numJoints() == jnt_data.position.size()); + assert(act_data.position[0] && jnt_data.position[0]); - *act_data.position[0] = (*jnt_data.position[0] - jnt_offset_) * reduction_; + *act_data.position[0] = (*jnt_data.position[0] - jnt_offset_) * reduction_; } -} // transmission_interface +} // transmission_interface -#endif // TRANSMISSION_INTERFACE_SIMPLE_TRANSMISSION_H +#endif // TRANSMISSION_INTERFACE_SIMPLE_TRANSMISSION_H diff --git a/transmission_interface/include/transmission_interface/transmission.h b/transmission_interface/include/transmission_interface/transmission.h index 787c960..111817d 100644 --- a/transmission_interface/include/transmission_interface/transmission.h +++ b/transmission_interface/include/transmission_interface/transmission.h @@ -31,40 +31,18 @@ #define TRANSMISSION_INTERFACE_TRANSMISSION_H #include +#include #include +#include +#include #include -#include - namespace transmission_interface { - /** * @defgroup transmission_types Transmission types */ -/** - * \brief Contains pointers to raw data representing the position, velocity and acceleration of a transmission's - * actuators. - */ -struct ActuatorData -{ - std::vector position; - std::vector velocity; - std::vector effort; -}; - -/** - * \brief Contains pointers to raw data representing the position, velocity and acceleration of a transmission's - * joints. - */ -struct JointData -{ - std::vector position; - std::vector velocity; - std::vector effort; -}; - /** * \brief Abstract base class for representing mechanical transmissions. * @@ -86,84 +64,80 @@ struct JointData */ class Transmission { -public: - virtual ~Transmission() {} - - /** - * \brief Transform \e effort variables from actuator to joint space. - * \param[in] act_data Actuator-space variables. - * \param[out] jnt_data Joint-space variables. - * \pre All non-empty vectors must contain valid data and their size should be consistent with the number of - * transmission actuators and joints. - * Data vectors not used in this map can remain empty. - */ - virtual void actuatorToJointEffort(const ActuatorData& act_data, - JointData& jnt_data) = 0; - - /** - * \brief Transform \e velocity variables from actuator to joint space. - * \param[in] act_data Actuator-space variables. - * \param[out] jnt_data Joint-space variables. - * \pre All non-empty vectors must contain valid data and their size should be consistent with the number of - * transmission actuators and joints. - * Data vectors not used in this map can remain empty. - */ - virtual void actuatorToJointVelocity(const ActuatorData& act_data, - JointData& jnt_data) = 0; - - /** - * \brief Transform \e position variables from actuator to joint space. - * \param[in] act_data Actuator-space variables. - * \param[out] jnt_data Joint-space variables. - * \pre All non-empty vectors must contain valid data and their size should be consistent with the number of - * transmission actuators and joints. - * Data vectors not used in this map can remain empty. - */ - virtual void actuatorToJointPosition(const ActuatorData& act_data, - JointData& jnt_data) = 0; - - /** - * \brief Transform \e effort variables from joint to actuator space. - * \param[in] jnt_data Joint-space variables. - * \param[out] act_data Actuator-space variables. - * \pre All non-empty vectors must contain valid data and their size should be consistent with the number of - * transmission actuators and joints. - * Data vectors not used in this map can remain empty. - */ - virtual void jointToActuatorEffort(const JointData& jnt_data, - ActuatorData& act_data) = 0; - - /** - * \brief Transform \e velocity variables from joint to actuator space. - * \param[in] jnt_data Joint-space variables. - * \param[out] act_data Actuator-space variables. - * \pre All non-empty vectors must contain valid data and their size should be consistent with the number of - * transmission actuators and joints. - * Data vectors not used in this map can remain empty. - */ - virtual void jointToActuatorVelocity(const JointData& jnt_data, - ActuatorData& act_data) = 0; - - /** - * \brief Transform \e position variables from joint to actuator space. - * \param[in] jnt_data Joint-space variables. - * \param[out] act_data Actuator-space variables. - * \pre All non-empty vectors must contain valid data and their size should be consistent with the number of - * transmission actuators and joints. - * Data vectors not used in this map can remain empty. - */ - virtual void jointToActuatorPosition(const JointData& jnt_data, - ActuatorData& act_data) = 0; - - /** \return Number of actuators managed by transmission, ie. the dimension of the actuator space. */ - virtual std::size_t numActuators() const = 0; - - /** \return Number of joints managed by transmission, ie. the dimension of the joint space. */ - virtual std::size_t numJoints() const = 0; + public: + virtual ~Transmission() + { + } + + /** + * \brief Transform \e effort variables from actuator to joint space. + * \param[in] act_data Actuator-space variables. + * \param[out] jnt_data Joint-space variables. + * \pre All non-empty vectors must contain valid data and their size should be consistent with the number of + * transmission actuators and joints. + * Data vectors not used in this map can remain empty. + */ + virtual void actuatorToJoint(const EffortActuatorData& act_data, EffortJointData& jnt_data) = 0; + + /** + * \brief Transform \e velocity variables from actuator to joint space. + * \param[in] act_data Actuator-space variables. + * \param[out] jnt_data Joint-space variables. + * \pre All non-empty vectors must contain valid data and their size should be consistent with the number of + * transmission actuators and joints. + * Data vectors not used in this map can remain empty. + */ + virtual void actuatorToJoint(const VelocityActuatorData& act_data, VelocityJointData& jnt_data) = 0; + + /** + * \brief Transform \e position variables from actuator to joint space. + * \param[in] act_data Actuator-space variables. + * \param[out] jnt_data Joint-space variables. + * \pre All non-empty vectors must contain valid data and their size should be consistent with the number of + * transmission actuators and joints. + * Data vectors not used in this map can remain empty. + */ + virtual void actuatorToJoint(const PositionActuatorData& act_data, PositionJointData& jnt_data) = 0; + + /** + * \brief Transform \e effort variables from joint to actuator space. + * \param[in] jnt_data Joint-space variables. + * \param[out] act_data Actuator-space variables. + * \pre All non-empty vectors must contain valid data and their size should be consistent with the number of + * transmission actuators and joints. + * Data vectors not used in this map can remain empty. + */ + virtual void jointToActuator(const EffortJointData& jnt_data, EffortActuatorData& act_data) = 0; + + /** + * \brief Transform \e velocity variables from joint to actuator space. + * \param[in] jnt_data Joint-space variables. + * \param[out] act_data Actuator-space variables. + * \pre All non-empty vectors must contain valid data and their size should be consistent with the number of + * transmission actuators and joints. + * Data vectors not used in this map can remain empty. + */ + virtual void jointToActuator(const VelocityJointData& jnt_data, VelocityActuatorData& act_data) = 0; + + /** + * \brief Transform \e position variables from joint to actuator space. + * \param[in] jnt_data Joint-space variables. + * \param[out] act_data Actuator-space variables. + * \pre All non-empty vectors must contain valid data and their size should be consistent with the number of + * transmission actuators and joints. + * Data vectors not used in this map can remain empty. + */ + virtual void jointToActuator(const PositionJointData& jnt_data, PositionActuatorData& act_data) = 0; + + /** \return Number of actuators managed by transmission, ie. the dimension of the actuator space. */ + virtual std::size_t numActuators() const = 0; + + /** \return Number of joints managed by transmission, ie. the dimension of the joint space. */ + virtual std::size_t numJoints() const = 0; }; typedef std::shared_ptr TransmissionSharedPtr; -} // transmission_interface +} // transmission_interface -#endif // TRANSMISSION_INTERFACE_TRANSMISSION_H +#endif // TRANSMISSION_INTERFACE_TRANSMISSION_H diff --git a/transmission_interface/include/transmission_interface/transmission_interface.h b/transmission_interface/include/transmission_interface/transmission_interface.h index 0db43d5..57ed8e5 100644 --- a/transmission_interface/include/transmission_interface/transmission_interface.h +++ b/transmission_interface/include/transmission_interface/transmission_interface.h @@ -35,289 +35,273 @@ #include #include +#include +#include #include #include namespace transmission_interface { - /** * \brief Handle for propagating a single map (position, velocity, or effort) on a single transmission * (eg. actuator to joint effort for a simple reducer). + * \tparam ActuatorDataType extends ActuatorDataBase + * \tparam JointDataType extends JointDataBase */ +template class TransmissionHandle { -public: - /** \return Transmission name. */ - std::string getName() const {return name_;} - -protected: - std::string name_; - Transmission* transmission_; - ActuatorData actuator_data_; - JointData joint_data_; - - /** - * \param name %Transmission name. - * \param transmission Pointer to transmission instance. - * \param actuator_data Actuator-space variables. - * \param joint_data Joint-space variables. - * \note The lifecycle of the pointed-to instances passed as parameters is not handled by this class. - * \pre Valid transmission pointer. Actuator and joint variable vectors required by this handle must contain valid - * data and their size should be consistent with the number of transmission actuators and joints. - * Data vectors not used by this handle can remain empty. - */ - TransmissionHandle(const std::string& name, - Transmission* transmission, - const ActuatorData& actuator_data, - const JointData& joint_data) - : name_(name), - transmission_(transmission), - actuator_data_(actuator_data), - joint_data_(joint_data) - { - // Precondition: Valid transmission - if (!transmission_) + public: + /** \return Transmission name. */ + std::string getName() const { - throw TransmissionInterfaceException("Unspecified transmission."); + return name_; } - // Catch trivial error: All data vectors are empty (handle can't do anything without data) - if (actuator_data.position.empty() && actuator_data.velocity.empty() && actuator_data.effort.empty() && - joint_data.position.empty() && joint_data.velocity.empty() && joint_data.effort.empty()) + protected: + std::string name_; + Transmission* transmission_; + ActuatorDataType actuator_data_; + JointDataType joint_data_; + + /** + * \param name %Transmission name. + * \param transmission Pointer to transmission instance. + * \param actuator_data Actuator-space variables. + * \param joint_data Joint-space variables. + * \note The lifecycle of the pointed-to instances passed as parameters is not handled by this class. + * \pre Valid transmission pointer. Actuator and joint variable vectors required by this handle must contain valid + * data and their size should be consistent with the number of transmission actuators and joints. + * Data vectors not used by this handle can remain empty. + */ + TransmissionHandle(const std::string& name, Transmission* transmission, const ActuatorDataType& actuator_data, + const JointDataType& joint_data) + : name_(name), transmission_(transmission), actuator_data_(actuator_data), joint_data_(joint_data) { - throw TransmissionInterfaceException("All data vectors are empty. Transmission instance can't do anything!."); + // Precondition: Valid transmission + if (!transmission_) + { + throw TransmissionInterfaceException("Unspecified transmission."); + } + + // Catch trivial error: All data vectors are empty (handle can't do anything without data) + if (actuator_data.empty() && joint_data.empty()) + { + throw TransmissionInterfaceException("All data vectors are empty. Transmission instance can't do " + "anything!."); + } + + // Precondition: All non-empty data vectors must have sizes consistent with the transmission + if (!actuator_data.hasSize(transmission_->numActuators())) + { + throw TransmissionInterfaceException("Actuator data size does not match transmission."); + } + + if (!joint_data.hasSize(transmission_->numActuators())) + { + throw TransmissionInterfaceException("Joint data size does not match transmission."); + } + + // Precondition: Valid pointers to raw data + if (!actuator_data.valid()) + { + throw TransmissionInterfaceException("Actuator data contains null pointers."); + } + + if (!joint_data.valid()) + { + throw TransmissionInterfaceException("Joint data contains null pointers."); + } } - // Precondition: All non-empty data vectors must have sizes consistent with the transmission - if (!actuator_data.position.empty() && actuator_data.position.size() != transmission_->numActuators()) - { - throw TransmissionInterfaceException("Actuator position data size does not match transmission."); - } - if (!actuator_data.velocity.empty() && actuator_data.velocity.size() != transmission_->numActuators()) + private: + static bool hasValidPointers(const std::vector& data) { - throw TransmissionInterfaceException("Actuator velocity data size does not match transmission."); - } - if (!actuator_data.effort.empty() && actuator_data.effort.size() != transmission_->numActuators()) - { - throw TransmissionInterfaceException("Actuator effort data size does not match transmission."); + for (std::vector::const_iterator it = data.begin(); it != data.end(); ++it) + { + if (!(*it)) + { + return false; + } + } + return true; } +}; - if (!joint_data.position.empty() && joint_data.position.size() != transmission_->numJoints()) - { - throw TransmissionInterfaceException("Joint position data size does not match transmission."); - } - if (!joint_data.velocity.empty() && joint_data.velocity.size() != transmission_->numJoints()) - { - throw TransmissionInterfaceException("Joint velocity data size does not match transmission."); - } - if (!joint_data.effort.empty() && joint_data.effort.size() != transmission_->numJoints()) +/** + *\brief Handle for propagating actuator state (position, velocity and effort) to joint state for a given transmission. + */ +class ActuatorToJointStateHandle : public TransmissionHandle<> +{ + public: + /** \sa TransmissionHandle::TransmissionHandle */ + ActuatorToJointStateHandle(const std::string& name, Transmission* transmission, const ActuatorData& actuator_data, + const JointData& joint_data) + : TransmissionHandle(name, transmission, actuator_data, joint_data) { - throw TransmissionInterfaceException("Joint effort data size does not match transmission."); } - // Precondition: Valid pointers to raw data - if (!hasValidPointers(actuator_data.position)) - { - throw TransmissionInterfaceException("Actuator position data contains null pointers."); - } - if (!hasValidPointers(actuator_data.velocity)) - { - throw TransmissionInterfaceException("Actuator velocity data contains null pointers."); - } - if (!hasValidPointers(actuator_data.effort)) + /** \name Real-Time Safe Functions + *\{*/ + /** \brief Propagate actuator state to joint state for the stored transmission. */ + void propagate() { - throw TransmissionInterfaceException("Actuator effort data contains null pointers."); + transmission_->actuatorToJoint((PositionActuatorData)actuator_data_, joint_data_); + transmission_->actuatorToJoint((VelocityActuatorData)actuator_data_, joint_data_); + transmission_->actuatorToJoint((EffortActuatorData)actuator_data_, joint_data_); } + /*\}*/ +}; - if (!hasValidPointers(joint_data.position)) - { - throw TransmissionInterfaceException("Joint position data contains null pointers."); - } - if (!hasValidPointers(joint_data.velocity)) - { - throw TransmissionInterfaceException("Joint velocity data contains null pointers."); - } - if (!hasValidPointers(joint_data.effort)) +/** \brief Handle for propagating actuator positions to joint positions for a given transmission. */ +class ActuatorToJointPositionHandle : public TransmissionHandle<> +{ + public: + /** \sa TransmissionHandle::TransmissionHandle */ + ActuatorToJointPositionHandle(const std::string& name, Transmission* transmission, + const ActuatorData& actuator_data, const JointData& joint_data) + : TransmissionHandle(name, transmission, actuator_data, joint_data) { - throw TransmissionInterfaceException("Joint effort data contains null pointers."); } - } -private: - static bool hasValidPointers(const std::vector& data) - { - for (std::vector::const_iterator it = data.begin(); it != data.end(); ++it) + /** \name Real-Time Safe Functions + *\{*/ + /** \brief Propagate actuator positions to joint positions for the stored transmission. */ + void propagate() { - if (!(*it)) {return false;} + transmission_->actuatorToJoint((PositionActuatorData)actuator_data_, joint_data_); } - return true; - } -}; - -/** - *\brief Handle for propagating actuator state (position, velocity and effort) to joint state for a given transmission. - */ -class ActuatorToJointStateHandle : public TransmissionHandle -{ -public: - /** \sa TransmissionHandle::TransmissionHandle */ - ActuatorToJointStateHandle(const std::string& name, - Transmission* transmission, - const ActuatorData& actuator_data, - const JointData& joint_data) - : TransmissionHandle(name, transmission, actuator_data, joint_data) {} - - /** \name Real-Time Safe Functions - *\{*/ - /** \brief Propagate actuator state to joint state for the stored transmission. */ - void propagate() - { - transmission_->actuatorToJointPosition(actuator_data_, joint_data_); - transmission_->actuatorToJointVelocity(actuator_data_, joint_data_); - transmission_->actuatorToJointEffort( actuator_data_, joint_data_); - } - /*\}*/ -}; - - -/** \brief Handle for propagating actuator positions to joint positions for a given transmission. */ -class ActuatorToJointPositionHandle : public TransmissionHandle -{ -public: - /** \sa TransmissionHandle::TransmissionHandle */ - ActuatorToJointPositionHandle(const std::string& name, - Transmission* transmission, - const ActuatorData& actuator_data, - const JointData& joint_data) - : TransmissionHandle(name, transmission, actuator_data, joint_data) {} - - /** \name Real-Time Safe Functions - *\{*/ - /** \brief Propagate actuator positions to joint positions for the stored transmission. */ - void propagate() {transmission_->actuatorToJointPosition(actuator_data_, joint_data_);} - /*\}*/ + /*\}*/ }; - /** \brief Handle for propagating actuator velocities to joint velocities for a given transmission. */ -class ActuatorToJointVelocityHandle : public TransmissionHandle +class ActuatorToJointVelocityHandle : public TransmissionHandle<> { -public: - /** \sa TransmissionHandle::TransmissionHandle */ - ActuatorToJointVelocityHandle(const std::string& name, - Transmission* transmission, - const ActuatorData& actuator_data, - const JointData& joint_data) - : TransmissionHandle(name, transmission, actuator_data, joint_data) {} - - /** \name Real-Time Safe Functions - *\{*/ - /** \brief Propagate actuator velocities to joint velocities for the stored transmission. */ - void propagate() {transmission_->actuatorToJointVelocity(actuator_data_, joint_data_);} - /*\}*/ -}; + public: + /** \sa TransmissionHandle::TransmissionHandle */ + ActuatorToJointVelocityHandle(const std::string& name, Transmission* transmission, + const ActuatorData& actuator_data, const JointData& joint_data) + : TransmissionHandle(name, transmission, actuator_data, joint_data) + { + } + /** \name Real-Time Safe Functions + *\{*/ + /** \brief Propagate actuator velocities to joint velocities for the stored transmission. */ + void propagate() + { + transmission_->actuatorToJoint((VelocityActuatorData)actuator_data_, joint_data_); + } + /*\}*/ +}; /** \brief Handle for propagating actuator efforts to joint efforts for a given transmission. */ -class ActuatorToJointEffortHandle : public TransmissionHandle +class ActuatorToJointEffortHandle : public TransmissionHandle<> { -public: - /** \sa TransmissionHandle::TransmissionHandle */ - ActuatorToJointEffortHandle(const std::string& name, - Transmission* transmission, - const ActuatorData& actuator_data, - const JointData& joint_data) - : TransmissionHandle(name, transmission, actuator_data, joint_data) {} - - /** \name Real-Time Safe Functions - *\{*/ - /** \brief Propagate actuator efforts to joint efforts for the stored transmission. */ - void propagate() {transmission_->actuatorToJointEffort(actuator_data_, joint_data_);} - /*\}*/ -}; + public: + /** \sa TransmissionHandle::TransmissionHandle */ + ActuatorToJointEffortHandle(const std::string& name, Transmission* transmission, const ActuatorData& actuator_data, + const JointData& joint_data) + : TransmissionHandle(name, transmission, actuator_data, joint_data) + { + } + /** \name Real-Time Safe Functions + *\{*/ + /** \brief Propagate actuator efforts to joint efforts for the stored transmission. */ + void propagate() + { + transmission_->actuatorToJoint((EffortActuatorData)actuator_data_, joint_data_); + } + /*\}*/ +}; /** *\brief Handle for propagating joint state (position, velocity and effort) to actuator state for a given transmission. */ -class JointToActuatorStateHandle : public TransmissionHandle +class JointToActuatorStateHandle : public TransmissionHandle<> { -public: - /** \sa TransmissionHandle::TransmissionHandle */ - JointToActuatorStateHandle(const std::string& name, - Transmission* transmission, - const ActuatorData& actuator_data, - const JointData& joint_data) - : TransmissionHandle(name, transmission, actuator_data, joint_data) {} - - /** \name Real-Time Safe Functions - *\{*/ - /** \brief Propagate joint state to actuator state for the stored transmission. */ - void propagate() - { - transmission_->jointToActuatorPosition(joint_data_, actuator_data_); - transmission_->jointToActuatorVelocity(joint_data_, actuator_data_); - transmission_->jointToActuatorEffort( joint_data_, actuator_data_); - } - /*\}*/ -}; + public: + /** \sa TransmissionHandle::TransmissionHandle */ + JointToActuatorStateHandle(const std::string& name, Transmission* transmission, const ActuatorData& actuator_data, + const JointData& joint_data) + : TransmissionHandle(name, transmission, actuator_data, joint_data) + { + } + /** \name Real-Time Safe Functions + *\{*/ + /** \brief Propagate joint state to actuator state for the stored transmission. */ + void propagate() + { + transmission_->jointToActuator((PositionJointData)joint_data_, actuator_data_); + transmission_->jointToActuator((VelocityJointData)joint_data_, actuator_data_); + transmission_->jointToActuator((EffortJointData)joint_data_, actuator_data_); + } + /*\}*/ +}; /** \brief Handle for propagating joint positions to actuator positions for a given transmission. */ -class JointToActuatorPositionHandle : public TransmissionHandle +class JointToActuatorPositionHandle : public TransmissionHandle<> { -public: - /** \sa TransmissionHandle::TransmissionHandle */ - JointToActuatorPositionHandle(const std::string& name, - Transmission* transmission, - const ActuatorData& actuator_data, - const JointData& joint_data) - : TransmissionHandle(name, transmission, actuator_data, joint_data) {} - - /** \name Real-Time Safe Functions - *\{*/ - /** \brief Propagate joint positions to actuator positions for the stored transmission. */ - void propagate() {transmission_->jointToActuatorPosition(joint_data_, actuator_data_);} - /*\}*/ -}; + public: + /** \sa TransmissionHandle::TransmissionHandle */ + JointToActuatorPositionHandle(const std::string& name, Transmission* transmission, + const ActuatorData& actuator_data, const JointData& joint_data) + : TransmissionHandle(name, transmission, actuator_data, joint_data) + { + } + /** \name Real-Time Safe Functions + *\{*/ + /** \brief Propagate joint positions to actuator positions for the stored transmission. */ + void propagate() + { + transmission_->jointToActuator((PositionJointData)joint_data_, actuator_data_); + } + /*\}*/ +}; /** \brief Handle for propagating joint velocities to actuator velocities for a given transmission. */ -class JointToActuatorVelocityHandle : public TransmissionHandle +class JointToActuatorVelocityHandle : public TransmissionHandle<> { -public: - /** \sa TransmissionHandle::TransmissionHandle */ - JointToActuatorVelocityHandle(const std::string& name, - Transmission* transmission, - const ActuatorData& actuator_data, - const JointData& joint_data) - : TransmissionHandle(name, transmission, actuator_data, joint_data) {} - - /** \name Real-Time Safe Functions - *\{*/ - /** \brief Propagate joint velocities to actuator velocities for the stored transmission. */ - void propagate() {transmission_->jointToActuatorVelocity(joint_data_, actuator_data_);} - /*\}*/ -}; + public: + /** \sa TransmissionHandle::TransmissionHandle */ + JointToActuatorVelocityHandle(const std::string& name, Transmission* transmission, + const ActuatorData& actuator_data, const JointData& joint_data) + : TransmissionHandle(name, transmission, actuator_data, joint_data) + { + } + /** \name Real-Time Safe Functions + *\{*/ + /** \brief Propagate joint velocities to actuator velocities for the stored transmission. */ + void propagate() + { + transmission_->jointToActuator((VelocityJointData)joint_data_, actuator_data_); + } + /*\}*/ +}; /** \brief Handle for propagating joint efforts to actuator efforts for a given transmission. */ -class JointToActuatorEffortHandle : public TransmissionHandle +class JointToActuatorEffortHandle : public TransmissionHandle<> { -public: - /** \sa TransmissionHandle::TransmissionHandle */ - JointToActuatorEffortHandle(const std::string& name, - Transmission* transmission, - const ActuatorData& actuator_data, - const JointData& joint_data) - : TransmissionHandle(name, transmission, actuator_data, joint_data) {} - - /** \name Real-Time Safe Functions - *\{*/ - /** \brief Propagate joint efforts to actuator efforts for the stored transmission. */ - void propagate() {transmission_->jointToActuatorEffort(joint_data_, actuator_data_);} - /*\}*/ + public: + /** \sa TransmissionHandle::TransmissionHandle */ + JointToActuatorEffortHandle(const std::string& name, Transmission* transmission, const ActuatorData& actuator_data, + const JointData& joint_data) + : TransmissionHandle(name, transmission, actuator_data, joint_data) + { + } + + /** \name Real-Time Safe Functions + *\{*/ + /** \brief Propagate joint efforts to actuator efforts for the stored transmission. */ + void propagate() + { + transmission_->jointToActuator((EffortJointData)joint_data_, actuator_data_); + } + /*\}*/ }; /** @@ -340,61 +324,78 @@ class JointToActuatorEffortHandle : public TransmissionHandle template class TransmissionInterface : public hardware_interface::ResourceManager { -public: - - HandleType getHandle(const std::string& name) - { - // Rethrow exception with a meaningful type - try - { - return this->hardware_interface::ResourceManager::getHandle(name); - } - catch(const std::logic_error& e) + public: + HandleType getHandle(const std::string& name) { - throw TransmissionInterfaceException(e.what()); + // Rethrow exception with a meaningful type + try + { + return this->hardware_interface::ResourceManager::getHandle(name); + } + catch (const std::logic_error& e) + { + throw TransmissionInterfaceException(e.what()); + } } - } - - /** \name Real-Time Safe Functions - *\{*/ - /** \brief Propagate the transmission maps of all managed handles. */ - void propagate() - { - typedef typename hardware_interface::ResourceManager::ResourceMap::iterator ItratorType; - for (ItratorType it = this->resource_map_.begin(); it != this->resource_map_.end(); ++it) + + /** \name Real-Time Safe Functions + *\{*/ + /** \brief Propagate the transmission maps of all managed handles. */ + void propagate() { - it->second.propagate(); + typedef typename hardware_interface::ResourceManager::ResourceMap::iterator ItratorType; + for (ItratorType it = this->resource_map_.begin(); it != this->resource_map_.end(); ++it) + { + it->second.propagate(); + } } - } - /*\}*/ + /*\}*/ }; // Convenience typedefs -/** Interface for propagating actuator state (position, velocity and effort) to joint state on a set of transmissions. */ -class ActuatorToJointStateInterface : public TransmissionInterface {}; +/** Interface for propagating actuator state (position, velocity and effort) to joint state on a set of transmissions. + */ +class ActuatorToJointStateInterface : public TransmissionInterface +{ +}; /** Interface for propagating actuator positions to joint positions on a set of transmissions. */ -class ActuatorToJointPositionInterface : public TransmissionInterface {}; +class ActuatorToJointPositionInterface : public TransmissionInterface +{ +}; /** Interface for propagating actuator velocities to joint velocities on a set of transmissions. */ -class ActuatorToJointVelocityInterface : public TransmissionInterface {}; +class ActuatorToJointVelocityInterface : public TransmissionInterface +{ +}; /** Interface for propagating actuator efforts to joint efforts on a set of transmissions. */ -class ActuatorToJointEffortInterface : public TransmissionInterface {}; +class ActuatorToJointEffortInterface : public TransmissionInterface +{ +}; -/** Interface for propagating joint state (position, velocity and effort) to actuator state on a set of transmissions. */ -class JointToActuatorStateInterface : public TransmissionInterface {}; +/** Interface for propagating joint state (position, velocity and effort) to actuator state on a set of transmissions. + */ +class JointToActuatorStateInterface : public TransmissionInterface +{ +}; /** Interface for propagating joint positions to actuator positions on a set of transmissions. */ -class JointToActuatorPositionInterface : public TransmissionInterface {}; +class JointToActuatorPositionInterface : public TransmissionInterface +{ +}; /** Interface for propagating joint velocities to actuator velocities on a set of transmissions. */ -class JointToActuatorVelocityInterface : public TransmissionInterface {}; +class JointToActuatorVelocityInterface : public TransmissionInterface +{ +}; /** Interface for propagating joint efforts to actuator efforts on a set of transmissions. */ -class JointToActuatorEffortInterface : public TransmissionInterface {}; +class JointToActuatorEffortInterface : public TransmissionInterface +{ +}; -} // transmission_interface +} // transmission_interface -#endif // TRANSMISSION_INTERFACE_TRANSMISSION_INTERFACE_H +#endif // TRANSMISSION_INTERFACE_TRANSMISSION_INTERFACE_H diff --git a/transmission_interface/include/transmission_interface/transmission_interface_exception.h b/transmission_interface/include/transmission_interface/transmission_interface_exception.h index 91568aa..e157269 100644 --- a/transmission_interface/include/transmission_interface/transmission_interface_exception.h +++ b/transmission_interface/include/transmission_interface/transmission_interface_exception.h @@ -29,6 +29,7 @@ #define TRANSMISSION_INTERFACE_TRANSMISSION_INTERFACE_EXCEPTION_H #include +#include namespace transmission_interface { @@ -36,7 +37,8 @@ namespace transmission_interface class TransmissionInterfaceException: public std::exception { public: - TransmissionInterfaceException(const std::string& message) : msg(message) {} + TransmissionInterfaceException(const std::string& message) : msg(message) + { std::cout << message << std::endl;} virtual ~TransmissionInterfaceException() throw() {} virtual const char* what() const throw() {return msg.c_str();} private: diff --git a/transmission_interface/test/differential_transmission_test.cpp b/transmission_interface/test/differential_transmission_test.cpp index 4d763f2..707d91a 100644 --- a/transmission_interface/test/differential_transmission_test.cpp +++ b/transmission_interface/test/differential_transmission_test.cpp @@ -118,54 +118,54 @@ TEST(PreconditionsTest, AssertionTriggering) vector(2, 1.0)); // Data with invalid pointers should trigger an assertion - EXPECT_DEATH(trans.actuatorToJointEffort(a_bad_data, j_bad_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointEffort(a_good_data, j_bad_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointEffort(a_bad_data, j_good_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((EffortActuatorData)a_bad_data, j_bad_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((EffortActuatorData)a_good_data, j_bad_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((EffortActuatorData)a_bad_data, j_good_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointVelocity(a_bad_data, j_bad_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointVelocity(a_good_data, j_bad_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointVelocity(a_bad_data, j_good_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((VelocityActuatorData)a_bad_data, j_bad_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((VelocityActuatorData)a_good_data, j_bad_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((VelocityActuatorData)a_bad_data, j_good_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointPosition(a_bad_data, j_bad_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointPosition(a_good_data, j_bad_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointPosition(a_bad_data, j_good_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((PositionActuatorData)a_bad_data, j_bad_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((PositionActuatorData)a_good_data, j_bad_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((PositionActuatorData)a_bad_data, j_good_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorEffort(j_bad_data, a_bad_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorEffort(j_good_data, a_bad_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorEffort(j_bad_data, a_good_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((EffortJointData)j_bad_data, a_bad_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((EffortJointData)j_good_data, a_bad_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((EffortJointData)j_bad_data, a_good_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorVelocity(j_bad_data, a_bad_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorVelocity(j_good_data, a_bad_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorVelocity(j_bad_data, a_good_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((VelocityJointData)j_bad_data, a_bad_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((VelocityJointData)j_good_data, a_bad_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((VelocityJointData)j_bad_data, a_good_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorPosition(j_bad_data, a_bad_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorPosition(j_good_data, a_bad_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorPosition(j_bad_data, a_good_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((PositionJointData)j_bad_data, a_bad_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((PositionJointData)j_good_data, a_bad_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((PositionJointData)j_bad_data, a_good_data), ".*"); // Wrong parameter sizes should trigger an assertion - EXPECT_DEATH(trans.actuatorToJointEffort(a_bad_size, j_bad_size), ".*"); - EXPECT_DEATH(trans.actuatorToJointEffort(a_good_data, j_bad_size), ".*"); - EXPECT_DEATH(trans.actuatorToJointEffort(a_bad_size, j_good_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((EffortActuatorData)a_bad_size, j_bad_size), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((EffortActuatorData)a_good_data, j_bad_size), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((EffortActuatorData)a_bad_size, j_good_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointVelocity(a_bad_size, j_bad_size), ".*"); - EXPECT_DEATH(trans.actuatorToJointVelocity(a_good_data, j_bad_size), ".*"); - EXPECT_DEATH(trans.actuatorToJointVelocity(a_bad_size, j_good_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((VelocityActuatorData)a_bad_size, j_bad_size), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((VelocityActuatorData)a_good_data, j_bad_size), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((VelocityActuatorData)a_bad_size, j_good_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointPosition(a_bad_size, j_bad_size), ".*"); - EXPECT_DEATH(trans.actuatorToJointPosition(a_good_data, j_bad_size), ".*"); - EXPECT_DEATH(trans.actuatorToJointPosition(a_bad_size, j_good_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((PositionActuatorData)a_bad_size, j_bad_size), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((PositionActuatorData)a_good_data, j_bad_size), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((PositionActuatorData)a_bad_size, j_good_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorEffort(j_bad_size, a_bad_size), ".*"); - EXPECT_DEATH(trans.jointToActuatorEffort(j_good_data, a_bad_size), ".*"); - EXPECT_DEATH(trans.jointToActuatorEffort(j_bad_size, a_good_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((EffortJointData)j_bad_size, a_bad_size), ".*"); + EXPECT_DEATH(trans.jointToActuator((EffortJointData)j_good_data, a_bad_size), ".*"); + EXPECT_DEATH(trans.jointToActuator((EffortJointData)j_bad_size, a_good_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorVelocity(j_bad_size, a_bad_size), ".*"); - EXPECT_DEATH(trans.jointToActuatorVelocity(j_good_data, a_bad_size), ".*"); - EXPECT_DEATH(trans.jointToActuatorVelocity(j_bad_size, a_good_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((VelocityJointData)j_bad_size, a_bad_size), ".*"); + EXPECT_DEATH(trans.jointToActuator((VelocityJointData)j_good_data, a_bad_size), ".*"); + EXPECT_DEATH(trans.jointToActuator((VelocityJointData)j_bad_size, a_good_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorPosition(j_bad_size, a_bad_size), ".*"); - EXPECT_DEATH(trans.jointToActuatorPosition(j_good_data, a_bad_size), ".*"); - EXPECT_DEATH(trans.jointToActuatorPosition(j_bad_size, a_good_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((PositionJointData)j_bad_size, a_bad_size), ".*"); + EXPECT_DEATH(trans.jointToActuator((PositionJointData)j_good_data, a_bad_size), ".*"); + EXPECT_DEATH(trans.jointToActuator((PositionJointData)j_bad_size, a_good_data), ".*"); } #endif // NDEBUG @@ -232,48 +232,48 @@ class BlackBoxTest : public TransmissionSetup { // Effort interface { - ActuatorData a_data; + EffortActuatorData a_data; a_data.effort = a_vec; *a_data.effort[0] = ref_val[0]; *a_data.effort[1] = ref_val[1]; - JointData j_data; + EffortJointData j_data; j_data.effort = j_vec; - trans.actuatorToJointEffort(a_data, j_data); - trans.jointToActuatorEffort(j_data, a_data); + trans.actuatorToJoint(a_data, j_data); + trans.jointToActuator(j_data, a_data); EXPECT_NEAR(ref_val[0], *a_data.effort[0], EPS); EXPECT_NEAR(ref_val[1], *a_data.effort[1], EPS); } // Velocity interface { - ActuatorData a_data; + VelocityActuatorData a_data; a_data.velocity = a_vec; *a_data.velocity[0] = ref_val[0]; *a_data.velocity[1] = ref_val[1]; - JointData j_data; + VelocityJointData j_data; j_data.velocity = j_vec; - trans.actuatorToJointVelocity(a_data, j_data); - trans.jointToActuatorVelocity(j_data, a_data); + trans.actuatorToJoint(a_data, j_data); + trans.jointToActuator(j_data, a_data); EXPECT_NEAR(ref_val[0], *a_data.velocity[0], EPS); EXPECT_NEAR(ref_val[1], *a_data.velocity[1], EPS); } // Position interface { - ActuatorData a_data; + PositionActuatorData a_data; a_data.position = a_vec; *a_data.position[0] = ref_val[0]; *a_data.position[1] = ref_val[1]; - JointData j_data; + PositionJointData j_data; j_data.position = j_vec; - trans.actuatorToJointPosition(a_data, j_data); - trans.jointToActuatorPosition(j_data, a_data); + trans.actuatorToJoint(a_data, j_data); + trans.jointToActuator(j_data, a_data); EXPECT_NEAR(ref_val[0], *a_data.position[0], EPS); EXPECT_NEAR(ref_val[1], *a_data.position[1], EPS); } @@ -342,39 +342,39 @@ TEST_F(WhiteBoxTest, DontMoveJoints) // Effort interface { - ActuatorData a_data; + EffortActuatorData a_data; a_data.effort = a_vec; - JointData j_data; + EffortJointData j_data; j_data.effort = j_vec; - trans.actuatorToJointEffort(a_data, j_data); + trans.actuatorToJoint(a_data, j_data); EXPECT_NEAR(0.0, *j_data.effort[0], EPS); EXPECT_NEAR(0.0, *j_data.effort[1], EPS); } // Velocity interface { - ActuatorData a_data; + VelocityActuatorData a_data; a_data.velocity = a_vec; - JointData j_data; + VelocityJointData j_data; j_data.velocity = j_vec; - trans.actuatorToJointVelocity(a_data, j_data); + trans.actuatorToJoint(a_data, j_data); EXPECT_NEAR(0.0, *j_data.velocity[0], EPS); EXPECT_NEAR(0.0, *j_data.velocity[1], EPS); } // Position interface { - ActuatorData a_data; + PositionActuatorData a_data; a_data.position = a_vec; - JointData j_data; + PositionJointData j_data; j_data.position = j_vec; - trans.actuatorToJointPosition(a_data, j_data); + trans.actuatorToJoint(a_data, j_data); EXPECT_NEAR(joint_offset[0], *j_data.position[0], EPS); EXPECT_NEAR(joint_offset[1], *j_data.position[1], EPS); } @@ -393,39 +393,39 @@ TEST_F(WhiteBoxTest, MoveFirstJointOnly) // Effort interface { - ActuatorData a_data; + EffortActuatorData a_data; a_data.effort = a_vec; - JointData j_data; + EffortJointData j_data; j_data.effort = j_vec; - trans.actuatorToJointEffort(a_data, j_data); + trans.actuatorToJoint(a_data, j_data); EXPECT_NEAR(400.0, *j_data.effort[0], EPS); EXPECT_NEAR(0.0, *j_data.effort[1], EPS); } // Velocity interface { - ActuatorData a_data; + VelocityActuatorData a_data; a_data.velocity = a_vec; - JointData j_data; + VelocityJointData j_data; j_data.velocity = j_vec; - trans.actuatorToJointVelocity(a_data, j_data); + trans.actuatorToJoint(a_data, j_data); EXPECT_NEAR(0.5, *j_data.velocity[0], EPS); EXPECT_NEAR(0.0, *j_data.velocity[1], EPS); } // Position interface { - ActuatorData a_data; + PositionActuatorData a_data; a_data.position = a_vec; - JointData j_data; + PositionJointData j_data; j_data.position = j_vec; - trans.actuatorToJointPosition(a_data, j_data); + trans.actuatorToJoint(a_data, j_data); EXPECT_NEAR(0.5, *j_data.position[0], EPS); EXPECT_NEAR(0.0, *j_data.position[1], EPS); } @@ -444,39 +444,39 @@ TEST_F(WhiteBoxTest, MoveSecondJointOnly) // Effort interface { - ActuatorData a_data; + EffortActuatorData a_data; a_data.effort = a_vec; - JointData j_data; + EffortJointData j_data; j_data.effort = j_vec; - trans.actuatorToJointEffort(a_data, j_data); + trans.actuatorToJoint(a_data, j_data); EXPECT_NEAR(0.0, *j_data.effort[0], EPS); EXPECT_NEAR(400.0, *j_data.effort[1], EPS); } // Velocity interface { - ActuatorData a_data; + VelocityActuatorData a_data; a_data.velocity = a_vec; - JointData j_data; + VelocityJointData j_data; j_data.velocity = j_vec; - trans.actuatorToJointVelocity(a_data, j_data); + trans.actuatorToJoint(a_data, j_data); EXPECT_NEAR(0.0, *j_data.velocity[0], EPS); EXPECT_NEAR(0.5, *j_data.velocity[1], EPS); } // Position interface { - ActuatorData a_data; + PositionActuatorData a_data; a_data.position = a_vec; - JointData j_data; + PositionJointData j_data; j_data.position = j_vec; - trans.actuatorToJointPosition(a_data, j_data); + trans.actuatorToJoint(a_data, j_data); EXPECT_NEAR(0.0, *j_data.position[0], EPS); EXPECT_NEAR(0.5, *j_data.position[1], EPS); } @@ -507,39 +507,39 @@ TEST_F(WhiteBoxTest, MoveBothJoints) // Effort interface { - ActuatorData a_data; + EffortActuatorData a_data; a_data.effort = a_vec; - JointData j_data; + EffortJointData j_data; j_data.effort = j_vec; - trans.actuatorToJointEffort(a_data, j_data); + trans.actuatorToJoint(a_data, j_data); EXPECT_NEAR(140.0, *j_data.effort[0], EPS); EXPECT_NEAR(520.0, *j_data.effort[1], EPS); } // Velocity interface { - ActuatorData a_data; + VelocityActuatorData a_data; a_data.velocity = a_vec; - JointData j_data; + VelocityJointData j_data; j_data.velocity = j_vec; - trans.actuatorToJointVelocity(a_data, j_data); + trans.actuatorToJoint(a_data, j_data); EXPECT_NEAR(-0.01250, *j_data.velocity[0], EPS); EXPECT_NEAR( 0.06875, *j_data.velocity[1], EPS); } // Position interface { - ActuatorData a_data; + PositionActuatorData a_data; a_data.position = a_vec; - JointData j_data; + PositionJointData j_data; j_data.position = j_vec; - trans.actuatorToJointPosition(a_data, j_data); + trans.actuatorToJoint(a_data, j_data); EXPECT_NEAR(-2.01250, *j_data.position[0], EPS); EXPECT_NEAR( 4.06875, *j_data.position[1], EPS); } diff --git a/transmission_interface/test/four_bar_linkage_transmission_test.cpp b/transmission_interface/test/four_bar_linkage_transmission_test.cpp index bb097e0..c5b0061 100644 --- a/transmission_interface/test/four_bar_linkage_transmission_test.cpp +++ b/transmission_interface/test/four_bar_linkage_transmission_test.cpp @@ -118,54 +118,54 @@ TEST(PreconditionsTest, AssertionTriggering) vector(2, 1.0)); // Data with invalid pointers should trigger an assertion - EXPECT_DEATH(trans.actuatorToJointEffort(a_bad_data, j_bad_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointEffort(a_good_data, j_bad_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointEffort(a_bad_data, j_good_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((EffortActuatorData)a_bad_data, j_bad_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((EffortActuatorData)a_good_data, j_bad_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((EffortActuatorData)a_bad_data, j_good_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointVelocity(a_bad_data, j_bad_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointVelocity(a_good_data, j_bad_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointVelocity(a_bad_data, j_good_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((VelocityActuatorData)a_bad_data, j_bad_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((VelocityActuatorData)a_good_data, j_bad_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((VelocityActuatorData)a_bad_data, j_good_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointPosition(a_bad_data, j_bad_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointPosition(a_good_data, j_bad_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointPosition(a_bad_data, j_good_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((PositionActuatorData)a_bad_data, j_bad_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((PositionActuatorData)a_good_data, j_bad_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((PositionActuatorData)a_bad_data, j_good_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorEffort(j_bad_data, a_bad_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorEffort(j_good_data, a_bad_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorEffort(j_bad_data, a_good_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((EffortJointData)j_bad_data, a_bad_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((EffortJointData)j_good_data, a_bad_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((EffortJointData)j_bad_data, a_good_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorVelocity(j_bad_data, a_bad_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorVelocity(j_good_data, a_bad_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorVelocity(j_bad_data, a_good_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((VelocityJointData)j_bad_data, a_bad_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((VelocityJointData)j_good_data, a_bad_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((VelocityJointData)j_bad_data, a_good_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorPosition(j_bad_data, a_bad_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorPosition(j_good_data, a_bad_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorPosition(j_bad_data, a_good_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((PositionJointData)j_bad_data, a_bad_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((PositionJointData)j_good_data, a_bad_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((PositionJointData)j_bad_data, a_good_data), ".*"); // Wrong parameter sizes should trigger an assertion - EXPECT_DEATH(trans.actuatorToJointEffort(a_bad_size, j_bad_size), ".*"); - EXPECT_DEATH(trans.actuatorToJointEffort(a_good_data, j_bad_size), ".*"); - EXPECT_DEATH(trans.actuatorToJointEffort(a_bad_size, j_good_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((EffortActuatorData)a_bad_size, j_bad_size), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((EffortActuatorData)a_good_data, j_bad_size), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((EffortActuatorData)a_bad_size, j_good_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointVelocity(a_bad_size, j_bad_size), ".*"); - EXPECT_DEATH(trans.actuatorToJointVelocity(a_good_data, j_bad_size), ".*"); - EXPECT_DEATH(trans.actuatorToJointVelocity(a_bad_size, j_good_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((VelocityActuatorData)a_bad_size, j_bad_size), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((VelocityActuatorData)a_good_data, j_bad_size), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((VelocityActuatorData)a_bad_size, j_good_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointPosition(a_bad_size, j_bad_size), ".*"); - EXPECT_DEATH(trans.actuatorToJointPosition(a_good_data, j_bad_size), ".*"); - EXPECT_DEATH(trans.actuatorToJointPosition(a_bad_size, j_good_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((PositionActuatorData)a_bad_size, j_bad_size), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((PositionActuatorData)a_good_data, j_bad_size), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((PositionActuatorData)a_bad_size, j_good_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorEffort(j_bad_size, a_bad_size), ".*"); - EXPECT_DEATH(trans.jointToActuatorEffort(j_good_data, a_bad_size), ".*"); - EXPECT_DEATH(trans.jointToActuatorEffort(j_bad_size, a_good_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((EffortJointData)j_bad_size, a_bad_size), ".*"); + EXPECT_DEATH(trans.jointToActuator((EffortJointData)j_good_data, a_bad_size), ".*"); + EXPECT_DEATH(trans.jointToActuator((EffortJointData)j_bad_size, a_good_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorVelocity(j_bad_size, a_bad_size), ".*"); - EXPECT_DEATH(trans.jointToActuatorVelocity(j_good_data, a_bad_size), ".*"); - EXPECT_DEATH(trans.jointToActuatorVelocity(j_bad_size, a_good_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((VelocityJointData)j_bad_size, a_bad_size), ".*"); + EXPECT_DEATH(trans.jointToActuator((VelocityJointData)j_good_data, a_bad_size), ".*"); + EXPECT_DEATH(trans.jointToActuator((VelocityJointData)j_bad_size, a_good_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorPosition(j_bad_size, a_bad_size), ".*"); - EXPECT_DEATH(trans.jointToActuatorPosition(j_good_data, a_bad_size), ".*"); - EXPECT_DEATH(trans.jointToActuatorPosition(j_bad_size, a_good_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((PositionJointData)j_bad_size, a_bad_size), ".*"); + EXPECT_DEATH(trans.jointToActuator((PositionJointData)j_good_data, a_bad_size), ".*"); + EXPECT_DEATH(trans.jointToActuator((PositionJointData)j_bad_size, a_good_data), ".*"); } #endif // NDEBUG @@ -232,48 +232,48 @@ class BlackBoxTest : public TransmissionSetup { // Effort interface { - ActuatorData a_data; + EffortActuatorData a_data; a_data.effort = a_vec; *a_data.effort[0] = ref_val[0]; *a_data.effort[1] = ref_val[1]; - JointData j_data; + EffortJointData j_data; j_data.effort = j_vec; - trans.actuatorToJointEffort(a_data, j_data); - trans.jointToActuatorEffort(j_data, a_data); + trans.actuatorToJoint(a_data, j_data); + trans.jointToActuator(j_data, a_data); EXPECT_NEAR(ref_val[0], *a_data.effort[0], EPS); EXPECT_NEAR(ref_val[1], *a_data.effort[1], EPS); } // Velocity interface { - ActuatorData a_data; + VelocityActuatorData a_data; a_data.velocity = a_vec; *a_data.velocity[0] = ref_val[0]; *a_data.velocity[1] = ref_val[1]; - JointData j_data; + VelocityJointData j_data; j_data.velocity = j_vec; - trans.actuatorToJointVelocity(a_data, j_data); - trans.jointToActuatorVelocity(j_data, a_data); + trans.actuatorToJoint(a_data, j_data); + trans.jointToActuator(j_data, a_data); EXPECT_NEAR(ref_val[0], *a_data.velocity[0], EPS); EXPECT_NEAR(ref_val[1], *a_data.velocity[1], EPS); } // Position interface { - ActuatorData a_data; + PositionActuatorData a_data; a_data.position = a_vec; *a_data.position[0] = ref_val[0]; *a_data.position[1] = ref_val[1]; - JointData j_data; + PositionJointData j_data; j_data.position = j_vec; - trans.actuatorToJointPosition(a_data, j_data); - trans.jointToActuatorPosition(j_data, a_data); + trans.actuatorToJoint(a_data, j_data); + trans.jointToActuator(j_data, a_data); EXPECT_NEAR(ref_val[0], *a_data.position[0], EPS); EXPECT_NEAR(ref_val[1], *a_data.position[1], EPS); } @@ -342,39 +342,39 @@ TEST_F(WhiteBoxTest, DontMoveJoints) // Effort interface { - ActuatorData a_data; + EffortActuatorData a_data; a_data.effort = a_vec; - JointData j_data; + EffortJointData j_data; j_data.effort = j_vec; - trans.actuatorToJointEffort(a_data, j_data); + trans.actuatorToJoint(a_data, j_data); EXPECT_NEAR(0.0, *j_data.effort[0], EPS); EXPECT_NEAR(0.0, *j_data.effort[1], EPS); } // Velocity interface { - ActuatorData a_data; + VelocityActuatorData a_data; a_data.velocity = a_vec; - JointData j_data; + VelocityJointData j_data; j_data.velocity = j_vec; - trans.actuatorToJointVelocity(a_data, j_data); + trans.actuatorToJoint(a_data, j_data); EXPECT_NEAR(0.0, *j_data.velocity[0], EPS); EXPECT_NEAR(0.0, *j_data.velocity[1], EPS); } // Position interface { - ActuatorData a_data; + PositionActuatorData a_data; a_data.position = a_vec; - JointData j_data; + PositionJointData j_data; j_data.position = j_vec; - trans.actuatorToJointPosition(a_data, j_data); + trans.actuatorToJoint(a_data, j_data); EXPECT_NEAR(joint_offset[0], *j_data.position[0], EPS); EXPECT_NEAR(joint_offset[1], *j_data.position[1], EPS); } @@ -391,13 +391,13 @@ TEST_F(WhiteBoxTest, MoveFirstJointOnly) *a_vec[0] = 5.0; *a_vec[1] = 10.0; - ActuatorData a_data; + EffortActuatorData a_data; a_data.effort = a_vec; - JointData j_data; + EffortJointData j_data; j_data.effort = j_vec; - trans.actuatorToJointEffort(a_data, j_data); + trans.actuatorToJoint(a_data, j_data); EXPECT_NEAR(100.0, *j_data.effort[0], EPS); EXPECT_NEAR(0.0, *j_data.effort[1], EPS); } @@ -407,13 +407,13 @@ TEST_F(WhiteBoxTest, MoveFirstJointOnly) *a_vec[0] = 10.0; *a_vec[1] = 5.0; - ActuatorData a_data; + VelocityActuatorData a_data; a_data.velocity = a_vec; - JointData j_data; + VelocityJointData j_data; j_data.velocity = j_vec; - trans.actuatorToJointVelocity(a_data, j_data); + trans.actuatorToJoint(a_data, j_data); EXPECT_NEAR(0.5, *j_data.velocity[0], EPS); EXPECT_NEAR(0.0, *j_data.velocity[1], EPS); } @@ -423,13 +423,13 @@ TEST_F(WhiteBoxTest, MoveFirstJointOnly) *a_vec[0] = 10.0; *a_vec[1] = 5.0; - ActuatorData a_data; + PositionActuatorData a_data; a_data.position = a_vec; - JointData j_data; + PositionJointData j_data; j_data.position = j_vec; - trans.actuatorToJointPosition(a_data, j_data); + trans.actuatorToJoint(a_data, j_data); EXPECT_NEAR(0.5, *j_data.position[0], EPS); EXPECT_NEAR(0.0, *j_data.position[1], EPS); } @@ -447,39 +447,39 @@ TEST_F(WhiteBoxTest, MoveSecondJointOnly) // Effort interface { - ActuatorData a_data; + EffortActuatorData a_data; a_data.effort = a_vec; - JointData j_data; + EffortJointData j_data; j_data.effort = j_vec; - trans.actuatorToJointEffort(a_data, j_data); + trans.actuatorToJoint(a_data, j_data); EXPECT_NEAR(0.0, *j_data.effort[0], EPS); EXPECT_NEAR(200.0, *j_data.effort[1], EPS); } // Velocity interface { - ActuatorData a_data; + VelocityActuatorData a_data; a_data.velocity = a_vec; - JointData j_data; + VelocityJointData j_data; j_data.velocity = j_vec; - trans.actuatorToJointVelocity(a_data, j_data); + trans.actuatorToJoint(a_data, j_data); EXPECT_NEAR(0.0, *j_data.velocity[0], EPS); EXPECT_NEAR(0.5, *j_data.velocity[1], EPS); } // Position interface { - ActuatorData a_data; + PositionActuatorData a_data; a_data.position = a_vec; - JointData j_data; + PositionJointData j_data; j_data.position = j_vec; - trans.actuatorToJointPosition(a_data, j_data); + trans.actuatorToJoint(a_data, j_data); EXPECT_NEAR(0.0, *j_data.position[0], EPS); EXPECT_NEAR(0.5, *j_data.position[1], EPS); } @@ -510,39 +510,39 @@ TEST_F(WhiteBoxTest, MoveBothJoints) // Effort interface { - ActuatorData a_data; + EffortActuatorData a_data; a_data.effort = a_vec; - JointData j_data; + EffortJointData j_data; j_data.effort = j_vec; - trans.actuatorToJointEffort(a_data, j_data); + trans.actuatorToJoint(a_data, j_data); EXPECT_NEAR( -60.0, *j_data.effort[0], EPS); EXPECT_NEAR(-160.0, *j_data.effort[1], EPS); } // Velocity interface { - ActuatorData a_data; + VelocityActuatorData a_data; a_data.velocity = a_vec; - JointData j_data; + VelocityJointData j_data; j_data.velocity = j_vec; - trans.actuatorToJointVelocity(a_data, j_data); + trans.actuatorToJoint(a_data, j_data); EXPECT_NEAR(-0.15, *j_data.velocity[0], EPS); EXPECT_NEAR(-0.025, *j_data.velocity[1], EPS); } // Position interface { - ActuatorData a_data; + PositionActuatorData a_data; a_data.position = a_vec; - JointData j_data; + PositionJointData j_data; j_data.position = j_vec; - trans.actuatorToJointPosition(a_data, j_data); + trans.actuatorToJoint(a_data, j_data); EXPECT_NEAR(-2.15, *j_data.position[0], EPS); EXPECT_NEAR( 3.975, *j_data.position[1], EPS); } diff --git a/transmission_interface/test/simple_transmission_test.cpp b/transmission_interface/test/simple_transmission_test.cpp index 4e088dc..d18cc76 100644 --- a/transmission_interface/test/simple_transmission_test.cpp +++ b/transmission_interface/test/simple_transmission_test.cpp @@ -88,54 +88,54 @@ TEST(PreconditionsTest, AssertionTriggering) SimpleTransmission trans = SimpleTransmission(1.0); // Data with invalid pointers should trigger an assertion - EXPECT_DEATH(trans.actuatorToJointEffort(a_bad_data, j_bad_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointEffort(a_good_data, j_bad_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointEffort(a_bad_data, j_good_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((EffortActuatorData)a_bad_data, j_bad_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((EffortActuatorData)a_good_data, j_bad_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((EffortActuatorData)a_bad_data, j_good_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointVelocity(a_bad_data, j_bad_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointVelocity(a_good_data, j_bad_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointVelocity(a_bad_data, j_good_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((VelocityActuatorData)a_bad_data, j_bad_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((VelocityActuatorData)a_good_data, j_bad_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((VelocityActuatorData)a_bad_data, j_good_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointPosition(a_bad_data, j_bad_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointPosition(a_good_data, j_bad_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointPosition(a_bad_data, j_good_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((PositionActuatorData)a_bad_data, j_bad_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((PositionActuatorData)a_good_data, j_bad_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((PositionActuatorData)a_bad_data, j_good_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorEffort(j_bad_data, a_bad_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorEffort(j_good_data, a_bad_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorEffort(j_bad_data, a_good_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((EffortJointData)j_bad_data, a_bad_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((EffortJointData)j_good_data, a_bad_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((EffortJointData)j_bad_data, a_good_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorVelocity(j_bad_data, a_bad_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorVelocity(j_good_data, a_bad_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorVelocity(j_bad_data, a_good_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((VelocityJointData)j_bad_data, a_bad_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((VelocityJointData)j_good_data, a_bad_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((VelocityJointData)j_bad_data, a_good_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorPosition(j_bad_data, a_bad_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorPosition(j_good_data, a_bad_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorPosition(j_bad_data, a_good_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((PositionJointData)j_bad_data, a_bad_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((PositionJointData)j_good_data, a_bad_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((PositionJointData)j_bad_data, a_good_data), ".*"); // Wrong parameter sizes should trigger an assertion - EXPECT_DEATH(trans.actuatorToJointEffort(a_bad_size, j_bad_size), ".*"); - EXPECT_DEATH(trans.actuatorToJointEffort(a_good_data, j_bad_size), ".*"); - EXPECT_DEATH(trans.actuatorToJointEffort(a_bad_size, j_good_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((EffortActuatorData)a_bad_size, j_bad_size), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((EffortActuatorData)a_good_data, j_bad_size), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((EffortActuatorData)a_bad_size, j_good_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointVelocity(a_bad_size, j_bad_size), ".*"); - EXPECT_DEATH(trans.actuatorToJointVelocity(a_good_data, j_bad_size), ".*"); - EXPECT_DEATH(trans.actuatorToJointVelocity(a_bad_size, j_good_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((VelocityActuatorData)a_bad_size, j_bad_size), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((VelocityActuatorData)a_good_data, j_bad_size), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((VelocityActuatorData)a_bad_size, j_good_data), ".*"); - EXPECT_DEATH(trans.actuatorToJointPosition(a_bad_size, j_bad_size), ".*"); - EXPECT_DEATH(trans.actuatorToJointPosition(a_good_data, j_bad_size), ".*"); - EXPECT_DEATH(trans.actuatorToJointPosition(a_bad_size, j_good_data), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((PositionActuatorData)a_bad_size, j_bad_size), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((PositionActuatorData)a_good_data, j_bad_size), ".*"); + EXPECT_DEATH(trans.actuatorToJoint((PositionActuatorData)a_bad_size, j_good_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorEffort(j_bad_size, a_bad_size), ".*"); - EXPECT_DEATH(trans.jointToActuatorEffort(j_good_data, a_bad_size), ".*"); - EXPECT_DEATH(trans.jointToActuatorEffort(j_bad_size, a_good_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((EffortJointData)j_bad_size, a_bad_size), ".*"); + EXPECT_DEATH(trans.jointToActuator((EffortJointData)j_good_data, a_bad_size), ".*"); + EXPECT_DEATH(trans.jointToActuator((EffortJointData)j_bad_size, a_good_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorVelocity(j_bad_size, a_bad_size), ".*"); - EXPECT_DEATH(trans.jointToActuatorVelocity(j_good_data, a_bad_size), ".*"); - EXPECT_DEATH(trans.jointToActuatorVelocity(j_bad_size, a_good_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((VelocityJointData)j_bad_size, a_bad_size), ".*"); + EXPECT_DEATH(trans.jointToActuator((VelocityJointData)j_good_data, a_bad_size), ".*"); + EXPECT_DEATH(trans.jointToActuator((VelocityJointData)j_bad_size, a_good_data), ".*"); - EXPECT_DEATH(trans.jointToActuatorPosition(j_bad_size, a_bad_size), ".*"); - EXPECT_DEATH(trans.jointToActuatorPosition(j_good_data, a_bad_size), ".*"); - EXPECT_DEATH(trans.jointToActuatorPosition(j_bad_size, a_good_data), ".*"); + EXPECT_DEATH(trans.jointToActuator((PositionJointData)j_bad_size, a_bad_size), ".*"); + EXPECT_DEATH(trans.jointToActuator((PositionJointData)j_good_data, a_bad_size), ".*"); + EXPECT_DEATH(trans.jointToActuator((PositionJointData)j_bad_size, a_good_data), ".*"); } #endif // NDEBUG @@ -183,43 +183,43 @@ class BlackBoxTest : public TransmissionSetup, { // Effort interface { - ActuatorData a_data; + EffortActuatorData a_data; a_data.effort = a_vec; *a_data.effort[0] = ref_val; - JointData j_data; + EffortJointData j_data; j_data.effort = j_vec; - trans.actuatorToJointEffort(a_data, j_data); - trans.jointToActuatorEffort(j_data, a_data); + trans.actuatorToJoint(a_data, j_data); + trans.jointToActuator(j_data, a_data); EXPECT_NEAR(ref_val, *a_data.effort[0], EPS); } // Velocity interface { - ActuatorData a_data; + VelocityActuatorData a_data; a_data.velocity = a_vec; *a_data.velocity[0] = ref_val; - JointData j_data; + VelocityJointData j_data; j_data.velocity = j_vec; - trans.actuatorToJointVelocity(a_data, j_data); - trans.jointToActuatorVelocity(j_data, a_data); + trans.actuatorToJoint(a_data, j_data); + trans.jointToActuator(j_data, a_data); EXPECT_NEAR(ref_val, *a_data.velocity[0], EPS); } // Position interface { - ActuatorData a_data; + PositionActuatorData a_data; a_data.position = a_vec; *a_data.position[0] = ref_val; - JointData j_data; + PositionJointData j_data; j_data.position = j_vec; - trans.actuatorToJointPosition(a_data, j_data); - trans.jointToActuatorPosition(j_data, a_data); + trans.actuatorToJoint(a_data, j_data); + trans.jointToActuator(j_data, a_data); EXPECT_NEAR(ref_val, *a_data.position[0], EPS); } } @@ -259,37 +259,37 @@ TEST_F(WhiteBoxTest, MoveJoint) // Effort interface { - ActuatorData a_data; + EffortActuatorData a_data; a_data.effort = a_vec; - JointData j_data; + EffortJointData j_data; j_data.effort = j_vec; - trans.actuatorToJointEffort(a_data, j_data); + trans.actuatorToJoint(a_data, j_data); EXPECT_NEAR(10.0, *j_data.effort[0], EPS); } // Velocity interface { - ActuatorData a_data; + VelocityActuatorData a_data; a_data.velocity = a_vec; - JointData j_data; + VelocityJointData j_data; j_data.velocity = j_vec; - trans.actuatorToJointVelocity(a_data, j_data); + trans.actuatorToJoint(a_data, j_data); EXPECT_NEAR(0.1, *j_data.velocity[0], EPS); } // Position interface { - ActuatorData a_data; + PositionActuatorData a_data; a_data.position = a_vec; - JointData j_data; + PositionJointData j_data; j_data.position = j_vec; - trans.actuatorToJointPosition(a_data, j_data); + trans.actuatorToJoint(a_data, j_data); EXPECT_NEAR(1.1, *j_data.position[0], EPS); } } diff --git a/transmission_interface/test/transmission_interface_test.cpp b/transmission_interface/test/transmission_interface_test.cpp index 5524380..beab6d0 100644 --- a/transmission_interface/test/transmission_interface_test.cpp +++ b/transmission_interface/test/transmission_interface_test.cpp @@ -42,7 +42,7 @@ using namespace transmission_interface; // Floating-point value comparison threshold const double EPS = 1e-6; -class DummyHandle : public TransmissionHandle +class DummyHandle : public TransmissionHandle<> { public: DummyHandle(const std::string& name, diff --git a/transmission_interface/test/transmission_interface_variable_test.cpp b/transmission_interface/test/transmission_interface_variable_test.cpp new file mode 100644 index 0000000..7bacf8e --- /dev/null +++ b/transmission_interface/test/transmission_interface_variable_test.cpp @@ -0,0 +1,168 @@ +#include +#include +#include +#include +#include +#include + +using namespace testing; + +namespace transmission_interface +{ +template +std::vector operator+(const std::vector& v1, const std::vector& v2) +{ + std::vector result; + result.insert(result.end(), v1.begin(), v1.end()); + result.insert(result.end(), v2.begin(), v2.end()); + return result; +} + +class PositionOnlyActuatorData : public ActuatorDataContainer +{ + public: + bool empty() const override + { + return position.empty(); + } + + bool hasSize(std::size_t size) const override + { + return (not position.empty() and position.size() == size); + } + + bool valid() const override + { + return hasValidPointers(position); + } + + size_t size() const + { + return position.size(); + } +}; + +class DummyHandle : public TransmissionHandle +{ + public: + DummyHandle(const std::string& name, Transmission* transmission, const PositionOnlyActuatorData& actuator_data, + const JointData& joint_data) + : TransmissionHandle(name, transmission, actuator_data, joint_data) + { + } +}; + +struct TransmissionTestParams +{ + PositionOnlyActuatorData actuator_data; + JointData joint_data; + bool should_throw; +}; + +std::string PrintToString(const TransmissionTestParams& param) +{ + std::stringstream ss; + ss << "ActuatorDataSize: " << param.actuator_data.size() + << " JointDataPositionSize: " << param.joint_data.position.size() + << " VelocitySize: " << param.joint_data.velocity.size() << " EffortSize: " << param.joint_data.effort.size() + << " ShouldThrow: " << (param.should_throw ? "Yes" : "No"); + return ss.str(); +} + +class VariableTransmissionInterfaceTest : public TestWithParam +{ + public: + VariableTransmissionInterfaceTest() : trans(1.0) + { + } + + SimpleTransmission trans; +}; + +TEST_P(VariableTransmissionInterfaceTest, transmissionHandleInitialization) +{ + if (GetParam().should_throw) + { + EXPECT_THROW(DummyHandle("trans", &trans, GetParam().actuator_data, GetParam().joint_data), + TransmissionInterfaceException); + } + else + { + EXPECT_NO_THROW(DummyHandle("trans", &trans, GetParam().actuator_data, GetParam().joint_data)); + } +} + +std::vector makeTransmissionHandleTestParams(const int num_dof) +{ + std::vector result; + constexpr bool SHOULD_THROW = true; + constexpr bool SHOULD_NOT_THROW = false; + + double val = 0.0; + std::vector good_vec(num_dof, &val); + + { + PositionOnlyActuatorData a_data; + a_data.position = good_vec; + result.push_back({ a_data, JointData(), SHOULD_NOT_THROW }); + } + { + PositionOnlyActuatorData a_data; + a_data.position = good_vec; + JointData j_data; + j_data.position = good_vec; + result.push_back({ a_data, j_data, SHOULD_NOT_THROW }); + } + { + PositionOnlyActuatorData a_data; + a_data.position = good_vec; + JointData j_data; + j_data.velocity = good_vec; + result.push_back({ a_data, j_data, SHOULD_NOT_THROW }); + } + { + PositionOnlyActuatorData a_data; + a_data.position = good_vec; + JointData j_data; + j_data.effort = good_vec; + result.push_back({ a_data, j_data, SHOULD_NOT_THROW }); + } + { + JointData j_data; + j_data.position = good_vec; + result.push_back({ PositionOnlyActuatorData(), j_data, SHOULD_THROW }); + } + { + JointData j_data; + j_data.velocity = good_vec; + result.push_back({ PositionOnlyActuatorData(), j_data, SHOULD_THROW }); + } + { + JointData j_data; + j_data.effort = good_vec; + result.push_back({ PositionOnlyActuatorData(), j_data, SHOULD_THROW }); + } + { + PositionOnlyActuatorData a_data; + JointData j_data; + a_data.position = good_vec; + j_data.position = good_vec; + j_data.velocity = good_vec; + j_data.effort = good_vec; + result.push_back({ a_data, j_data, SHOULD_NOT_THROW }); + } + + return result; +} + +// SimpleTransmission only handles one joint +INSTANTIATE_TEST_CASE_P(TransmissionHandleInitialization, VariableTransmissionInterfaceTest, + ValuesIn(makeTransmissionHandleTestParams(1))); + +} // namespace transmission_interface + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}