diff --git a/src/OncillaL01.cpp b/src/OncillaL01.cpp index 5fd2081..ec020db 100644 --- a/src/OncillaL01.cpp +++ b/src/OncillaL01.cpp @@ -1,101 +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(), 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) { - if (this->synchronizer.active()) { + if (this->synchronizer.tooBusy()) { 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/OncillaL2.cpp b/src/OncillaL2.cpp index a318962..880c970 100644 --- a/src/OncillaL2.cpp +++ b/src/OncillaL2.cpp @@ -1,71 +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(), 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) { - if (this->synchronizer.active()) { + if (this->synchronizer.tooBusy()) { 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/OncillaSynchronizer.cpp b/src/OncillaSynchronizer.cpp index 09e7a87..f50e5dd 100644 --- a/src/OncillaSynchronizer.cpp +++ b/src/OncillaSynchronizer.cpp @@ -1,16 +1,16 @@ #include "OncillaSynchronizer.h" using namespace std; namespace rci { namespace oncilla { OncillaSynchronizer::OncillaSynchronizer(const std::string &sname) : - Synchronizer(sname), _onlyReceive(true) { + Synchronizer(sname) { } OncillaSynchronizer::~OncillaSynchronizer() { } } } diff --git a/src/OncillaSynchronizer.h b/src/OncillaSynchronizer.h index 8b32f93..176588f 100644 --- a/src/OncillaSynchronizer.h +++ b/src/OncillaSynchronizer.h @@ -1,98 +1,87 @@ #pragma once #include #include #include #include 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 { 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(unsigned int index, OncillaTrunkPtr node) = 0; /** * Registers an L0 node. * @todo use Oncilla::Leg as index (problem with cyclic includes) */ 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(unsigned int index, OncillaL1Ptr node) = 0; /** * Registers an L2 node. * @todo use Oncilla::Leg as index (problem with cyclic includes) */ 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(unsigned int index, OncillaL3Ptr node) = 0; /** * Registers an L4 node. * @todo use Oncilla::Leg as index (problem with cyclic includes) */ 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; }; } }