Diffusion liboncilla (newexample) Browse

OncillaL2.cpp
3cc9013c8013newexample

OncillaL2.cpp
View Options

			#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;
			this->_lastCommandedPosition = JointAngles::create(1,0.0);
			this->_latestJointPosition = JointAngles::create(2,0.0);
			}

			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 << "<Oncilla Knee>" << endl;
			return outstream.str();
			}

			bool OncillaL2::setJointTorque(JointTorquesPtr ) {
			throw std::runtime_error("Not yet implemented.");
			}

			bool OncillaL2::setJointImpedance(JointImpedancePtr ) {
			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 ) {
			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;
			}

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

			double OncillaL2::getRawLocalCommand() const{
			return this->_lastCommandedPosition->asDouble(0);
			}

			double OncillaL2::getRawLocalMotorAxisPosition() const{
			return this->_latestJointPosition->asDouble(MotorAxisIndex);
			}

			double OncillaL2::getRawLocalMagneticEncoderPosition() const{
			return this->_latestJointPosition->asDouble(MagneticEncoderIndex);
			}

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


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


			}
			}

liboncilla/src/OncillaL2.cpp3cc9013c8013newexample

OncillaL2.cppView Options

liboncilla/src/OncillaL2.cpp
3cc9013c8013newexample

OncillaL2.cpp
View Options