OncillaL1L2.cpp
3e9f1f8e42bedev-arne

OncillaL1L2.cpp
View Options

			#include "OncillaL1L2.h"

			#include "OncillaSynchronizer.h"

			#include <liboncilla/exceptions/NotImplementedError.h>

			using namespace std;

			namespace rci{
			namespace oncilla{
			OncillaL1L2::OncillaL1L2(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 {
			throw NotImplementedError("OncillaL1L2","isConverged");
			}

			bool OncillaL1L2::setJointPosition(JointAnglesPtr position) {
			// Successfull
			this->_lastCommandedPosition = position;
			return true;
			}

			JointAnglesPtr OncillaL1L2::getJointPosition() const {
			return this->_latestJointPosition;
			}

			bool OncillaL1L2::setJointVelocity(JointVelocitiesPtr ) {
			throw NotImplementedError("OncillaL1L2","SetJointVelocity");
			}

			bool OncillaL1L2::setJointTorque(JointTorquesPtr ) {
			throw NotImplementedError("OncillaL1L2","SetJointTorque");
			}

			bool OncillaL1L2::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 {
			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() {
			}

			std::string OncillaL1L2::print() const {
			ostringstream outstream(ostringstream::out);
			outstream.precision(3); // Precision when printing double values
			outstream << "<Oncilla Hip>" << endl;
			return outstream.str();
			}


			void OncillaL1L2::unsafeSetCommand(double value){
			this->_lastCommandedPosition->setValue(0,value);
			}

			double OncillaL1L2::unsafeGetCommand() const{
			return this->_lastCommandedPosition->asDouble(0);
			}

			double OncillaL1L2::unsafeGetMotorAxisPosition() const{
			return this->_latestJointPosition->asDouble(MotorAxisIndex);
			}

			double OncillaL1L2::unsafeGetMagneticEncoderPosition() const{
			return this->_latestJointPosition->asDouble(MagneticEncoderIndex);
			}

			void OncillaL1L2::unsafeUpdateMotorAxisPosition(double value){
			this->_latestJointPosition->setValue(MotorAxisIndex,value);
			}


			void OncillaL1L2::unsafeUpdateMagneticEncoderPosition(double value){
			this->_latestJointPosition->setValue(MagneticEncoderIndex,value);
			}

			}
			}

liboncilla/src/liboncilla/OncillaL1L2.cpp3e9f1f8e42bedev-arne

OncillaL1L2.cppView Options

liboncilla/src/liboncilla/OncillaL1L2.cpp
3e9f1f8e42bedev-arne

OncillaL1L2.cpp
View Options