diff --git a/examples/SimpleSineMovement.cpp b/examples/SimpleSineMovement.cpp index 830853b..f81b549 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(); // Get all the joints we are interested in - L1 and L2 - OncillaL1Ptr left_fore_hip = oncilla.getL1(LEFT_FORE); - OncillaL2Ptr left_fore_knee = oncilla.getL2(LEFT_FORE); - OncillaL1Ptr right_fore_hip = oncilla.getL1(RIGHT_FORE); - OncillaL2Ptr right_fore_knee = oncilla.getL2(RIGHT_FORE); - OncillaL1Ptr left_hind_hip = oncilla.getL1(LEFT_HIND); - OncillaL2Ptr left_hind_knee = oncilla.getL2(LEFT_HIND); - OncillaL1Ptr right_hind_hip = oncilla.getL1(RIGHT_HIND); - OncillaL2Ptr right_hind_knee = oncilla.getL2(RIGHT_HIND); + 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 31def7b..b778afb 100644 --- a/src/liboncilla/CMakeLists.txt +++ b/src/liboncilla/CMakeLists.txt @@ -1,49 +1,49 @@ set(ONCILLAHEADERS - nodes/OncillaTrunk.h - nodes/OncillaL0.h - nodes/OncillaL1L2.h - nodes/OncillaL3.h - nodes/OncillaL4.h + nodes/Trunk.h + nodes/L0.h + nodes/L1L2.h + nodes/L3.h + nodes/L4.h Oncilla.h OncillaSynchronizer.h utils/BackendLoader.h utils/OncillaBackend.h common.h exceptions/LoadingError.h exceptions/NotImplementedError.h) set(ONCILLASOURCES - nodes/OncillaTrunk.cpp - nodes/OncillaL0.cpp - nodes/OncillaL1L2.cpp - nodes/OncillaL3.cpp - nodes/OncillaL4.cpp + nodes/Trunk.cpp + nodes/L0.cpp + nodes/L1L2.cpp + nodes/L3.cpp + nodes/L4.cpp Oncilla.cpp OncillaSynchronizer.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 a60649d..288cfc1 100644 --- a/src/liboncilla/Oncilla.cpp +++ b/src/liboncilla/Oncilla.cpp @@ -1,129 +1,129 @@ /* * Oncilla.cpp * * Created on: 15 dec. 2011 * Author: Alexandre Tuleu, Arne Nordmann */ #include "Oncilla.h" #include namespace rci { namespace oncilla { OncillaSynchronizerPtr 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)do{\ - d_L0s.push_back(OncillaL0Ptr(new OncillaL0(LegName " Oncilla L0" )));\ - d_L1s.push_back(OncillaL1Ptr(new OncillaL1L2(*s_synchronizer,\ + d_L0s.push_back(L0::Ptr(new L0(LegName " Oncilla L0" )));\ + d_L1s.push_back(L1L2::Ptr(new L1L2(*s_synchronizer,\ LegName " Oncilla L1")));\ - d_L2s.push_back(OncillaL2Ptr(new OncillaL1L2(*s_synchronizer,\ + d_L2s.push_back(L1L2::Ptr(new L1L2(*s_synchronizer,\ LegName " Oncilla L2")));\ - d_L3s.push_back(OncillaL3Ptr(new OncillaL3(LegName " Oncilla L3")));\ - d_L4s.push_back(OncillaL4Ptr(new OncillaL4(LegName " Oncilla L4")));\ + d_L3s.push_back(L3::Ptr(new L3(LegName " Oncilla L3")));\ + d_L4s.push_back(L4::Ptr(new L4(LegName " Oncilla L4")));\ }while(0) #define REGISTER_NODES(Type)do{\ - for(std::vector < Oncilla ## Type ## Ptr >::const_iterator n = d_ ## Type ## s.begin();\ + for(std::vector < Type::Ptr >::const_iterator n = d_ ## Type ## s.begin();\ n != d_ ## Type ## s.end();\ ++n){\ Leg l = static_cast(n - d_ ## Type ## s.begin());\ s_synchronizer->register ## Type ## Node(l,*n);\ }\ }while(0) void Oncilla::init() { d_L0s.reserve(4); d_L1s.reserve(4); d_L2s.reserve(4); d_L3s.reserve(4); d_L4s.reserve(4); CREATE_NODES("Left Fore"); CREATE_NODES("Right Fore"); CREATE_NODES("Left Hind"); CREATE_NODES("Right Hind"); - d_trunk = OncillaTrunkPtr(new OncillaTrunk("Oncilla Trunk")); + d_trunk = Trunk::Ptr(new Trunk("Oncilla Trunk")); REGISTER_NODES(L0); REGISTER_NODES(L1); REGISTER_NODES(L2); REGISTER_NODES(L3); REGISTER_NODES(L4); s_synchronizer->registerTrunkNode(d_trunk); } -OncillaL0Ptr Oncilla::getL0(Leg l) const { +const L0::Ptr & Oncilla::getL0(Leg l) const { return this->d_L0s[l]; } -OncillaL1Ptr Oncilla::getL1(Leg l) const { +const L1::Ptr & Oncilla::getL1(Leg l) const { return this->d_L1s[l]; } -OncillaL2Ptr Oncilla::getL2(Leg l) const { +const L2::Ptr & Oncilla::getL2(Leg l) const { return this->d_L2s[l]; } -OncillaL3Ptr Oncilla::getL3(Leg l) const { +const L3::Ptr & Oncilla::getL3(Leg l) const { return this->d_L3s[l]; } -OncillaL4Ptr Oncilla::getL4(Leg l) const { +const L4::Ptr & Oncilla::getL4(Leg l) const { return this->d_L4s[l]; } -OncillaTrunkPtr Oncilla::getTrunk() const { +const Trunk::Ptr & Oncilla::getTrunk() const { return this->d_trunk; } OncillaSynchronizerPtr 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 671bbcf..6b61e58 100644 --- a/src/liboncilla/Oncilla.h +++ b/src/liboncilla/Oncilla.h @@ -1,66 +1,66 @@ /* * Oncilla.h * * Created on: 15 dec. 2011 * Author: Alexandre Tuleu, Arne Nordmann */ #pragma once #include #include #include #include "common.h" #include "OncillaSynchronizer.h" #include namespace rci { namespace oncilla { class BackendLoader; class Oncilla; typedef boost::shared_ptr OncillaPtr; class OncillaSynchronizer; typedef boost::shared_ptr OncillaSynchronizerPtr; class Oncilla { public: static const std::string & nameOfLeg(Leg l); 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; + 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 L4::Ptr & getL4(Leg l) const; + const Trunk::Ptr & getTrunk() const; OncillaSynchronizerPtr getSynchronizer() const; private: typedef std::vector LegNames; static LegNames s_legNames; void init(); static OncillaSynchronizerPtr s_synchronizer; - std::vector d_L0s; - std::vector d_L1s; - std::vector d_L2s; - std::vector d_L3s; - std::vector d_L4s; + std::vector d_L0s; + std::vector d_L1s; + std::vector d_L2s; + std::vector d_L3s; + std::vector d_L4s; - OncillaTrunkPtr d_trunk; + Trunk::Ptr d_trunk; OncillaBackend::Ptr d_backend; }; } } diff --git a/src/liboncilla/OncillaSynchronizer.h b/src/liboncilla/OncillaSynchronizer.h index 8ecc464..e5c4f94 100644 --- a/src/liboncilla/OncillaSynchronizer.h +++ b/src/liboncilla/OncillaSynchronizer.h @@ -1,77 +1,77 @@ #pragma once #include #include #include #include #include "common.h" -#include +#include namespace rci { namespace oncilla { class OncillaSynchronizer; typedef boost::shared_ptr OncillaSynchronizerPtr; /** * 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 { public: OncillaSynchronizer(const std::string &name); virtual ~OncillaSynchronizer(); /** * Registers a trunk node. */ - virtual void registerTrunkNode(OncillaTrunkPtr node) = 0; + virtual void registerTrunkNode(Trunk::Ptr node) = 0; /** * Registers an L0 node. */ - virtual void registerL0Node(Leg l, OncillaL0Ptr node) = 0; + virtual void registerL0Node(Leg l, L0::Ptr node) = 0; /** * Registers an L1 node. */ - virtual void registerL1Node(Leg l, OncillaL1Ptr node) = 0; + virtual void registerL1Node(Leg l, L1::Ptr node) = 0; /** * Registers an L2 node. */ - virtual void registerL2Node(Leg l, OncillaL2Ptr node) = 0; + virtual void registerL2Node(Leg l, L2::Ptr node) = 0; /** * Registers an L3 node. */ - virtual void registerL3Node(Leg l, OncillaL3Ptr node) = 0; + virtual void registerL3Node(Leg l, L3::Ptr node) = 0; /** * Registers an L4 node. */ - virtual void registerL4Node(Leg l, OncillaL4Ptr node) = 0; + virtual void registerL4Node(Leg l, L4::Ptr node) = 0; /** * */ friend std::ostream & operator<<(std::ostream& os, const OncillaSynchronizer& val); virtual double lastProcessTimeStep() const = 0; }; } } diff --git a/src/liboncilla/nodes/OncillaL0.cpp b/src/liboncilla/nodes/L0.cpp similarity index 64% rename from src/liboncilla/nodes/OncillaL0.cpp rename to src/liboncilla/nodes/L0.cpp index 154f848..5a4924a 100644 --- a/src/liboncilla/nodes/OncillaL0.cpp +++ b/src/liboncilla/nodes/L0.cpp @@ -1,54 +1,54 @@ -#include "OncillaL0.h" +#include "L0.h" using namespace std; namespace rci { namespace oncilla { -OncillaL0::OncillaL0(const std::string &name) +L0::L0(const std::string &name) : ResourceNode(name) , Controlled() , PositionControlled() { // \todo add default value as a setting _controlMode = modeJointPositionControl; _lastCommandedPosition = JointAngles::create(1,0.0); } -bool OncillaL0::isConverged() const { +bool L0::isConverged() const { return false; } -void OncillaL0::unsafeSetCommand(double value){ +void L0::unsafeSetCommand(double value){ this->_lastCommandedPosition->setValue(value); } -double OncillaL0::unsafeGetCommand() const{ +double L0::unsafeGetCommand() const{ return this->_lastCommandedPosition->asDouble(0,false); } -bool OncillaL0::setJointPosition(JointAnglesPtr position) { +bool L0::setJointPosition(JointAnglesPtr position) { // Successfull this->_lastCommandedPosition = position; return true; } -JointAnglesPtr OncillaL0::getLastPositionCommand() const { +JointAnglesPtr L0::getLastPositionCommand() const { if (this->_lastCommandedPosition) { return this->_lastCommandedPosition; } throw runtime_error("No position command received yet."); } -OncillaL0::~OncillaL0() { +L0::~L0() { } -std::string OncillaL0::print() const { +std::string L0::print() const { ostringstream outstream(ostringstream::out); outstream.precision(3); // Precision when printing double values - outstream << "" << endl; + outstream << "" << endl; return outstream.str(); } } } diff --git a/src/liboncilla/nodes/OncillaL0.h b/src/liboncilla/nodes/L0.h similarity index 81% rename from src/liboncilla/nodes/OncillaL0.h rename to src/liboncilla/nodes/L0.h index fce8ae1..11e6925 100644 --- a/src/liboncilla/nodes/OncillaL0.h +++ b/src/liboncilla/nodes/L0.h @@ -1,68 +1,66 @@ #pragma once #include #include #include #include #include #include namespace rci { namespace oncilla { -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 OncillaL0: public rci::ResourceNode, - public rci::Controlled, - public rci::PositionControlled { +class L0: public rci::ResourceNode, + public rci::Controlled, + public rci::PositionControlled { public: - + typedef boost::shared_ptr Ptr; /** * Special constructor to also link to webots */ - OncillaL0(const std::string &name = "Oncilla Hip"); + L0(const std::string &name = "Oncilla Hip"); - virtual ~OncillaL0(); + virtual ~L0(); /** * 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 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; }; } } diff --git a/src/liboncilla/nodes/OncillaL1L2.cpp b/src/liboncilla/nodes/L1L2.cpp similarity index 68% rename from src/liboncilla/nodes/OncillaL1L2.cpp rename to src/liboncilla/nodes/L1L2.cpp index 2a12387..c5ea94b 100644 --- a/src/liboncilla/nodes/OncillaL1L2.cpp +++ b/src/liboncilla/nodes/L1L2.cpp @@ -1,108 +1,108 @@ -#include "OncillaL1L2.h" +#include "L1L2.h" #include using namespace std; namespace rci{ namespace oncilla{ -OncillaL1L2::OncillaL1L2(OncillaSynchronizer &s, const std::string &name) +L1L2::L1L2(OncillaSynchronizer &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 OncillaL1L2::isConverged() const { +bool L1L2::isConverged() const { throw NotImplementedError("OncillaL1L2","isConverged"); } -bool OncillaL1L2::setJointPosition(JointAnglesPtr position) { +bool L1L2::setJointPosition(JointAnglesPtr position) { // Successfull this->_lastCommandedPosition = position; return true; } -JointAnglesPtr OncillaL1L2::getJointPosition() const { +JointAnglesPtr L1L2::getJointPosition() const { return this->_latestJointPosition; } -bool OncillaL1L2::setJointVelocity(JointVelocitiesPtr ) { +bool L1L2::setJointVelocity(JointVelocitiesPtr ) { throw NotImplementedError("OncillaL1L2","SetJointVelocity"); } -bool OncillaL1L2::setJointTorque(JointTorquesPtr ) { +bool L1L2::setJointTorque(JointTorquesPtr ) { throw NotImplementedError("OncillaL1L2","SetJointTorque"); } -bool OncillaL1L2::setJointImpedance(JointImpedancePtr ) { +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 OncillaL1L2::getLastPositionCommand() const { +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."); } -OncillaL1L2::~OncillaL1L2() { +L1L2::~L1L2() { } -std::string OncillaL1L2::print() const { +std::string L1L2::print() const { ostringstream outstream(ostringstream::out); outstream.precision(3); // Precision when printing double values outstream << "" << endl; return outstream.str(); } -void OncillaL1L2::unsafeSetCommand(double value){ +void L1L2::unsafeSetCommand(double value){ this->_lastCommandedPosition->setValue(0,value); } -double OncillaL1L2::unsafeGetCommand() const{ +double L1L2::unsafeGetCommand() const{ return this->_lastCommandedPosition->asDouble(0); } -double OncillaL1L2::unsafeGetMotorAxisPosition() const{ +double L1L2::unsafeGetMotorAxisPosition() const{ return this->_latestJointPosition->asDouble(MotorAxisIndex); } -double OncillaL1L2::unsafeGetMagneticEncoderPosition() const{ +double L1L2::unsafeGetMagneticEncoderPosition() const{ return this->_latestJointPosition->asDouble(MagneticEncoderIndex); } -void OncillaL1L2::unsafeUpdateMotorAxisPosition(double value){ +void L1L2::unsafeUpdateMotorAxisPosition(double value){ this->_latestJointPosition->setValue(MotorAxisIndex,value); } -void OncillaL1L2::unsafeUpdateMagneticEncoderPosition(double value){ +void L1L2::unsafeUpdateMagneticEncoderPosition(double value){ this->_latestJointPosition->setValue(MagneticEncoderIndex,value); } } } diff --git a/src/liboncilla/nodes/OncillaL1L2.h b/src/liboncilla/nodes/L1L2.h similarity index 92% rename from src/liboncilla/nodes/OncillaL1L2.h rename to src/liboncilla/nodes/L1L2.h index c35c6bb..46c6365 100644 --- a/src/liboncilla/nodes/OncillaL1L2.h +++ b/src/liboncilla/nodes/L1L2.h @@ -1,125 +1,129 @@ #pragma once #include #include #include #include #include namespace rci{ namespace oncilla{ class OncillaSynchronizer; -class OncillaL1L2; -typedef boost::shared_ptr OncillaL1Ptr; -typedef boost::shared_ptr OncillaL2Ptr; +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 OncillaL1L2: public rci::ResourceNode, +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 */ - OncillaL1L2(OncillaSynchronizer & s, + L1L2(OncillaSynchronizer & s, const std::string & name = "Oncilla Hip"); - virtual ~OncillaL1L2(); + 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; }; + +typedef L1L2 L1; + +typedef L1L2 L2; + } } diff --git a/src/liboncilla/nodes/OncillaL3.cpp b/src/liboncilla/nodes/L3.cpp similarity index 68% rename from src/liboncilla/nodes/OncillaL3.cpp rename to src/liboncilla/nodes/L3.cpp index ff25975..4958fe2 100644 --- a/src/liboncilla/nodes/OncillaL3.cpp +++ b/src/liboncilla/nodes/L3.cpp @@ -1,43 +1,43 @@ -#include "OncillaL3.h" +#include "L3.h" #include using namespace std; namespace rci { namespace oncilla { -OncillaL3::OncillaL3(const std::string & name) +L3::L3(const std::string & name) : rci::ResourceNode(name) , rci::Sensing() , rci::PositionSensing() { this->_latestJointPosition = JointAngles::create(1,0.0); } -JointAnglesPtr OncillaL3::getJointPosition() const { +JointAnglesPtr L3::getJointPosition() const { throw NotImplementedError("OncillaL3","getJointPosition"); } -OncillaL3::~OncillaL3() { +L3::~L3() { } -std::string OncillaL3::print() const { +std::string L3::print() const { ostringstream outstream(ostringstream::out); outstream.precision(3); // Precision when printing double values outstream << "" << endl; return outstream.str(); } -double OncillaL3::unsafeGetPosition() const{ +double L3::unsafeGetPosition() const{ return this->_latestJointPosition->asDouble(0); } -void OncillaL3::unsafeUpdatePosition(double value){ +void L3::unsafeUpdatePosition(double value){ this->_latestJointPosition->setValue(0,value); } } } diff --git a/src/liboncilla/nodes/OncillaL3.h b/src/liboncilla/nodes/L3.h similarity index 78% rename from src/liboncilla/nodes/OncillaL3.h rename to src/liboncilla/nodes/L3.h index 9c4aa1d..f0d4d2e 100644 --- a/src/liboncilla/nodes/OncillaL3.h +++ b/src/liboncilla/nodes/L3.h @@ -1,49 +1,49 @@ #pragma once #include #include #include #include #include #include #include namespace rci { namespace oncilla { -class OncillaL3; -typedef boost::shared_ptr OncillaL3Ptr; +class L3; + /** * Node class, representing the foot/ankle of the quadruped robot. Not actuated, * but force sensing. */ -class OncillaL3: public rci::ResourceNode, +class L3: public rci::ResourceNode, public rci::Sensing, public rci::PositionSensing { public: + typedef boost::shared_ptr Ptr; + L3(const std::string & name = "Oncilla Ankle"); - OncillaL3(const std::string & name = "Oncilla Ankle"); - - virtual ~OncillaL3(); + virtual ~L3(); /** * Returns contact forces */ JointAnglesPtr getJointPosition() const; double unsafeGetPosition() const; void unsafeUpdatePosition(double value); /** * Print */ std::string print() const; }; } } diff --git a/src/liboncilla/nodes/OncillaL4.cpp b/src/liboncilla/nodes/L4.cpp similarity index 66% rename from src/liboncilla/nodes/OncillaL4.cpp rename to src/liboncilla/nodes/L4.cpp index da786a0..3e814ca 100644 --- a/src/liboncilla/nodes/OncillaL4.cpp +++ b/src/liboncilla/nodes/L4.cpp @@ -1,34 +1,34 @@ -#include "OncillaL4.h" +#include "L4.h" using namespace std; namespace rci { namespace oncilla { -OncillaL4::OncillaL4(const std::string & name) +L4::L4(const std::string & name) : rci::ResourceNode(name) , rci::Sensing() , rci::ForceSensing(){ this->forces = (ForcesPtr(new Forces(0,0,0))); } -ForcesPtr OncillaL4::getForces() const { +ForcesPtr L4::getForces() const { return this->forces; } -OncillaL4::~OncillaL4() { +L4::~L4() { } -std::string OncillaL4::print() const { +std::string L4::print() const { ostringstream outstream(ostringstream::out); outstream.precision(3); // Precision when printing double values outstream << "" << endl; return outstream.str(); } -void OncillaL4::unsafeUpdateForce(const double * data){ +void L4::unsafeUpdateForce(const double * data){ this->updateForces(ForcesPtr(new Forces(data[0],data[1],data[2]))); } } } diff --git a/src/liboncilla/nodes/OncillaL4.h b/src/liboncilla/nodes/L4.h similarity index 77% rename from src/liboncilla/nodes/OncillaL4.h rename to src/liboncilla/nodes/L4.h index 964e890..fd9d915 100644 --- a/src/liboncilla/nodes/OncillaL4.h +++ b/src/liboncilla/nodes/L4.h @@ -1,49 +1,49 @@ #pragma once #include #include #include #include #include #include #include namespace rci { namespace oncilla { -class OncillaL4; -typedef boost::shared_ptr OncillaL4Ptr; +class L4; /** * Node class, representing the foot of the quadruped robot. Not actuated, * but force sensing. */ -class OncillaL4: public rci::ResourceNode, +class L4: public rci::ResourceNode, public rci::Sensing, public rci::ForceSensing, public rci::TorqueSensing{ public: + typedef boost::shared_ptr Ptr; - OncillaL4(const std::string & name = "Oncilla Foot"); + L4(const std::string & name = "Oncilla Foot"); - virtual ~OncillaL4(); + virtual ~L4(); /** * Returns contact forces */ ForcesPtr getForces() const; /** * Print */ std::string print() const; void unsafeUpdateForce(const double * data); }; } } diff --git a/src/liboncilla/nodes/OncillaNodes.h b/src/liboncilla/nodes/Nodes.h similarity index 61% rename from src/liboncilla/nodes/OncillaNodes.h rename to src/liboncilla/nodes/Nodes.h index 92f8547..e685a28 100644 --- a/src/liboncilla/nodes/OncillaNodes.h +++ b/src/liboncilla/nodes/Nodes.h @@ -1,18 +1,18 @@ /* * OncillaNodes.h * * Created on: Jan 21, 2013 * Author: tuleu */ #ifndef LIBONCILLA_ONCILLANODES_H_ #define LIBONCILLA_ONCILLANODES_H_ -#include "OncillaL0.h" -#include "OncillaL1L2.h" -#include "OncillaL3.h" -#include "OncillaL4.h" -#include "OncillaTrunk.h" +#include "L0.h" +#include "L1L2.h" +#include "L3.h" +#include "L4.h" +#include "Trunk.h" #endif /* LIBONCILLA_ONCILLANODES_H_ */ diff --git a/src/liboncilla/nodes/OncillaTrunk.cpp b/src/liboncilla/nodes/Trunk.cpp similarity index 62% rename from src/liboncilla/nodes/OncillaTrunk.cpp rename to src/liboncilla/nodes/Trunk.cpp index 15bfb41..4fe6cdd 100644 --- a/src/liboncilla/nodes/OncillaTrunk.cpp +++ b/src/liboncilla/nodes/Trunk.cpp @@ -1,37 +1,37 @@ -#include "OncillaTrunk.h" +#include "Trunk.h" #include "liboncilla/exceptions/NotImplementedError.h" using namespace std; namespace rci { namespace oncilla { -OncillaTrunk::OncillaTrunk(const std::string & name) +Trunk::Trunk(const std::string & name) : ResourceNode(name) , Sensing() { } -OncillaTrunk::~OncillaTrunk() { +Trunk::~Trunk() { } -TranslationalAccelerationPtr OncillaTrunk::getTranslationalAcceleration() const{ +TranslationalAccelerationPtr Trunk::getTranslationalAcceleration() const{ throw NotImplementedError("OncillaTrunk","getTranslationalAcceleration"); } -OrientationPtr OncillaTrunk::getOrientation() const{ +OrientationPtr Trunk::getOrientation() const{ throw NotImplementedError("OncillaTrunk","Orientation"); } -std::string OncillaTrunk::print() const { +std::string Trunk::print() const { ostringstream outstream(ostringstream::out); outstream.precision(3); // Precision when printing double values outstream << "" << endl; return outstream.str(); } } } diff --git a/src/liboncilla/nodes/OncillaTrunk.h b/src/liboncilla/nodes/Trunk.h similarity index 74% rename from src/liboncilla/nodes/OncillaTrunk.h rename to src/liboncilla/nodes/Trunk.h index 9a54fa8..945aa9a 100644 --- a/src/liboncilla/nodes/OncillaTrunk.h +++ b/src/liboncilla/nodes/Trunk.h @@ -1,45 +1,44 @@ #pragma once #include #include #include #include #include #include namespace rci { namespace oncilla { -class OncillaTrunk; -typedef boost::shared_ptr OncillaTrunkPtr; +class Trunk; /** * Node class, representing the quadruped trunk, including pose sensing. * * @todo Expose translational accelerations and rotational velocities */ -class OncillaTrunk: public rci::ResourceNode, public Sensing, +class Trunk: public rci::ResourceNode, public Sensing, public TranslationalAccelerationSensing, public OrientationSensing { public: - - OncillaTrunk(const std::string & name = "Oncilla Trunk"); - virtual ~OncillaTrunk(); + typedef boost::shared_ptr Ptr; + Trunk(const std::string & name = "Oncilla Trunk"); + virtual ~Trunk(); virtual TranslationalAccelerationPtr getTranslationalAcceleration() const; virtual OrientationPtr getOrientation() const; /** * Print */ std::string print() const; }; } } diff --git a/test/nodes/OncillaL0Test.cpp b/test/nodes/OncillaL0Test.cpp index 0b2b30e..19dc629 100644 --- a/test/nodes/OncillaL0Test.cpp +++ b/test/nodes/OncillaL0Test.cpp @@ -1,49 +1,49 @@ #include -#include +#include using namespace nemo; using namespace rci; using namespace rci::oncilla; class OncillaL0Test: public ::testing::Test { protected: OncillaL0Test() : joint(), ja_values(), ji_values(), ja(), ji() { - joint = OncillaL0Ptr(new OncillaL0("FooBar")); + joint = L0::Ptr(new L0("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 ~OncillaL0Test() { } - OncillaL0Ptr joint; + L0::Ptr joint; RealVector ja_values, ji_values; JointAnglesPtr ja; JointImpedancePtr ji; }; TEST_F(OncillaL0Test, testInitialialState) { EXPECT_NO_THROW(joint->getLastPositionCommand()); EXPECT_EQ(0.0,joint->getLastPositionCommand()->rad()); } TEST_F(OncillaL0Test, testPositionCommand) { EXPECT_NO_THROW(joint->getLastPositionCommand()); EXPECT_EQ(0.0,joint->getLastPositionCommand()->rad()); joint->setJointPosition(ja); EXPECT_EQ(1.234, joint->getLastPositionCommand()->rad()); } diff --git a/test/nodes/OncillaL1L2Test.cpp b/test/nodes/OncillaL1L2Test.cpp index f66f3e6..981a72e 100644 --- a/test/nodes/OncillaL1L2Test.cpp +++ b/test/nodes/OncillaL1L2Test.cpp @@ -1,85 +1,85 @@ #include -#include +#include #include "liboncilla/OncillaSynchronizer.h" using namespace nemo; using namespace rci; using namespace rci::oncilla; class MockSynchronizer: public OncillaSynchronizer { public: MockSynchronizer() : OncillaSynchronizer("Sync") { } virtual void processAsync() { } virtual void waitForProcessAsync() { } - virtual void registerTrunkNode(rci::oncilla::OncillaTrunkPtr) { + virtual void registerTrunkNode(rci::oncilla::Trunk::Ptr) { } virtual void registerL0Node(rci::oncilla::Leg, - rci::oncilla::OncillaL0Ptr) { + rci::oncilla::L0::Ptr) { } virtual void registerL1Node(rci::oncilla::Leg, - rci::oncilla::OncillaL1Ptr) { + rci::oncilla::L1::Ptr) { } virtual void registerL2Node(rci::oncilla::Leg, - rci::oncilla::OncillaL2Ptr) { + rci::oncilla::L2::Ptr) { } virtual void registerL3Node(rci::oncilla::Leg, - rci::oncilla::OncillaL3Ptr) { + rci::oncilla::L3::Ptr) { } virtual void registerL4Node(rci::oncilla::Leg, - rci::oncilla::OncillaL4Ptr) { + rci::oncilla::L4::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 = OncillaL1Ptr(new OncillaL1L2(synchr, "FooBar")); + 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() { } - OncillaL1Ptr joint; + 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()); }