diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index e68e10a..a387aab 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,41 +1,35 @@ INCLUDE_DIRECTORIES(BEFORE SYSTEM ${CMAKE_CURRENT_BINARY_DIR}) INCLUDE_DIRECTORIES(BEFORE SYSTEM ${CMAKE_CURRENT_SOURCE_DIR}) INCLUDE_DIRECTORIES(BEFORE ${CMAKE_CURRENT_SOURCE_DIR}/..) # Configure config header CONFIGURE_FILE("config.h.in" "${PROJECTNAME}Config.h") SET(ONCILLAHEADERS OncillaTrunk.h OncillaHip.h OncillaKnee.h OncillaAnkle.h OncillaFoot.h) SET(ONCILLASOURCES OncillaTrunk.cpp OncillaHip.cpp OncillaKnee.cpp OncillaAnkle.cpp OncillaFoot.cpp) -ADD_LIBRARY(${RCIONCILLA_NAME} SHARED ${ONCILLASOURCES}) -TARGET_LINK_LIBRARIES(${RCIONCILLA_NAME} +ADD_LIBRARY(${LIBONCILLA_NAME} SHARED ${ONCILLASOURCES}) +TARGET_LINK_LIBRARIES(${LIBONCILLA_NAME} ${RSC_LIBRARIES} - ${RSB_LIBRARIES} - ${RST_LIBRARIES} - ${SPREAD_LIBRARIES} - ${CCA_LIBRARIES} ${RCI_LIBRARIES} - ${PROTOBUF_LIBRARY} - ${PROTOLIB} ${LIBRARIES_LIBRARIES}) -SET_TARGET_PROPERTIES(${RCIONCILLA_NAME} +SET_TARGET_PROPERTIES(${LIBONCILLA_NAME} PROPERTIES - VERSION ${RCIONCILLA_VERSION}) + VERSION ${LIBONCILLA_VERSION}) -INSTALL(TARGETS ${RCIONCILLA_NAME} +INSTALL(TARGETS ${LIBONCILLA_NAME} RUNTIME DESTINATION bin LIBRARY DESTINATION lib ARCHIVE DESTINATION lib) INSTALL_FILES_RECURSIVE(include/${LIBNAME} ONCILLAHEADERS) diff --git a/src/OncillaAnkle.cpp b/src/OncillaAnkle.cpp index eb8fae5..1faf1e1 100644 --- a/src/OncillaAnkle.cpp +++ b/src/OncillaAnkle.cpp @@ -1,40 +1,30 @@ #include "OncillaAnkle.h" using namespace std; namespace cca { namespace rci { namespace driver { OncillaAnkle::OncillaAnkle(std::string name) : rci::ResourceNode(name), rci::Sensing(), rci::PositionSensing() { this->_dimension = 1; - - this->addOutputPort (); } JointAnglesPtr OncillaAnkle::getJointPosition() const { throw std::runtime_error("Not yet implemented."); } OncillaAnkle::~OncillaAnkle() { } -void OncillaAnkle::onProcess() { -} - std::string OncillaAnkle::print() const { ostringstream outstream(ostringstream::out); outstream.precision(3); // Precision when printing double values outstream << "" << endl; return outstream.str(); } -CCANodePtr OncillaAnkle::create(string name) { - CCANodePtr node(new OncillaAnkle(name)); - return node; -} - } } } diff --git a/src/OncillaAnkle.h b/src/OncillaAnkle.h index 29e000e..1c76cf1 100644 --- a/src/OncillaAnkle.h +++ b/src/OncillaAnkle.h @@ -1,62 +1,47 @@ #pragma once #include #include -#include - #include #include #include #include #include namespace cca { namespace rci { namespace driver { class OncillaAnkle; typedef boost::shared_ptr OncillaAnklePtr; /** * Node class, representing the foot/ankle of the quadruped robot. Not actuated, * but force sensing. */ class OncillaAnkle: public rci::ResourceNode, public rci::Sensing, public rci::PositionSensing { public: OncillaAnkle(std::string name = "Oncilla Ankle"); virtual ~OncillaAnkle(); /** * Returns contact forces */ JointAnglesPtr getJointPosition() const; - /** - * Main data-flow process method - * Main data-flow process method. Every time called, when a new data-flow - * item arrives. - * @todo marshalling and calling control methods - */ - void onProcess(); - /** * Print */ std::string print() const; - - /** - * Shared Pointer Factory - */ - static CCANodePtr create(std::string name = "Oncilla Ankle"); }; } } } diff --git a/src/OncillaFoot.cpp b/src/OncillaFoot.cpp index 2198a42..14de5e2 100644 --- a/src/OncillaFoot.cpp +++ b/src/OncillaFoot.cpp @@ -1,40 +1,30 @@ #include "OncillaFoot.h" using namespace std; namespace cca { namespace rci { namespace driver { OncillaFoot::OncillaFoot(std::string name) : rci::ResourceNode(name), rci::Sensing(), rci::ForceSensing() { this->_dimension = 1; - - this->addOutputPort (); } ForcesPtr OncillaFoot::getForces() const { throw std::runtime_error("Not yet implemented."); } OncillaFoot::~OncillaFoot() { } -void OncillaFoot::onProcess() { -} - std::string OncillaFoot::print() const { ostringstream outstream(ostringstream::out); outstream.precision(3); // Precision when printing double values outstream << "" << endl; return outstream.str(); } -CCANodePtr OncillaFoot::create(string name) { - CCANodePtr node(new OncillaFoot(name)); - return node; -} - } } } diff --git a/src/OncillaFoot.h b/src/OncillaFoot.h index 4c84b5a..41a2c11 100644 --- a/src/OncillaFoot.h +++ b/src/OncillaFoot.h @@ -1,62 +1,47 @@ #pragma once #include #include -#include - #include #include #include #include #include namespace cca { namespace rci { namespace driver { class OncillaFoot; typedef boost::shared_ptr OncillaFootPtr; /** * Node class, representing the foot of the quadruped robot. Not actuated, * but force sensing. */ class OncillaFoot: public rci::ResourceNode, public rci::Sensing, public rci::ForceSensing { public: OncillaFoot(std::string name = "Oncilla Foot"); virtual ~OncillaFoot(); /** * Returns contact forces */ ForcesPtr getForces() const; - /** - * Main data-flow process method - * Main data-flow process method. Every time called, when a new data-flow - * item arrives. - * @todo marshalling and calling control methods - */ - void onProcess(); - /** * Print */ std::string print() const; - - /** - * Shared Pointer Factory - */ - static CCANodePtr create(std::string name = "Oncilla Foot"); }; } } } diff --git a/src/OncillaHip.cpp b/src/OncillaHip.cpp index aae9c6c..8a09942 100644 --- a/src/OncillaHip.cpp +++ b/src/OncillaHip.cpp @@ -1,145 +1,97 @@ #include "OncillaHip.h" using namespace std; -using namespace rsb; namespace cca { namespace rci { namespace driver { OncillaHipSagittal::OncillaHipSagittal(std::string name) : ResourceNode(name), Controlled(), Sensing(), PositionControlled(), PositionSensing(), TorqueControlled(), ImpedanceControlled() { this->_dimension = 2; // Two encoder values - should always be the same - - this->addInputPort (); // PositionControlled - this->addInputPort (); // VelocityControlled - this->addInputPort (); // TorqueControlled - this->addInputPort (); // ImpedanceControlled - - this->addOutputPort (); // PositionSensing } bool OncillaHipSagittal::isConverged() const { throw std::runtime_error("Not yet implemented."); } bool OncillaHipSagittal::setJointPosition(JointAnglesPtr position) { // Successfull this->_lastCommandedPosition = position; return true; } JointAnglesPtr OncillaHipSagittal::getJointPosition() const { return this->_latestJointPosition; } bool OncillaHipSagittal::setJointVelocity(JointVelocitiesPtr vel) { throw std::runtime_error("Not yet implemented."); } bool OncillaHipSagittal::setJointTorque(JointTorquesPtr torque) { throw std::runtime_error("Not yet implemented."); } bool OncillaHipSagittal::setJointImpedance(JointImpedancePtr imped) { throw std::runtime_error("Not yet implemented."); } JointAnglesPtr OncillaHipSagittal::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; } OncillaHipSagittal::~OncillaHipSagittal() { } -void OncillaHipSagittal::onProcess() { - // std::cout << "[Node " << this->getName() << "] Processing ..." << std::endl; - if (!this->inputs[0]->empty() && this->inputs[0]->newItem()) { - // Cache new value - this->setJointPosition( - boost::static_pointer_cast( - this->inputs[0]->tryGet())); - } - - // Publish value - if ((this->_latestJointPosition != NULL) - && (this->_latestJointPosition.get() != NULL)) { - Informer::DataPtr status( - new JointAngles(this->_latestJointPosition->asDouble(0))); - this->publish(0, status); - } -} - std::string OncillaHipSagittal::print() const { ostringstream outstream(ostringstream::out); outstream.precision(3); // Precision when printing double values outstream << "" << endl; return outstream.str(); } -CCANodePtr OncillaHipSagittal::create(string name) { - CCANodePtr hipnode(new OncillaHipSagittal(name)); - return hipnode; -} - /** Oncilla Hip Transverse **/ OncillaHipTransverse::OncillaHipTransverse(std::string name) : ResourceNode(name), Controlled(), PositionControlled() { - - this->addInputPort (); // PositionControlled } bool OncillaHipTransverse::isConverged() const { return false; } bool OncillaHipTransverse::setJointPosition(JointAnglesPtr position) { // Successfull this->_lastCommandedPosition = position; return true; } JointAnglesPtr OncillaHipTransverse::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; } } OncillaHipTransverse::~OncillaHipTransverse() { } -void OncillaHipTransverse::onProcess() { - // std::cout << "[Node " << this->getName() << "] Processing ..." << std::endl; - if (!this->inputs[0]->empty() && this->inputs[0]->newItem()) { - // Cache new value - this->setJointPosition( - boost::static_pointer_cast( - this->inputs[0]->tryGet())); - } -} - std::string OncillaHipTransverse::print() const { ostringstream outstream(ostringstream::out); outstream.precision(3); // Precision when printing double values outstream << "" << endl; return outstream.str(); } -CCANodePtr OncillaHipTransverse::create(string name) { - CCANodePtr hipnode(new OncillaHipTransverse(name)); - return hipnode; -} - } } } diff --git a/src/OncillaHip.h b/src/OncillaHip.h index 456b5cc..7e4ed73 100644 --- a/src/OncillaHip.h +++ b/src/OncillaHip.h @@ -1,174 +1,146 @@ #pragma once #include #include -#include - #include #include #include #include #include namespace cca { namespace rci { namespace driver { class OncillaHipSagittal; typedef boost::shared_ptr OncillaHipSagittalPtr; /** * Node class, representing the hip node of the quadruped robot. * * @todo In case of simulation, this node can also sense the power consumption. */ class OncillaHipSagittal: 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: /** * Special constructor to also link to webots */ OncillaHipSagittal(std::string name = "Oncilla Hip"); virtual ~OncillaHipSagittal(); /** * 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(cca::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; - /** - * Main data-flow process method - * Main data-flow process method. Every time called, when a new data-flow - * item arrives. - * @todo marshalling and calling control methods - */ - void onProcess(); - /** * Print */ std::string print() const; - - /** - * Shared Pointer Factory - */ - static CCANodePtr create(std::string name = "Oncilla Hip"); }; class OncillaHipTransverse; typedef boost::shared_ptr OncillaHipTwoPtr; /** * Node class, representing the hip node of the quadruped robot. * @todo In case of simulation, this node can also sense the power consumption. */ class OncillaHipTransverse: public rci::ResourceNode, public rci::Controlled, public rci::PositionControlled { public: /** * Special constructor to also link to webots */ OncillaHipTransverse(std::string name = "Oncilla Hip"); virtual ~OncillaHipTransverse(); /** * 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 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; - /** - * Main data-flow process method - * Main data-flow process method. Every time called, when a new data-flow - * item arrives. - * @todo marshalling and calling control methods - */ - void onProcess(); - /** * Print */ std::string print() const; - - /** - * Shared Pointer Factory - */ - static CCANodePtr create(std::string name = "Oncilla Hip"); }; } } } diff --git a/src/OncillaKnee.cpp b/src/OncillaKnee.cpp index 0f1a364..5e6ed9f 100644 --- a/src/OncillaKnee.cpp +++ b/src/OncillaKnee.cpp @@ -1,98 +1,68 @@ #include "OncillaKnee.h" using namespace std; using namespace boost; -using namespace rsb; namespace cca { namespace rci { namespace driver { OncillaKnee::OncillaKnee(std::string name) : ResourceNode(name), Controlled(), Sensing(), PositionControlled(), PositionSensing(), VelocityControlled(), TorqueControlled(), ImpedanceControlled() { // Two encoder values - difference is due to passive compliance this->_dimension = 2; - - this->addInputPort (); // PositionControlled - this->addInputPort (); // VelocityControlled - this->addInputPort (); // TorqueControlled - this->addInputPort (); // ImpedanceControlled - - this->addOutputPort (); // PositionSensing -} - -void OncillaKnee::onProcess() { - if (!this->inputs[0]->empty() && this->inputs[0]->newItem()) { - // Cache new value - this->setJointPosition( - boost::static_pointer_cast( - this->inputs[0]->tryGet())); - } - - // Publish value - if ((this->_latestJointPosition != NULL) - && (this->_latestJointPosition.get() != NULL)) { - Informer::DataPtr status( - new JointAngles(this->_latestJointPosition->asDouble(0))); - this->publish(0, status); - } } bool OncillaKnee::isConverged() const { return false; } bool OncillaKnee::setJointPosition(JointAnglesPtr position) { // Successfull // std::cerr << "[OncillaKnee] " << position << std::endl; this->_lastCommandedPosition = position; return true; } JointAnglesPtr OncillaKnee::getJointPosition() const { return this->_latestJointPosition; } OncillaKnee::~OncillaKnee() { } std::string OncillaKnee::print() const { ostringstream outstream(ostringstream::out); outstream.precision(3); // Precision when printing double values outstream << "" << endl; return outstream.str(); } -CCANodePtr OncillaKnee::create(string name) { - CCANodePtr kneenode(new OncillaKnee(name)); - return kneenode; -} - bool OncillaKnee::setJointTorque(JointTorquesPtr torques) { throw std::runtime_error("Not yet implemented."); } bool OncillaKnee::setJointImpedance(JointImpedancePtr imped) { throw std::runtime_error("Not yet implemented."); } bool OncillaKnee::setJointVelocity(JointVelocitiesPtr vel) { throw std::runtime_error("Not yet implemented."); } JointAnglesPtr OncillaKnee::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/OncillaKnee.h b/src/OncillaKnee.h index c2aec39..136b1b8 100644 --- a/src/OncillaKnee.h +++ b/src/OncillaKnee.h @@ -1,114 +1,97 @@ #pragma once #include #include -#include - -#include - #include #include #include #include #include namespace cca { namespace rci { namespace driver { class OncillaKnee; typedef boost::shared_ptr OncillaKneePtr; /** * Node class, representing the knee node of the quadruped robot. * @todo In case of simulation, this node can also sense the power consumption. */ class OncillaKnee: 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: OncillaKnee(std::string name = "Oncilla Knee"); virtual ~OncillaKnee(); - /** - * Handle commands and send status - * - * Main processing. handles incoming commands and published current node - * status - */ - void onProcess(); - /** * 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(cca::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; - - /** - * Shared Pointer Factory - */ - static CCANodePtr create(std::string name = "Oncilla Knee"); }; } } } diff --git a/src/OncillaTrunk.cpp b/src/OncillaTrunk.cpp index 2b9cfee..186c2e6 100644 --- a/src/OncillaTrunk.cpp +++ b/src/OncillaTrunk.cpp @@ -1,50 +1,38 @@ #include "OncillaTrunk.h" using namespace std; namespace cca { namespace rci { namespace driver { OncillaTrunk::OncillaTrunk(std::string name) : ResourceNode(name), Sensing(), PoseSensing() { this->_dimension = 1; - - // TODO: Don`t send pose, but acceleration - this->addOutputPort (); // PoseSensing } PosePtr OncillaTrunk::getAbsolutePose() const { throw std::runtime_error("Not yet implemented."); } PosePtr OncillaTrunk::getDisplacement() const { throw std::runtime_error("Not yet implemented."); } PosePtr OncillaTrunk::getAcceleration() const { throw std::runtime_error("Not yet implemented."); } OncillaTrunk::~OncillaTrunk() { } -void OncillaTrunk::onProcess() { - throw std::runtime_error("Not yet implemented."); -} - std::string OncillaTrunk::print() const { ostringstream outstream(ostringstream::out); outstream.precision(3); // Precision when printing double values outstream << "" << endl; return outstream.str(); } -CCANodePtr OncillaTrunk::create(string name) { - CCANodePtr node(new OncillaTrunk(name)); - return node; -} - } } } diff --git a/src/OncillaTrunk.h b/src/OncillaTrunk.h index f952205..13af210 100644 --- a/src/OncillaTrunk.h +++ b/src/OncillaTrunk.h @@ -1,69 +1,57 @@ #pragma once #include #include -#include - #include #include #include #include namespace cca { namespace rci { namespace driver { class OncillaTrunk; typedef boost::shared_ptr OncillaTrunkPtr; /** * Node class, representing the quadruped trunk, including pose sensing. */ class OncillaTrunk: public rci::ResourceNode, public rci::Sensing, public rci::PoseSensing { public: OncillaTrunk(std::string name = "Oncilla Trunk"); virtual ~OncillaTrunk(); /** * Returns current pose. * @return Current pose */ virtual PosePtr getAbsolutePose() const; /** * Returns current displacement. * @return Current pose */ virtual PosePtr getDisplacement() const; /** * Returns current translational and rotational acceleration. * @return Current pose */ virtual PosePtr getAcceleration() const; - /** - * Main processing method - */ - void onProcess(); - /** * Print */ std::string print() const; - /** - * Shared Pointer Factory - */ - static CCANodePtr create(std::string name = "Oncilla Trunk"); - }; } } }