Page MenuHomec4science

L1L2.cpp
No OneTemporary

File Metadata

Created
Mon, May 20, 13:59

L1L2.cpp

#include "L1L2.h"
#include <liboncilla/exceptions/NotImplementedError.h>
using namespace std;
namespace rci{
namespace oncilla{
L1L2::L1L2(Synchronizer &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 L1L2::isConverged() const {
throw NotImplementedError("OncillaL1L2","isConverged");
}
bool L1L2::setJointPosition(JointAnglesPtr position) {
// Successfull
this->_lastCommandedPosition = position;
return true;
}
JointAnglesPtr L1L2::getJointPosition() const {
return this->_latestJointPosition;
}
bool L1L2::setJointVelocity(JointVelocitiesPtr ) {
throw NotImplementedError("OncillaL1L2","SetJointVelocity");
}
bool L1L2::setJointTorque(JointTorquesPtr ) {
throw NotImplementedError("OncillaL1L2","SetJointTorque");
}
bool L1L2::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 L1L2::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.");
}
L1L2::~L1L2() {
}
std::string L1L2::print() const {
ostringstream outstream(ostringstream::out);
outstream.precision(3); // Precision when printing double values
outstream << "<Oncilla Hip>" << endl;
return outstream.str();
}
void L1L2::unsafeSetCommand(double value){
this->_lastCommandedPosition->setValue(0,value);
}
double L1L2::unsafeGetCommand() const{
return this->_lastCommandedPosition->asDouble(0);
}
double L1L2::unsafeGetMotorAxisPosition() const{
return this->_latestJointPosition->asDouble(MotorAxisIndex);
}
double L1L2::unsafeGetMagneticEncoderPosition() const{
return this->_latestJointPosition->asDouble(MagneticEncoderIndex);
}
void L1L2::unsafeUpdateMotorAxisPosition(double value){
this->_latestJointPosition->setValue(MotorAxisIndex,value);
}
void L1L2::unsafeUpdateMagneticEncoderPosition(double value){
this->_latestJointPosition->setValue(MagneticEncoderIndex,value);
}
}
}

Event Timeline