diff --git a/src/liboncilla/nodes/L0.cpp b/src/liboncilla/nodes/L0.cpp index 2ca4207..d25484d 100644 --- a/src/liboncilla/nodes/L0.cpp +++ b/src/liboncilla/nodes/L0.cpp @@ -1,63 +1,52 @@ #include "L0.h" #include using namespace std; namespace rci { namespace oncilla { L0::L0(const std::string &name) : ResourceNode(name) , Controlled() , PositionControlled() , Sensing() , ForceSensing(){ // \todo add default value as a setting _controlMode = modeJointPositionControl; _lastCommandedPosition = JointAngles::create(1,0.0); } bool L0::isConverged() const { return false; } -void L0::unsafeSetCommand(double value){ - this->_lastCommandedPosition->setValue(value); -} + double L0::unsafeGetCommand() const{ return this->_lastCommandedPosition->asDouble(0,false); } -bool L0::setJointPosition(JointAnglesPtr position) { - // Successfull - this->_lastCommandedPosition = position; - return true; -} - -JointAnglesPtr L0::getLastPositionCommand() const { - if (this->_lastCommandedPosition) { - return this->_lastCommandedPosition; - } +void L0::unsafeUpdateForce(double x, double y,double torque){ + this->forces->setValue(0,x); + this->forces->setValue(1,y); + this->forces->setValue(2,torque); - throw runtime_error("No position command received yet."); } + L0::~L0() { } std::string L0::print() const { ostringstream outstream(ostringstream::out); outstream.precision(3); // Precision when printing double values outstream << "" << endl; return outstream.str(); } -ForcesPtr L0::getForces() const{ - throw NotImplementedError("liboncilla","getForces","L0"); -} } } diff --git a/src/liboncilla/nodes/L0.h b/src/liboncilla/nodes/L0.h index d4ebd48..baacc0e 100644 --- a/src/liboncilla/nodes/L0.h +++ b/src/liboncilla/nodes/L0.h @@ -1,74 +1,67 @@ #pragma once #include #include #include #include #include #include namespace rci { namespace oncilla { /** * Node class, representing the hip node of the quadruped robot. * @todo In case of simulation, this node can also sense the power consumption. */ class L0: public rci::ResourceNode, public rci::Controlled, public rci::PositionControlled, public rci::Sensing, public rci::ForceSensing{ public: typedef boost::shared_ptr Ptr; - /** - * Special constructor to also link to webots - */ - L0(const std::string &name = "Oncilla Hip"); + + L0(const std::string &name = "Oncilla Scapula"); virtual ~L0(); /** - * Returns, if controller is converged. - * @return True, if controller is converged after last command. + * Returns, if the controller is converged. + * \note This always return false for Oncilla : reason we use PWM + * controlled servos for this joint. + * \return always false, always. */ 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 + * Print */ - bool setJointPosition(JointAnglesPtr position); - - void unsafeSetCommand(double value); - - double unsafeGetCommand() const; - - - ForcesPtr getForces() const; - - void unsafeUpdateForce(double x, double y,double torque); + std::string print() const; +protected : /** - * Returns latest position command. Note, that this is the latest valid - * commanded position. If the position command exceeds limits and is - * therefore rejected, it .. + * Helper function to get command in derived classes. + * \internal */ - virtual JointAnglesPtr getLastPositionCommand() const; + double unsafeGetCommand() const; /** - * Print + * Helper function to update forces in derived classes. + * \internal */ - std::string print() const; + void unsafeUpdateForce(double x, double y,double torque); + }; + + } } diff --git a/src/liboncilla/nodes/L1L2.cpp b/src/liboncilla/nodes/L1L2.cpp index a99a49d..c825fbe 100644 --- a/src/liboncilla/nodes/L1L2.cpp +++ b/src/liboncilla/nodes/L1L2.cpp @@ -1,108 +1,67 @@ #include "L1L2.h" #include using namespace std; namespace rci{ namespace oncilla{ 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("liboncilla","isConverged","L1L2"); -} - -bool L1L2::setJointPosition(JointAnglesPtr position) { - // Successfull - this->_lastCommandedPosition = position; - return true; -} - -JointAnglesPtr L1L2::getJointPosition() const { - return this->_latestJointPosition; -} - -bool L1L2::setJointVelocity(JointVelocitiesPtr ) { - throw NotImplementedError("liboncilla","SetJointVelocity","L1L2"); -} - -bool L1L2::setJointTorque(JointTorquesPtr ) { - throw NotImplementedError("liboncilla","SetJointTorque","L1L2"); -} -bool L1L2::setJointImpedance(JointImpedancePtr ) { - throw NotImplementedError("liboncilla","setJointImpedence","L1L2"); - -// 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(); } +bool L1L2::isConverged() const{ + throw NotImplementedError("liboncilla","isConverged","L1L2"); +} + 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 6f7e352..0631796 100644 --- a/src/liboncilla/nodes/L1L2.h +++ b/src/liboncilla/nodes/L1L2.h @@ -1,129 +1,83 @@ #pragma once #include #include #include #include #include namespace rci{ namespace oncilla{ 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(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 + * Print */ - bool setJointPosition(JointAnglesPtr position); - + std::string print() const; +protected : void unsafeSetCommand(double value); - double unsafeGetCommand() const ; - - /** - * Returns current joint position. - * @return Current joint position - */ - virtual JointAnglesPtr getJointPosition() const; - + double unsafeGetCommand() 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. */ Synchronizer &synchronizer; }; typedef L1L2 L1; typedef L1L2 L2; } } diff --git a/src/liboncilla/nodes/L3.cpp b/src/liboncilla/nodes/L3.cpp index c81b8e8..9ae9902 100644 --- a/src/liboncilla/nodes/L3.cpp +++ b/src/liboncilla/nodes/L3.cpp @@ -1,43 +1,39 @@ #include "L3.h" #include using namespace std; namespace rci { namespace oncilla { L3::L3(const std::string & name) : rci::ResourceNode(name) , rci::Sensing() , rci::PositionSensing() { this->_latestJointPosition = JointAngles::create(1,0.0); } -JointAnglesPtr L3::getJointPosition() const { - throw NotImplementedError("liboncilla","getJointPosition","L3"); -} - L3::~L3() { } std::string L3::print() const { ostringstream outstream(ostringstream::out); outstream.precision(3); // Precision when printing double values outstream << "" << endl; return outstream.str(); } double L3::unsafeGetPosition() const{ return this->_latestJointPosition->asDouble(0); } void L3::unsafeUpdatePosition(double value){ this->_latestJointPosition->setValue(0,value); } } } diff --git a/src/liboncilla/nodes/L3.h b/src/liboncilla/nodes/L3.h index f0d4d2e..5e841b8 100644 --- a/src/liboncilla/nodes/L3.h +++ b/src/liboncilla/nodes/L3.h @@ -1,49 +1,46 @@ #pragma once #include #include #include #include #include #include #include namespace rci { namespace oncilla { class L3; /** * Node class, representing the foot/ankle of the quadruped robot. Not actuated, * but force sensing. */ class L3: public rci::ResourceNode, public rci::Sensing, public rci::PositionSensing { public: typedef boost::shared_ptr Ptr; L3(const std::string & name = "Oncilla Ankle"); virtual ~L3(); /** - * Returns contact forces + * Print */ - JointAnglesPtr getJointPosition() const; + std::string print() const; +protected : double unsafeGetPosition() const; void unsafeUpdatePosition(double value); - /** - * Print - */ - std::string print() const; }; } }