Homec4science
Diffusion liboncilla (debian-0.2) Browse

liboncilla/src/OncillaKnee.cpp
a92ac2ba5450debian-0.2

Last Change
Raw File

OncillaKnee.cpp
View Options

#include "OncillaKnee.h"
using namespace std;
using namespace boost;
using namespace rsb;
namespace cca {
namespace rci {
namespace driver {
OncillaKnee::OncillaKnee(std::string name) :
ResourceNode(name), Controlled(), Sensing(), PositionControlled(),
PositionSensing(), VelocityControlled(), TorqueControlled(),
ImpedanceControlled() {
// Two encoder values - difference is due to passive compliance
this->_dimension = 2;
this->addInputPort<JointAngles> (); // PositionControlled
this->addInputPort<JointVelocities> (); // VelocityControlled
this->addInputPort<JointTorques> (); // TorqueControlled
this->addInputPort<JointImpedance> (); // ImpedanceControlled
this->addOutputPort<JointAngles> (); // PositionSensing
}
void OncillaKnee::onProcess() {
if (!this->inputs[0]->empty() && this->inputs[0]->newItem()) {
// Cache new value
this->setJointPosition(
boost::static_pointer_cast<JointAngles>(
this->inputs[0]->tryGet()));
}
// Publish value
if ((this->_latestJointPosition != NULL)
&& (this->_latestJointPosition.get() != NULL)) {
Informer<JointAngles>::DataPtr status(
new JointAngles(this->_latestJointPosition->asDouble(0)));
this->publish(0, status);
}
}
bool OncillaKnee::isConverged() const {
return false;
}
bool OncillaKnee::setJointPosition(JointAnglesPtr position) {
// Successfull
// std::cerr << "[OncillaKnee] " << position << std::endl;
this->_lastCommandedPosition = position;
return true;
}
JointAnglesPtr OncillaKnee::getJointPosition() const {
return this->_latestJointPosition;
}
OncillaKnee::~OncillaKnee() {
}
std::string OncillaKnee::print() const {
ostringstream outstream(ostringstream::out);
outstream.precision(3); // Precision when printing double values
outstream << "<Oncilla Knee>" << endl;
return outstream.str();
}
CCANodePtr OncillaKnee::create(string name) {
CCANodePtr kneenode(new OncillaKnee(name));
return kneenode;
}
bool OncillaKnee::setJointTorque(JointTorquesPtr torques) {
throw std::runtime_error("Not yet implemented.");
}
bool OncillaKnee::setJointImpedance(JointImpedancePtr imped) {
throw std::runtime_error("Not yet implemented.");
}
bool OncillaKnee::setJointVelocity(JointVelocitiesPtr vel) {
throw std::runtime_error("Not yet implemented.");
}
JointAnglesPtr OncillaKnee::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