Homec4science
Diffusion liboncilla (newexample) Browse

liboncilla/src/OncillaL2.cpp
b6739edb9ec2newexample

Last Change
Raw File

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;
}
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 torques) {
throw std::runtime_error("Not yet implemented.");
}
bool OncillaL2::setJointImpedance(JointImpedancePtr imped) {
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;
}
}
}
c4science · Help