diff --git a/src/liboncilla/nodes/L0.cpp b/src/liboncilla/nodes/L0.cpp index 5a4924a..8e98a35 100644 --- a/src/liboncilla/nodes/L0.cpp +++ b/src/liboncilla/nodes/L0.cpp @@ -1,54 +1,63 @@ #include "L0.h" +#include + using namespace std; namespace rci { namespace oncilla { L0::L0(const std::string &name) : ResourceNode(name) , Controlled() - , PositionControlled() { + , 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; } 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("L0","getForces"); +} + } } diff --git a/src/liboncilla/nodes/L0.h b/src/liboncilla/nodes/L0.h index 11e6925..d4ebd48 100644 --- a/src/liboncilla/nodes/L0.h +++ b/src/liboncilla/nodes/L0.h @@ -1,66 +1,74 @@ #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::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"); 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; + + ForcesPtr getForces() const; + + void unsafeUpdateForce(double x, double y,double torque); + + /** * 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; }; } }