diff --git a/CMakeLists.txt b/CMakeLists.txt index 171783f..c3300b7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,169 +1,170 @@ PROJECT(libOncilla C CXX) CMAKE_MINIMUM_REQUIRED(VERSION 2.6) SET(LIBNAME "oncilla") SET(CMAKE_ALLOW_LOOSE_LOOP_CONSTRUCTS TRUE) SET(LIBONCILLA_CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) # CMake module path and modules FIND_PACKAGE(RSC REQUIRED) SET(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${RSC_CMAKE_MODULE_PATH}") MESSAGE(STATUS "RSC version: ${RSC_VERSION}") INCLUDE(CheckIncludeFileCXX) INCLUDE(InstallFilesRecursive) INCLUDE(EnableCoverageReport) INCLUDE(EnableSlocCount) INCLUDE(GenerateDoxygen) INCLUDE(GenerateCppcheck) +INCLUDE(PedanticCompilerWarnings) FIND_PACKAGE(PkgConfig REQUIRED) SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) # --- user options --- OPTION(BUILD_TESTS "Decide whether unit-tests are built" ON) OPTION(BUILD_EXAMPLES "Decide whether the examples are built" ON) OPTION(BUILD_DOCS "Decide whether the docygen documentation is built" OFF) # Dependencies FIND_PACKAGE(Threads REQUIRED) MESSAGE(STATUS "Threading library: ${CMAKE_THREAD_LIBS_INIT}") set(OLD_CXX_FLAGS ${CMAKE_CXX_FLAGS}) FIND_PACKAGE(NemoMath REQUIRED) MESSAGE(STATUS "NemoMath version: ${NemoMath_VERSION}") set(CMAKE_CXX_FLAGS ${OLD_CXX_FLAGS}) FIND_PACKAGE(RCI REQUIRED) MESSAGE(STATUS "RCI version: ${RCI_VERSION}") # --- global definitions --- SET(LIBONCILLA_NAME "${LIBNAME}") SET(DRIVER_TEST_NAME "${PROJECT_NAME}Tests") SET(DTO_TEST_NAME "${DRIVER_TEST_NAME}DTO") SET(LIBONCILLA_VERSION_MAJOR 0) SET(LIBONCILLA_VERSION_MINOR 2) SET(LIBONCILLA_VERSION_PATCH 0) SET(LIBONCILLA_VERSION_STRING "${LIBONCILLA_VERSION_MAJOR}.${LIBONCILLA_VERSION_MINOR}.${LIBONCILLA_VERSION_PATCH}") SET(LIBONCILLA_CMAKE_MODULE_PATH "share/cmake/Modules") SET(GTEST_DIR ${PROJECT_SOURCE_DIR}/3rdparty/gtest-1.5.0) SET(GTEST_INCLUDE_DIR ${GTEST_DIR}/include) SET(GMOCK_DIR ${PROJECT_SOURCE_DIR}/3rdparty/gmock-1.5.0) SET(GMOCK_INCLUDE_DIR ${GMOCK_DIR}/include) SET(GMOCK_LIBRARY_NAME "gmock") SET(OUTPUT_PATH ${PROJECT_BINARY_DIR}/build) SET(ARCHIVE_OUTPUT_PATH ${OUTPUT_PATH}) SET(LIBRARY_OUTPUT_PATH ${OUTPUT_PATH}) SET(EXECUTABLE_OUTPUT_PATH ${OUTPUT_PATH}) # --- dependency handling --- IF(BUILD_DOCS) FIND_PACKAGE(Doxygen) ENDIF() # Includes INCLUDE_DIRECTORIES(BEFORE src ${PROJECT_BINARY_DIR}/src) message(STATUS ${RSC_INCLUDE_DIRS}) INCLUDE_DIRECTORIES(SYSTEM ${RCI_INCLUDE_DIRS} ${RSC_INCLUDE_DIRS} ${NemoMath_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${LIBRARIES_INCLUDE_DIRS}) #INCLUDE_DIRECTORIES(BEFORE SYSTEM ${PROTOBUF_INCLUDE_DIR} ${RST_INCLUDE_DIRS}) LINK_DIRECTORIES(${RCI_LIBRARY_DIR} ${LIBRARIES_LIBRARY_DIRS}) ADD_DEFINITIONS(${NEMOMATH_DEFINITIONS}) ADD_SUBDIRECTORY(src) ADD_SUBDIRECTORY(cmake) IF(BUILD_TESTS) ADD_SUBDIRECTORY(test) ENDIF() #IF(BUILD_EXAMPLES) # ADD_SUBDIRECTORY(examples) #ENDIF() # --- cmake config file --- SET(LIB_SUFFIX ${CMAKE_SHARED_LIBRARY_SUFFIX}) IF(CMAKE_LINK_LIBRARY_SUFFIX) SET(LIB_SUFFIX ${CMAKE_LINK_LIBRARY_SUFFIX}) ENDIF() # --- pkgconfig file --- CONFIGURE_FILE(lib${LIBONCILLA_NAME}.pc.in ${CMAKE_CURRENT_BINARY_DIR}/lib${LIBONCILLA_NAME}.pc @ONLY) INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/lib${LIBONCILLA_NAME}.pc DESTINATION lib/pkgconfig) # --- cmake config file --- SET(LIB_SUFFIX ${CMAKE_SHARED_LIBRARY_SUFFIX}) IF(CMAKE_LINK_LIBRARY_SUFFIX) SET(LIB_SUFFIX ${CMAKE_LINK_LIBRARY_SUFFIX}) ENDIF() CONFIGURE_FILE(${CMAKE_PROJECT_NAME}Config.cmake.in ${CMAKE_PROJECT_NAME}Config.cmake @ONLY) INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/${CMAKE_PROJECT_NAME}Config.cmake DESTINATION share/lib${LIBNAME}) # --- documentation generation --- IF(BUILD_DOCS) IF(DOXYGEN_FOUND) SET(API_DIR "${CMAKE_BINARY_DIR}/doc") SET(SOURCE_DIR "${CMAKE_SOURCE_DIR}/src") SET(DOXYFILE "${CMAKE_BINARY_DIR}/Doxyfile") SET(WARNFILE "${CMAKE_BINARY_DIR}/doxygen-warn.log") CONFIGURE_FILE(Doxyfile.in ${DOXYFILE}) ADD_CUSTOM_COMMAND(OUTPUT ${API_DIR} DEPENDS ${DOXYFILE} COMMAND ${DOXYGEN_EXECUTABLE} ARGS ${DOXYFILE} WORKING_DIRECTORY ${CMAKE_BINARY_DIR}) ADD_CUSTOM_TARGET(doc DEPENDS ${API_DIR}) INSTALL(CODE "execute_process(COMMAND ${CMAKE_BUILD_TOOL} doc)") INSTALL(DIRECTORY ${API_DIR} DESTINATION share/lib${LIBNAME}) ENDIF() ENDIF(BUILD_DOCS) # --- coverage --- IF(BUILD_TESTS) ENABLE_COVERAGE_REPORT(TARGETS ${LIBONCILLA_NAME} TESTS ${DTO_TEST_NAME} ${INTEGRATED_TEST_NAME} FILTER "*3rdparty*" "*test/*") ENDIF(BUILD_TESTS) # --- sloccount --- ENABLE_SLOCCOUNT(FOLDERS src test examples) # --- cppcheck --- GENERATE_CPPCHECK(SOURCES src test examples "${CMAKE_CURRENT_BINARY_DIR}/src" "${CMAKE_CURRENT_BINARY_DIR}/test" "${CMAKE_CURRENT_BINARY_DIR}/examples" ENABLE_IDS style) # --- package --- INCLUDE(CheckLSBTypes) SET(CPACK_PACKAGE_VERSION_MAJOR ${LIBONCILLA_VERSION_MAJOR}) SET(CPACK_PACKAGE_VERSION_MINOR ${LIBONCILLA_VERSION_MINOR}) SET(CPACK_PACKAGE_VERSION_PATCH ${LIBONCILLA_VERSION_PATCH}) SET(CPACK_PACKAGE_VERSION "${CPACK_PACKAGE_VERSION_MAJOR}.${CPACK_PACKAGE_VERSION_MINOR}.${CPACK_PACKAGE_VERSION_PATCH}") SET(CPACK_PACKAGE_FILE_NAME ${CMAKE_PROJECT_NAME}-${CPACK_PACKAGE_VERSION}_${LSB_CODENAME}_${LSB_PROCESSOR_ARCH}) SET(CPACK_GENERATOR "TGZ") SET(CPACK_PACKAGE_VENDOR "CoR-Lab Bielefeld University") IF(CMAKE_SYSTEM_NAME STREQUAL "Linux") SET(CPACK_GENERATOR "${CPACK_GENERATOR};DEB") SET(CPACK_DEBIAN_PACKAGE_NAME "amarsi-liboncilla") SET(CPACK_DEBIAN_PACKAGE_VERSION ${LIBONCILLA_VERSION_MAJOR}.${LIBONCILLA_VERSION_MINOR}.${LIBONCILLA_VERSION_PATCH}) SET(CPACK_DEBIAN_PACKAGE_MAINTAINER "Arne Nordmann (anordman@cor-lab.uni-bielefeld.de)") SET(CPACK_DEBIAN_PACKAGE_DESCRIPTION "AMARSi RCI representations of the AMARSi quadruped robot 'Oncilla'.") SET(CPACK_DEBIAN_PACKAGE_PRIORITY "optional") SET(CPACK_DEBIAN_PACKAGE_SECTION "devel") SET(CPACK_DEBIAN_ARCHITECTURE ${CMAKE_SYSTEM_PROCESSOR}) SET(CPACK_DEBIAN_PACKAGE_DEPENDS "amarsi-rci-all") SET(CPACK_DEBIAN_PACKAGE_SUGGESTS "cmake, cmake-data, doxygen, lcov, cppcheck, sloccount") ENDIF(CMAKE_SYSTEM_NAME STREQUAL "Linux") INCLUDE(CPack) diff --git a/src/Oncilla.cpp b/src/Oncilla.cpp index 84829df..98ec867 100644 --- a/src/Oncilla.cpp +++ b/src/Oncilla.cpp @@ -1,87 +1,92 @@ /* * Oncilla.cpp * * Created on: 15 dec. 2011 * Author: Alexandre Tuleu, Arne Nordmann */ #include "Oncilla.h" +#include "OncillaTrunk.h" +#include "OncillaL01.h" +#include "OncillaL2.h" +#include "OncillaL3.h" +#include "OncillaL4.h" namespace rci { namespace oncilla { Oncilla::Oncilla() : synchronizer(), d_l0s(), d_l1s(), d_l2s(), d_l3s(), d_l4s(), d_trunk() { } Oncilla::~Oncilla() { // TODO Auto-generated destructor stub } void Oncilla::init() { d_l0s.push_back(OncillaL0Ptr(new OncillaL0("Oncilla L0 LEFT FORE"))); d_l0s.push_back(OncillaL0Ptr(new OncillaL0("Oncilla L0 RIGHT FORE"))); d_l0s.push_back(OncillaL0Ptr(new OncillaL0("Oncilla L0 LEFT HIND"))); d_l0s.push_back(OncillaL0Ptr(new OncillaL0("Oncilla L0 RIGHT HIND"))); d_l1s.push_back( OncillaL1Ptr(new OncillaL1(*synchronizer, "Oncilla L1 LEFT FORE"))); d_l1s.push_back( OncillaL1Ptr(new OncillaL1(*synchronizer, "Oncilla L1 RIGHT FORE"))); d_l1s.push_back( OncillaL1Ptr(new OncillaL1(*synchronizer, "Oncilla L1 LEFT HIND"))); d_l1s.push_back( OncillaL1Ptr(new OncillaL1(*synchronizer, "Oncilla L1 RIGHT HIND"))); d_l2s.push_back( OncillaL2Ptr(new OncillaL2(*synchronizer, "Oncilla L2 LEFT FORE"))); d_l2s.push_back( OncillaL2Ptr(new OncillaL2(*synchronizer, "Oncilla L2 RIGHT FORE"))); d_l2s.push_back( OncillaL2Ptr(new OncillaL2(*synchronizer, "Oncilla L2 LEFT HIND"))); d_l2s.push_back( OncillaL2Ptr(new OncillaL2(*synchronizer, "Oncilla L2 RIGHT HIND"))); d_l3s.push_back(OncillaL3Ptr(new OncillaL3("Oncilla L3 LEFT FORE"))); d_l3s.push_back(OncillaL3Ptr(new OncillaL3("Oncilla L3 RIGHT FORE"))); d_l3s.push_back(OncillaL3Ptr(new OncillaL3("Oncilla L3 LEFT HIND"))); d_l3s.push_back(OncillaL3Ptr(new OncillaL3("Oncilla L3 RIGHT HIND"))); d_l4s.push_back(OncillaL4Ptr(new OncillaL4("L4 LEFT FORE"))); d_l4s.push_back(OncillaL4Ptr(new OncillaL4("L4 RIGHT FORE"))); d_l4s.push_back(OncillaL4Ptr(new OncillaL4("L4 LEFT HIND"))); d_l4s.push_back(OncillaL4Ptr(new OncillaL4("L4 RIGHT HIND"))); d_trunk = OncillaTrunkPtr(new OncillaTrunk("Oncilla Trunk")); } OncillaL0Ptr Oncilla::getL0(Leg l) const { return this->d_l0s[l]; } OncillaL1Ptr Oncilla::getL1(Leg l) const { return this->d_l1s[l]; } OncillaL2Ptr Oncilla::getL2(Leg l) const { return this->d_l2s[l]; } OncillaL3Ptr Oncilla::getL3(Leg l) const { return this->d_l3s[l]; } OncillaL4Ptr Oncilla::getL4(Leg l) const { return this->d_l4s[l]; } OncillaTrunkPtr Oncilla::getTrunk() const { return this->d_trunk; } void Oncilla::setSynchronizer(OncillaSynchronizerPtr sync) { this->synchronizer = sync; } } } diff --git a/src/Oncilla.h b/src/Oncilla.h index 1330ad4..0dcdbe5 100644 --- a/src/Oncilla.h +++ b/src/Oncilla.h @@ -1,67 +1,78 @@ /* * Oncilla.h * * Created on: 15 dec. 2011 * Author: Alexandre Tuleu, Arne Nordmann */ #pragma once -#include "OncillaTrunk.h" -#include "OncillaL01.h" -#include "OncillaL2.h" -#include "OncillaL3.h" -#include "OncillaL4.h" +#include +#include +#include /* * */ /* * */ namespace rci { namespace oncilla { +class Oncilla; +typedef boost::shared_ptr OncillaPtr; + class OncillaSynchronizer; typedef boost::shared_ptr OncillaSynchronizerPtr; -class Oncilla; -typedef boost::shared_ptr OncillaPtr; +class OncillaTrunk; +typedef boost::shared_ptr OncillaTrunkPtr; +class OncillaL0; +typedef boost::shared_ptr OncillaL0Ptr; +class OncillaL1; +typedef boost::shared_ptr OncillaL1Ptr; +class OncillaL2; +typedef boost::shared_ptr OncillaL2Ptr; +class OncillaL3; +typedef boost::shared_ptr OncillaL3Ptr; +class OncillaL4; +typedef boost::shared_ptr OncillaL4Ptr; class Oncilla { public: enum Leg { LEFT_FORE = 0, RIGHT_FORE = 1, LEFT_HIND = 2, RIGHT_HIND = 3, NUM_LEGS = 4 }; Oncilla(); virtual ~Oncilla(); OncillaL0Ptr getL0(Leg l) const; OncillaL1Ptr getL1(Leg l) const; OncillaL2Ptr getL2(Leg l) const; OncillaL3Ptr getL3(Leg l) const; OncillaL4Ptr getL4(Leg l) const; OncillaTrunkPtr getTrunk() const; void setSynchronizer(OncillaSynchronizerPtr synchronizer); private: void init(); OncillaSynchronizerPtr synchronizer; std::vector d_l0s; std::vector d_l1s; std::vector d_l2s; std::vector d_l3s; std::vector d_l4s; OncillaTrunkPtr d_trunk; }; } } diff --git a/src/OncillaL01.cpp b/src/OncillaL01.cpp index 4b1c2db..5fd2081 100644 --- a/src/OncillaL01.cpp +++ b/src/OncillaL01.cpp @@ -1,95 +1,101 @@ #include "OncillaL01.h" using namespace std; namespace rci { namespace oncilla { OncillaL0::OncillaL0(const std::string &name) : ResourceNode(name), Controlled(), PositionControlled() { } bool OncillaL0::isConverged() const { return false; } bool OncillaL0::setJointPosition(JointAnglesPtr position) { // Successfull this->_lastCommandedPosition = position; return true; } JointAnglesPtr OncillaL0::getLastPositionCommand() const { // TODO: The second part f this condition shouldn`t be necessary if (this->_lastCommandedPosition && this->_lastCommandedPosition.get()) { //std::cout << this->_lastCommandedPosition->print(); return this->_lastCommandedPosition; } return JointAnglesPtr(); } OncillaL0::~OncillaL0() { } std::string OncillaL0::print() const { ostringstream outstream(ostringstream::out); outstream.precision(3); // Precision when printing double values outstream << "" << endl; return outstream.str(); } OncillaL1::OncillaL1(OncillaSynchronizer &s, const std::string &name) : ResourceNode(name), Controlled(), Sensing(), PositionControlled(), - PositionSensing(), TorqueControlled(), ImpedanceControlled() { + PositionSensing(), TorqueControlled(), ImpedanceControlled(), + synchronizer(s) { this->_dimension = 2; // Two encoder values - should always be the same } bool OncillaL1::isConverged() const { throw std::runtime_error("Not yet implemented."); return false; } bool OncillaL1::setJointPosition(JointAnglesPtr position) { // Successfull this->_lastCommandedPosition = position; return true; } JointAnglesPtr OncillaL1::getJointPosition() const { return this->_latestJointPosition; } bool OncillaL1::setJointVelocity(JointVelocitiesPtr vel) { throw std::runtime_error("Not yet implemented."); } bool OncillaL1::setJointTorque(JointTorquesPtr torque) { throw std::runtime_error("Not yet implemented."); } bool OncillaL1::setJointImpedance(JointImpedancePtr imped) { - throw std::runtime_error("Not yet implemented."); + if (this->synchronizer.active()) { + throw std::runtime_error( + "Synchronizer has to be switched off to change JointImpedance."); + } else { + throw std::runtime_error("Not yet implemented."); + } } JointAnglesPtr OncillaL1::getLastPositionCommand() const { // TODO: The second part f this condition shouldn`t be necessary if (this->_lastCommandedPosition && this->_lastCommandedPosition.get()) { //std::cout << this->_lastCommandedPosition->print(); return this->_lastCommandedPosition; } // No command set yet. Returning current joint values instead return this->_latestJointPosition; } OncillaL1::~OncillaL1() { } std::string OncillaL1::print() const { ostringstream outstream(ostringstream::out); outstream.precision(3); // Precision when printing double values outstream << "" << endl; return outstream.str(); } } } diff --git a/src/OncillaL01.h b/src/OncillaL01.h index 6497610..0341a78 100644 --- a/src/OncillaL01.h +++ b/src/OncillaL01.h @@ -1,146 +1,153 @@ #pragma once #include #include #include #include #include #include #include +#include "OncillaSynchronizer.h" + namespace rci { namespace oncilla { -class OncillaSynchronizer; - -class OncillaL1; -typedef boost::shared_ptr OncillaL1Ptr; +class OncillaL0; +typedef boost::shared_ptr OncillaL0Ptr; /** * Node class, representing the hip node of the quadruped robot. - * * @todo In case of simulation, this node can also sense the power consumption. */ -class OncillaL1: public rci::ResourceNode, +class OncillaL0: public rci::ResourceNode, public rci::Controlled, - public rci::Sensing, - public rci::PositionControlled, - public rci::PositionSensing, // Two encoder values - public rci::VelocityControlled, - public rci::TorqueControlled, - public rci::ImpedanceControlled { + public rci::PositionControlled { public: /** * Special constructor to also link to webots */ - OncillaL1(OncillaSynchronizer &s, - const std::string &name = "Oncilla Hip"); + OncillaL0(const std::string &name = "Oncilla Hip"); - virtual ~OncillaL1(); + virtual ~OncillaL0(); /** * Returns, if controller is converged. * @return True, if controller is converged after last command. */ bool isConverged() const; /** * Commanding a joint position. * @param position Position command * @return Return, if successfull. (e.g. not exceeding joint limits) * @todo Check for correct control mode */ bool setJointPosition(JointAnglesPtr position); - /** - * Returns current joint position. - * @return Current joint position - */ - virtual JointAnglesPtr getJointPosition() const; - - /** - * Commanding a joint velocity. - * @param position Position command - * @return Return, if successfull. (e.g. not exceeding joint limits) - * @todo Check for correct control mode - */ - virtual bool setJointVelocity(rci::JointVelocitiesPtr vel); - - /** - * Returns current joint torque. - * @return Current joint torque - */ - virtual bool setJointTorque(JointTorquesPtr torque); - - /** - * Returns current joint torque. - * @return Current joint torque - */ - virtual bool setJointImpedance(JointImpedancePtr imped); - /** * Returns latest position command. Note, that this is the latest valid * commanded position. If the position command exceeds limits and is * therefore rejected, it .. */ virtual JointAnglesPtr getLastPositionCommand() const; /** * Print */ std::string print() const; }; -class OncillaL0; -typedef boost::shared_ptr OncillaL0Ptr; +class OncillaL1; +typedef boost::shared_ptr OncillaL1Ptr; /** * Node class, representing the hip node of the quadruped robot. + * * @todo In case of simulation, this node can also sense the power consumption. */ -class OncillaL0: public rci::ResourceNode, +class OncillaL1: public rci::ResourceNode, public rci::Controlled, - public rci::PositionControlled { + public rci::Sensing, + public rci::PositionControlled, + public rci::PositionSensing, // Two encoder values + public rci::VelocityControlled, + public rci::TorqueControlled, + public rci::ImpedanceControlled { public: /** * Special constructor to also link to webots */ - OncillaL0(const std::string &name = "Oncilla Hip"); + OncillaL1(OncillaSynchronizer &s, const std::string &name = "Oncilla Hip"); - virtual ~OncillaL0(); + virtual ~OncillaL1(); /** * Returns, if controller is converged. * @return True, if controller is converged after last command. */ bool isConverged() const; /** * Commanding a joint position. * @param position Position command * @return Return, if successfull. (e.g. not exceeding joint limits) * @todo Check for correct control mode */ bool setJointPosition(JointAnglesPtr position); + /** + * Returns current joint position. + * @return Current joint position + */ + virtual JointAnglesPtr getJointPosition() const; + + /** + * Commanding a joint velocity. + * @param position Position command + * @return Return, if successfull. (e.g. not exceeding joint limits) + * @todo Check for correct control mode + */ + virtual bool setJointVelocity(rci::JointVelocitiesPtr vel); + + /** + * Returns current joint torque. + * @return Current joint torque + */ + virtual bool setJointTorque(JointTorquesPtr torque); + + /** + * Returns current joint torque. + * @return Current joint torque + */ + virtual bool setJointImpedance(JointImpedancePtr imped); + /** * Returns latest position command. Note, that this is the latest valid * commanded position. If the position command exceeds limits and is * therefore rejected, it .. */ virtual JointAnglesPtr getLastPositionCommand() const; /** * Print */ std::string print() const; + +private: + /** + * Reference to OncillaSynchronizer + * @todo This is a dangerous reference which will become invalid as soon as + * OncillaSynchronizer gets deleted outside. + */ + OncillaSynchronizer &synchronizer; }; } } diff --git a/src/OncillaL2.cpp b/src/OncillaL2.cpp index 07c6f8d..a318962 100644 --- a/src/OncillaL2.cpp +++ b/src/OncillaL2.cpp @@ -1,66 +1,71 @@ #include "OncillaL2.h" using namespace std; using namespace boost; namespace rci { namespace oncilla { OncillaL2::OncillaL2(OncillaSynchronizer &s, const std::string &name) : ResourceNode(name), Controlled(), Sensing(), PositionControlled(), PositionSensing(), VelocityControlled(), TorqueControlled(), - ImpedanceControlled() { + ImpedanceControlled(), synchronizer(s) { // Two encoder values - difference is due to passive compliance this->_dimension = 2; } bool OncillaL2::isConverged() const { return false; } bool OncillaL2::setJointPosition(JointAnglesPtr position) { // Successfull // std::cerr << "[OncillaKnee] " << position << std::endl; this->_lastCommandedPosition = position; return true; } JointAnglesPtr OncillaL2::getJointPosition() const { return this->_latestJointPosition; } OncillaL2::~OncillaL2() { } std::string OncillaL2::print() const { ostringstream outstream(ostringstream::out); outstream.precision(3); // Precision when printing double values outstream << "" << endl; return outstream.str(); } bool OncillaL2::setJointTorque(JointTorquesPtr torques) { throw std::runtime_error("Not yet implemented."); } bool OncillaL2::setJointImpedance(JointImpedancePtr imped) { - throw std::runtime_error("Not yet implemented."); + if (this->synchronizer.active()) { + throw std::runtime_error( + "Synchronizer has to be switched off to change JointImpedance."); + } else { + throw std::runtime_error("Not yet implemented."); + } } bool OncillaL2::setJointVelocity(JointVelocitiesPtr vel) { throw std::runtime_error("Not yet implemented."); } JointAnglesPtr OncillaL2::getLastPositionCommand() const { // TODO: The second part f this condition shouldn`t be necessary if (!this->_lastCommandedPosition || !this->_lastCommandedPosition.get()) { // No command set yet. Returning current joint values instead // FIXME: Instead of return current position set command to current position return this->_latestJointPosition; } return this->_lastCommandedPosition; } } } diff --git a/src/OncillaL2.h b/src/OncillaL2.h index ce38d86..fe3cdba 100644 --- a/src/OncillaL2.h +++ b/src/OncillaL2.h @@ -1,98 +1,100 @@ #pragma once #include #include #include #include #include #include #include +#include "OncillaSynchronizer.h" + namespace rci { namespace oncilla { -class OncillaSynchronizer; - class OncillaL2; typedef boost::shared_ptr OncillaL2Ptr; /** * Node class, representing the knee node of the quadruped robot. * @todo In case of simulation, this node can also sense the power consumption. */ class OncillaL2: public rci::ResourceNode, public rci::Controlled, public rci::Sensing, public rci::PositionControlled, public rci::PositionSensing, public rci::VelocityControlled, public rci::TorqueControlled, public rci::ImpedanceControlled { public: - OncillaL2(OncillaSynchronizer &s, - const std::string &name = "Oncilla Knee"); + OncillaL2(OncillaSynchronizer &s, const std::string &name = "Oncilla Knee"); virtual ~OncillaL2(); /** * Returns, if controller is converged. * @return True, if controller is converged after last command. */ bool isConverged() const; /** * Commanding a joint position. * @param position Position command * @return Return, if successfull. (e.g. not exceeding joint limits) * @todo Check for correct control mode */ bool setJointPosition(JointAnglesPtr position); /** * Returns current joint position. * @return Current joint position */ virtual JointAnglesPtr getJointPosition() const; /** * Commanding a joint velocity. * @param position Velocity command * @return Return, if successfull. (e.g. not exceeding joint limits) * @todo Check for correct control mode */ bool setJointVelocity(JointVelocitiesPtr vel); /** * Commanding a joint velocity. * @param position Velocity command * @return Return, if successfull. (e.g. not exceeding joint limits) * @todo Check for correct control mode */ bool setJointImpedance(JointImpedancePtr imped); /** * Commanding a joint torque. * @param position Torque command * @return Return, if successfull. (e.g. not exceeding joint limits) * @todo Check for correct control mode */ virtual bool setJointTorque(rci::JointTorquesPtr torques); /** * Returns latest position command. Note, that this is the latest valid * commanded position. If the position command exceeds limits and is * therefore rejected, it .. */ virtual JointAnglesPtr getLastPositionCommand() const; /** * Print */ std::string print() const; + +private: + OncillaSynchronizer &synchronizer; }; } } diff --git a/src/OncillaSynchronizer.h b/src/OncillaSynchronizer.h index 8a7e23b..8b32f93 100644 --- a/src/OncillaSynchronizer.h +++ b/src/OncillaSynchronizer.h @@ -1,86 +1,98 @@ #pragma once #include #include #include #include -#include "Oncilla.h" -#include "OncillaTrunk.h" -#include "OncillaL01.h" -#include "OncillaL2.h" -#include "OncillaL3.h" -#include "OncillaL4.h" - namespace rci { namespace oncilla { class OncillaSynchronizer; typedef boost::shared_ptr OncillaSynchronizerPtr; +class OncillaTrunk; +typedef boost::shared_ptr OncillaTrunkPtr; +class OncillaL0; +typedef boost::shared_ptr OncillaL0Ptr; +class OncillaL1; +typedef boost::shared_ptr OncillaL1Ptr; +class OncillaL2; +typedef boost::shared_ptr OncillaL2Ptr; +class OncillaL3; +typedef boost::shared_ptr OncillaL3Ptr; +class OncillaL4; +typedef boost::shared_ptr OncillaL4Ptr; + /** * OncillaSynchronizer * Different OncillaSynchronizers may be for example: * * Update with a given frequency * ** Send data every time * ** Send only if new data * * Update only if new data (event-based) * * Triggered from robot side (e.g. FRI) */ -class OncillaSynchronizer : public Synchronizer { +class OncillaSynchronizer: public Synchronizer { public: OncillaSynchronizer(const std::string &name); virtual ~OncillaSynchronizer(); /** * Registers a trunk node. + * @todo use Oncilla::Leg as index (problem with cyclic includes) */ - virtual void registerTrunkNode(Oncilla::Leg index, OncillaTrunkPtr node) = 0; + virtual void registerTrunkNode(unsigned int index, OncillaTrunkPtr node) = 0; /** * Registers an L0 node. + * @todo use Oncilla::Leg as index (problem with cyclic includes) */ - virtual void registerL0Node(Oncilla::Leg index, OncillaL0Ptr node) = 0; + virtual void registerL0Node(unsigned int index, OncillaL0Ptr node) = 0; /** * Registers an L1 node. + * @todo use Oncilla::Leg as index (problem with cyclic includes) */ - virtual void registerL1Node(Oncilla::Leg index, OncillaL1Ptr node) = 0; + virtual void registerL1Node(unsigned int index, OncillaL1Ptr node) = 0; /** * Registers an L2 node. + * @todo use Oncilla::Leg as index (problem with cyclic includes) */ - virtual void registerL2Node(Oncilla::Leg index, OncillaL2Ptr node) = 0; + virtual void registerL2Node(unsigned int index, OncillaL2Ptr node) = 0; /** * Registers an L3 node. + * @todo use Oncilla::Leg as index (problem with cyclic includes) */ - virtual void registerL3Node(Oncilla::Leg index, OncillaL3Ptr node) = 0; + virtual void registerL3Node(unsigned int index, OncillaL3Ptr node) = 0; /** * Registers an L4 node. + * @todo use Oncilla::Leg as index (problem with cyclic includes) */ - virtual void registerL4Node(Oncilla::Leg index, OncillaL4Ptr node) = 0; + virtual void registerL4Node(unsigned int index, OncillaL4Ptr node) = 0; /** * */ friend std::ostream & operator<<(std::ostream& os, const OncillaSynchronizer& val); protected: /** * Only true for the first time */ bool _onlyReceive; /** * */ std::string name; }; } }