diff --git a/examples/SimpleSineMovement.cpp b/examples/SimpleSineMovement.cpp index f81b549..a975558 100644 --- a/examples/SimpleSineMovement.cpp +++ b/examples/SimpleSineMovement.cpp @@ -1,82 +1,82 @@ #include #include #include "liboncilla/Oncilla.h" using namespace std; using namespace boost; using namespace rci; using namespace rci::oncilla; double hip_position(double phase,double amplitude,double offset){ return amplitude * std::cos(phase) + offset; } double knee_position(double phase, double phase_lag,double amplitude,double offset){ return amplitude *((phase + phase_lag) > M_PI ? 1.0 : 0.0) + offset; } int main() { // Instantiate the actual robot object Oncilla oncilla; // Get the synchronizer, so that we can control the actual simulation // stepping from within our code - OncillaSynchronizerPtr synchronizer = oncilla.getSynchronizer(); + ::rci::oncilla::Synchronizer::Ptr synchronizer = oncilla.getSynchronizer(); // Get all the joints we are interested in - L1 and L2 L1L2::Ptr left_fore_hip = oncilla.getL1(LEFT_FORE); L1L2::Ptr left_fore_knee = oncilla.getL2(LEFT_FORE); L1L2::Ptr right_fore_hip = oncilla.getL1(RIGHT_FORE); L1L2::Ptr right_fore_knee = oncilla.getL2(RIGHT_FORE); L1L2::Ptr left_hind_hip = oncilla.getL1(LEFT_HIND); L1L2::Ptr left_hind_knee = oncilla.getL2(LEFT_HIND); L1L2::Ptr right_hind_hip = oncilla.getL1(RIGHT_HIND); L1L2::Ptr right_hind_knee = oncilla.getL2(RIGHT_HIND); //sets some parameter for the sinewave double hip_amplitude(10), fore_hip_offset(0), hind_hip_offset(0), fore_knee_amplitude(0.5), fore_knee_offset(0), hind_knee_amplitude(0), hind_knee_offset(0), fore_hip_knee_phase_lag(0), hind_hip_knee_phase_lag(0), frequency(0.5); // Now move the joints double time_s, phase, antiphase; time_s = 0; while (true) { // The time is our phase - TODO: Use virtual / simulated time time_s += synchronizer->lastProcessTimeStep(); phase = std::fmod(time_s * frequency * 2 * M_PI, 2 * M_PI); antiphase = std::fmod(phase + M_PI , 2 * M_PI); left_fore_hip->setJointPosition(JointAngles::fromDeg(hip_position(phase,hip_amplitude,fore_hip_offset))); left_fore_knee->setJointPosition(JointAngles::fromRad(knee_position(phase,fore_hip_knee_phase_lag,fore_knee_amplitude,fore_knee_offset))); right_fore_hip->setJointPosition(JointAngles::fromDeg(hip_position(antiphase,hip_amplitude,fore_hip_offset))); right_fore_knee->setJointPosition(JointAngles::fromRad(knee_position(antiphase,fore_hip_knee_phase_lag,fore_knee_amplitude,fore_knee_offset))); left_fore_hip->setJointPosition(JointAngles::fromDeg(hip_position(antiphase,hip_amplitude,hind_hip_offset))); left_fore_knee->setJointPosition(JointAngles::fromRad(knee_position(antiphase,hind_hip_knee_phase_lag,hind_knee_amplitude,hind_knee_offset))); right_fore_hip->setJointPosition(JointAngles::fromDeg(hip_position(phase,hip_amplitude,hind_hip_offset))); right_fore_knee->setJointPosition(JointAngles::fromRad(knee_position(phase,hind_hip_knee_phase_lag,hind_knee_amplitude,hind_knee_offset))); // Now that we set the new angles, we do one processing step synchronizer->process(); } return EXIT_SUCCESS; } diff --git a/src/liboncilla/CMakeLists.txt b/src/liboncilla/CMakeLists.txt index 121b223..8532345 100644 --- a/src/liboncilla/CMakeLists.txt +++ b/src/liboncilla/CMakeLists.txt @@ -1,47 +1,47 @@ set(ONCILLAHEADERS nodes/Trunk.h nodes/L0.h nodes/L1L2.h nodes/L3.h Oncilla.h - OncillaSynchronizer.h + Synchronizer.h utils/BackendLoader.h utils/OncillaBackend.h common.h exceptions/LoadingError.h exceptions/NotImplementedError.h) set(ONCILLASOURCES nodes/Trunk.cpp nodes/L0.cpp nodes/L1L2.cpp nodes/L3.cpp Oncilla.cpp - OncillaSynchronizer.cpp + Synchronizer.cpp utils/OncillaBackend.cpp exceptions/LoadingError.cpp exceptions/NotImplementedError.cpp utils/BackendLoader.cpp) add_library(oncilla SHARED ${ONCILLASOURCES} ${ONCILLAHEADERS}) target_link_libraries(oncilla ${RCI_LIBRARIES} ${BIOROB_CPP_LIBRARIES}) set_target_properties(oncilla PROPERTIES VERSION ${VERSION_STRING} SOVERSION ${VERSION_ABI}) install(TARGETS oncilla DESTINATION lib) set(DEST_INCLUDE_DIR include/liboncilla-${VERSION_API}) install(DIRECTORY ${PROJECT_SOURCE_DIR}/src/liboncilla ${PROJECT_BINARY_DIR}/src/liboncilla DESTINATION ${DEST_INCLUDE_DIR} FILES_MATCHING PATTERN "*.h" PATTERN "CMakeFiles" EXCLUDE ) diff --git a/src/liboncilla/Oncilla.cpp b/src/liboncilla/Oncilla.cpp index 0c16bdb..1bd2af1 100644 --- a/src/liboncilla/Oncilla.cpp +++ b/src/liboncilla/Oncilla.cpp @@ -1,109 +1,109 @@ /* * Oncilla.cpp * * Created on: 15 dec. 2011 * Author: Alexandre Tuleu, Arne Nordmann */ #include "Oncilla.h" #include namespace rci { namespace oncilla { -OncillaSynchronizerPtr Oncilla::s_synchronizer; +::rci::oncilla::Synchronizer::Ptr Oncilla::s_synchronizer; Oncilla::LegNames Oncilla::s_legNames; const std::string & Oncilla::nameOfLeg(Leg l){ if(s_legNames.empty()){ s_legNames.push_back("Left Fore"); s_legNames.push_back("Right Fore"); s_legNames.push_back("Left Hind"); s_legNames.push_back("Right Hind"); s_legNames.push_back("Undefined"); } if(l >= NUM_LEGS){ return s_legNames.back(); } return s_legNames[l]; } Oncilla::Oncilla(){ if(!s_synchronizer){ if(!d_backend){ BackendLoader bl; //we have to keep alive the pointer or dlclose() will be called once //We free him. Or we could add BackendLoader as a member of Oncilla // I prefer only to have the Object creator interface. d_backend = bl.SuitableBackend(); } s_synchronizer = d_backend->CreateSynchronizer(); init(); } } Oncilla::~Oncilla() { } #define CREATE_NODES(LegName,Leg)do{\ d_L0s.push_back(L0::Ptr(d_backend->CreateL0(Leg,LegName " Oncilla L0" )));\ d_L1s.push_back(L1::Ptr(d_backend->CreateL1(Leg,\ LegName " Oncilla L1")));\ d_L2s.push_back(L2::Ptr(d_backend->CreateL2(Leg,\ LegName " Oncilla L2")));\ d_L3s.push_back(L3::Ptr(d_backend->CreateL3(Leg,LegName " Oncilla L3")));\ }while(0) void Oncilla::init() { d_L0s.reserve(4); d_L1s.reserve(4); d_L2s.reserve(4); d_L3s.reserve(4); CREATE_NODES("Left Fore",LEFT_FORE); CREATE_NODES("Right Fore",RIGHT_FORE); CREATE_NODES("Left Hind",LEFT_HIND); CREATE_NODES("Right Hind",RIGHT_HIND); d_trunk = Trunk::Ptr(new Trunk("Oncilla Trunk")); } const L0::Ptr & Oncilla::getL0(Leg l) const { return this->d_L0s[l]; } const L1::Ptr & Oncilla::getL1(Leg l) const { return this->d_L1s[l]; } const L2::Ptr & Oncilla::getL2(Leg l) const { return this->d_L2s[l]; } const L3::Ptr & Oncilla::getL3(Leg l) const { return this->d_L3s[l]; } const Trunk::Ptr & Oncilla::getTrunk() const { return this->d_trunk; } -OncillaSynchronizerPtr Oncilla::getSynchronizer() const { +const rci::oncilla::Synchronizer::Ptr & Oncilla::getSynchronizer() const { if(!s_synchronizer){ throw std::logic_error("Internal error : synchronizer seems unitialized" ", but it should never happen. Please report this bug."); } return this->s_synchronizer; } } } diff --git a/src/liboncilla/Oncilla.h b/src/liboncilla/Oncilla.h index 9bcdc87..1ec0fc7 100644 --- a/src/liboncilla/Oncilla.h +++ b/src/liboncilla/Oncilla.h @@ -1,64 +1,60 @@ /* * Oncilla.h * * Created on: 15 dec. 2011 * Author: Alexandre Tuleu, Arne Nordmann */ #pragma once #include #include #include #include "common.h" -#include "OncillaSynchronizer.h" #include +#include +#include namespace rci { namespace oncilla { class BackendLoader; -class Oncilla; -typedef boost::shared_ptr OncillaPtr; - -class OncillaSynchronizer; -typedef boost::shared_ptr OncillaSynchronizerPtr; class Oncilla { public: - + typedef boost::shared_ptr Ptr; static const std::string & nameOfLeg(Leg l); Oncilla(); virtual ~Oncilla(); const L0::Ptr & getL0(Leg l) const; const L1::Ptr & getL1(Leg l) const; const L2::Ptr & getL2(Leg l) const; const L3::Ptr & getL3(Leg l) const; const Trunk::Ptr & getTrunk() const; - OncillaSynchronizerPtr getSynchronizer() const; + const rci::oncilla::Synchronizer::Ptr & getSynchronizer() const; private: typedef std::vector LegNames; static LegNames s_legNames; void init(); - static OncillaSynchronizerPtr s_synchronizer; + static rci::oncilla::Synchronizer::Ptr s_synchronizer; std::vector d_L0s; std::vector d_L1s; std::vector d_L2s; std::vector d_L3s; Trunk::Ptr d_trunk; OncillaBackend::Ptr d_backend; }; } } diff --git a/src/liboncilla/OncillaSynchronizer.cpp b/src/liboncilla/OncillaSynchronizer.cpp deleted file mode 100644 index aed519b..0000000 --- a/src/liboncilla/OncillaSynchronizer.cpp +++ /dev/null @@ -1,18 +0,0 @@ -#include "OncillaSynchronizer.h" - -#include - -using namespace std; - -namespace rci { -namespace oncilla { - -OncillaSynchronizer::OncillaSynchronizer(const std::string &sname) - : Synchronizer(sname){ -} - -OncillaSynchronizer::~OncillaSynchronizer() { -} - -} -} diff --git a/src/liboncilla/Synchronizer.cpp b/src/liboncilla/Synchronizer.cpp new file mode 100644 index 0000000..0ce4d6d --- /dev/null +++ b/src/liboncilla/Synchronizer.cpp @@ -0,0 +1,18 @@ +#include "Synchronizer.h" + +#include + +using namespace std; + +namespace rci { +namespace oncilla { + +Synchronizer::Synchronizer(const std::string &sname) + : ::rci::Synchronizer(sname){ +} + +Synchronizer::~Synchronizer() { +} + +} +} diff --git a/src/liboncilla/OncillaSynchronizer.h b/src/liboncilla/Synchronizer.h similarity index 59% rename from src/liboncilla/OncillaSynchronizer.h rename to src/liboncilla/Synchronizer.h index 5e09b00..327c9ab 100644 --- a/src/liboncilla/OncillaSynchronizer.h +++ b/src/liboncilla/Synchronizer.h @@ -1,48 +1,46 @@ #pragma once #include #include #include #include #include "common.h" -#include namespace rci { namespace oncilla { -class OncillaSynchronizer; -typedef boost::shared_ptr OncillaSynchronizerPtr; - /** - * OncillaSynchronizer + * Synchronizer * 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 Synchronizer: public ::rci::Synchronizer { public: - OncillaSynchronizer(const std::string &name); - virtual ~OncillaSynchronizer(); + typedef boost::shared_ptr Ptr; + + Synchronizer(const std::string &name); + virtual ~Synchronizer(); /** * */ friend std::ostream - & operator<<(std::ostream& os, const OncillaSynchronizer& val); + & operator<<(std::ostream& os, const Synchronizer& val); virtual double lastProcessTimeStep() const = 0; }; } } diff --git a/src/liboncilla/nodes/L1L2.cpp b/src/liboncilla/nodes/L1L2.cpp index c5ea94b..45bb205 100644 --- a/src/liboncilla/nodes/L1L2.cpp +++ b/src/liboncilla/nodes/L1L2.cpp @@ -1,108 +1,108 @@ #include "L1L2.h" #include using namespace std; namespace rci{ namespace oncilla{ -L1L2::L1L2(OncillaSynchronizer &s, const std::string &name) +L1L2::L1L2(Synchronizer &s, const std::string &name) : ResourceNode(name) , Controlled() , Sensing() , PositionControlled() , PositionSensing() , TorqueControlled() , ImpedanceControlled() , synchronizer(s) { /// \todo add default value as setting this->_controlMode = modeJointPositionControl; this->_lastCommandedPosition = JointAngles::create(1,0.0); this->_latestJointPosition = JointAngles::create(2,0.0); } bool L1L2::isConverged() const { throw NotImplementedError("OncillaL1L2","isConverged"); } bool L1L2::setJointPosition(JointAnglesPtr position) { // Successfull this->_lastCommandedPosition = position; return true; } JointAnglesPtr L1L2::getJointPosition() const { return this->_latestJointPosition; } bool L1L2::setJointVelocity(JointVelocitiesPtr ) { throw NotImplementedError("OncillaL1L2","SetJointVelocity"); } bool L1L2::setJointTorque(JointTorquesPtr ) { throw NotImplementedError("OncillaL1L2","SetJointTorque"); } bool L1L2::setJointImpedance(JointImpedancePtr ) { throw NotImplementedError("OncillaL1L2","setJointImpedence"); // if (this->synchronizer.tooBusy()) { // throw std::runtime_error( // "Synchronizer has to be switched off to change JointImpedance."); // } else { // throw std::runtime_error("Impedance control for OncillaL1/L2 is not yet implemented."); // } } JointAnglesPtr L1L2::getLastPositionCommand() const { if (this->_lastCommandedPosition) { return this->_lastCommandedPosition; } // If we don`t have a command yet, we return the latest sensor value if (this->_latestJointPosition) { // TODO: Log return this->_latestJointPosition; } throw runtime_error("No position command received yet."); } L1L2::~L1L2() { } std::string L1L2::print() const { ostringstream outstream(ostringstream::out); outstream.precision(3); // Precision when printing double values outstream << "" << endl; return outstream.str(); } void L1L2::unsafeSetCommand(double value){ this->_lastCommandedPosition->setValue(0,value); } double L1L2::unsafeGetCommand() const{ return this->_lastCommandedPosition->asDouble(0); } double L1L2::unsafeGetMotorAxisPosition() const{ return this->_latestJointPosition->asDouble(MotorAxisIndex); } double L1L2::unsafeGetMagneticEncoderPosition() const{ return this->_latestJointPosition->asDouble(MagneticEncoderIndex); } void L1L2::unsafeUpdateMotorAxisPosition(double value){ this->_latestJointPosition->setValue(MotorAxisIndex,value); } void L1L2::unsafeUpdateMagneticEncoderPosition(double value){ this->_latestJointPosition->setValue(MagneticEncoderIndex,value); } } } diff --git a/src/liboncilla/nodes/L1L2.h b/src/liboncilla/nodes/L1L2.h index 46c6365..6f7e352 100644 --- a/src/liboncilla/nodes/L1L2.h +++ b/src/liboncilla/nodes/L1L2.h @@ -1,129 +1,129 @@ #pragma once #include #include #include #include #include namespace rci{ namespace oncilla{ -class OncillaSynchronizer; +class Synchronizer; class L1L2; /** * Node class, representing the hip node of the quadruped robot. * * @todo In case of simulation, this node can also sense the power consumption. */ class L1L2: 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: typedef boost::shared_ptr Ptr; const static unsigned int MotorAxisIndex = 0; const static unsigned int MagneticEncoderIndex = 1; /** * Special constructor to also link to webots */ - L1L2(OncillaSynchronizer & s, + L1L2(Synchronizer & s, const std::string & name = "Oncilla Hip"); virtual ~L1L2(); /** * 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); void unsafeSetCommand(double value); double unsafeGetCommand() const ; /** * Returns current joint position. * @return Current joint position */ virtual JointAnglesPtr getJointPosition() const; double unsafeGetMotorAxisPosition() const; double unsafeGetMagneticEncoderPosition() const; void unsafeUpdateMotorAxisPosition(double value); void unsafeUpdateMagneticEncoderPosition(double value); /** * 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; + Synchronizer &synchronizer; }; typedef L1L2 L1; typedef L1L2 L2; } } diff --git a/src/liboncilla/utils/BackendLoader.h b/src/liboncilla/utils/BackendLoader.h index ceeea77..5468bf5 100644 --- a/src/liboncilla/utils/BackendLoader.h +++ b/src/liboncilla/utils/BackendLoader.h @@ -1,32 +1,32 @@ /** * \file BackendLoader.h * * \date Jun 21, 2012 * \author Alexandre Tuleu */ #ifndef LIBONCILLA_BACKENDLOADER_H_ #define LIBONCILLA_BACKENDLOADER_H_ #include "OncillaBackend.h" namespace rci{ namespace oncilla{ - class OncillaSynchronizer; + class Synchronizer; class BackendLoader { public : BackendLoader(); virtual ~BackendLoader(); const OncillaBackend::Ptr & SuitableBackend(); private: PluginLoader d_loader; OncillaBackend::Ptr d_loadedBackend; }; } } #endif // LIBONCILLA_BACKENDFACTORY_H_ diff --git a/src/liboncilla/utils/OncillaBackend.h b/src/liboncilla/utils/OncillaBackend.h index 07545ff..efbf731 100644 --- a/src/liboncilla/utils/OncillaBackend.h +++ b/src/liboncilla/utils/OncillaBackend.h @@ -1,50 +1,50 @@ /* * Backend.h * * Created on: Jan 18, 2013 * Author: tuleu */ #ifndef LIBONCILLA_ONCILLABACKEND_H_ #define LIBONCILLA_ONCILLABACKEND_H_ #include #include #include #include namespace rci{ namespace oncilla{ - class OncillaSynchronizer; + class Synchronizer; class L0; class L1L2; class L3; typedef L1L2 L1; typedef L1L2 L2; } } class OncillaBackend { BIOROB_CPP_DECLARE_PLUGIN(OncillaBackend); DISABLE_ASSIGNEMENT(OncillaBackend); DISABLE_COPY(OncillaBackend); public : typedef std::tr1::shared_ptr Ptr; virtual ~OncillaBackend(); - virtual boost::shared_ptr CreateSynchronizer() = 0; + virtual boost::shared_ptr CreateSynchronizer() = 0; virtual boost::shared_ptr CreateL0(rci::oncilla::Leg l, const std::string & name ) = 0; virtual boost::shared_ptr CreateL1(rci::oncilla::Leg l, const std::string & name ) = 0; virtual boost::shared_ptr CreateL2(rci::oncilla::Leg l, const std::string & name ) = 0; virtual boost::shared_ptr CreateL3(rci::oncilla::Leg l, const std::string & name ) = 0; }; #endif /* LIBONCILLA_ONCILLABACKEND_H_ */ diff --git a/test/nodes/OncillaL1L2Test.cpp b/test/nodes/OncillaL1L2Test.cpp index 0f679c2..b534a7e 100644 --- a/test/nodes/OncillaL1L2Test.cpp +++ b/test/nodes/OncillaL1L2Test.cpp @@ -1,82 +1,69 @@ #include #include -#include "liboncilla/OncillaSynchronizer.h" +#include "liboncilla/Synchronizer.h" + using namespace nemo; using namespace rci; using namespace rci::oncilla; -class MockSynchronizer: public OncillaSynchronizer { +class MockSynchronizer: public ::rci::oncilla::Synchronizer { public: MockSynchronizer() : - OncillaSynchronizer("Sync") { + Synchronizer("Sync") { } virtual void processAsync() { } virtual void waitForProcessAsync() { } - virtual void registerTrunkNode(rci::oncilla::Trunk::Ptr) { - } - virtual void registerL0Node(rci::oncilla::Leg, - rci::oncilla::L0::Ptr) { - } - virtual void registerL1Node(rci::oncilla::Leg, - rci::oncilla::L1::Ptr) { - } - virtual void registerL2Node(rci::oncilla::Leg, - rci::oncilla::L2::Ptr) { - } - virtual void registerL3Node(rci::oncilla::Leg, - rci::oncilla::L3::Ptr) { - } virtual double lastProcessTimeStep() const { return 0.005; } virtual void switchToSynchronizationModeHook(){ } }; class OncillaL1L2Test: public ::testing::Test { protected: OncillaL1L2Test() : joint(), ja_values(), ji_values(), ja(), ji(), synchr() { joint = rci::oncilla::L1::Ptr(new L1(synchr, "FooBar")); ja_values = RealVector(dim(1), 1.234); ji_values = RealVector(dim(2), 1.234); ji_values[1] = 2.345; ja = JointAngles::fromRad(ja_values); ji = JointImpedancePtr(new JointImpedance(ji_values)); } virtual ~OncillaL1L2Test() { } rci::oncilla::L1L2::Ptr joint; RealVector ja_values, ji_values; JointAnglesPtr ja; JointImpedancePtr ji; MockSynchronizer synchr; }; TEST_F(OncillaL1L2Test, testInitialialState) { EXPECT_NO_THROW(joint->getLastPositionCommand()); EXPECT_EQ(0.0,joint->getLastPositionCommand()->rad()); } TEST_F(OncillaL1L2Test, testPositionCommand) { EXPECT_NO_THROW(joint->getLastPositionCommand()); EXPECT_EQ(0.0,joint->getLastPositionCommand()->rad()); joint->setJointPosition(ja); EXPECT_EQ(1.234, joint->getLastPositionCommand()->rad()); }