diff --git a/examples/SimpleSineMovement.cpp b/examples/SimpleSineMovement.cpp index 2683834..c8cc944 100644 --- a/examples/SimpleSineMovement.cpp +++ b/examples/SimpleSineMovement.cpp @@ -1,89 +1,88 @@ #include #include #include #include #include #include #include "liboncilla/Oncilla.h" -#include "liboncilla/OncillaL1L2.h" -#include "liboncilla/OncillaSynchronizer.h" + using namespace std; using namespace boost; using namespace rci; using namespace rci::oncilla; double hip_position(double phase,double amplitude,double offset){ return amplitude * std::cos(phase) + offset; } double knee_position(double phase, double phase_lag,double amplitude,double offset){ return amplitude *((phase + phase_lag) > M_PI ? 1.0 : 0.0) + offset; } int main() { // Instantiate the actual robot object Oncilla oncilla; // Get the synchronizer, so that we can control the actual simulation // stepping from within our code OncillaSynchronizerPtr synchronizer = oncilla.getSynchronizer(); // Get all the joints we are interested in - L1 and L2 - OncillaL1Ptr left_fore_hip = oncilla.getL1(Oncilla::LEFT_FORE); - OncillaL2Ptr left_fore_knee = oncilla.getL2(Oncilla::LEFT_FORE); - OncillaL1Ptr right_fore_hip = oncilla.getL1(Oncilla::RIGHT_FORE); - OncillaL2Ptr right_fore_knee = oncilla.getL2(Oncilla::RIGHT_FORE); - OncillaL1Ptr left_hind_hip = oncilla.getL1(Oncilla::LEFT_HIND); - OncillaL2Ptr left_hind_knee = oncilla.getL2(Oncilla::LEFT_HIND); - OncillaL1Ptr right_hind_hip = oncilla.getL1(Oncilla::RIGHT_HIND); - OncillaL2Ptr right_hind_knee = oncilla.getL2(Oncilla::RIGHT_HIND); + OncillaL1Ptr left_fore_hip = oncilla.getL1(LEFT_FORE); + OncillaL2Ptr left_fore_knee = oncilla.getL2(LEFT_FORE); + OncillaL1Ptr right_fore_hip = oncilla.getL1(RIGHT_FORE); + OncillaL2Ptr right_fore_knee = oncilla.getL2(RIGHT_FORE); + OncillaL1Ptr left_hind_hip = oncilla.getL1(LEFT_HIND); + OncillaL2Ptr left_hind_knee = oncilla.getL2(LEFT_HIND); + OncillaL1Ptr right_hind_hip = oncilla.getL1(RIGHT_HIND); + OncillaL2Ptr right_hind_knee = oncilla.getL2(RIGHT_HIND); //sets some parameter for the sinewave double hip_amplitude(30), fore_hip_offset(0), hind_hip_offset(0), fore_knee_amplitude(0.5), fore_knee_offset(0), hind_knee_amplitude(0), hind_knee_offset(0), fore_hip_knee_phase_lag(0), hind_hip_knee_phase_lag(0), frequency(1.5); // Now move the joints double time_s, phase, antiphase; time_s = 0; while (true) { // The time is our phase - TODO: Use virtual / simulated time time_s += synchronizer->latestProcessLoopDuration(); phase = std::fmod(time_s * frequency * 2 * M_PI, 2 * M_PI); antiphase = std::fmod(phase + M_PI , 2 * M_PI); left_fore_hip->setJointPosition(JointAngles::fromDeg(hip_position(phase,hip_amplitude,fore_hip_offset))); left_fore_knee->setJointPosition(JointAngles::fromRad(knee_position(phase,fore_hip_knee_phase_lag,fore_knee_amplitude,fore_knee_offset))); right_fore_hip->setJointPosition(JointAngles::fromDeg(hip_position(antiphase,hip_amplitude,fore_hip_offset))); right_fore_knee->setJointPosition(JointAngles::fromRad(knee_position(antiphase,fore_hip_knee_phase_lag,fore_knee_amplitude,fore_knee_offset))); left_fore_hip->setJointPosition(JointAngles::fromDeg(hip_position(antiphase,hip_amplitude,hind_hip_offset))); left_fore_knee->setJointPosition(JointAngles::fromRad(knee_position(antiphase,hind_hip_knee_phase_lag,hind_knee_amplitude,hind_knee_offset))); right_fore_hip->setJointPosition(JointAngles::fromDeg(hip_position(phase,hip_amplitude,hind_hip_offset))); right_fore_knee->setJointPosition(JointAngles::fromRad(knee_position(phase,hind_hip_knee_phase_lag,hind_knee_amplitude,hind_knee_offset))); // Now that we set the new angles, we do one processing step synchronizer->process(); } return EXIT_SUCCESS; } diff --git a/src/liboncilla/Oncilla.cpp b/src/liboncilla/Oncilla.cpp index 8519e54..09c2b33 100644 --- a/src/liboncilla/Oncilla.cpp +++ b/src/liboncilla/Oncilla.cpp @@ -1,134 +1,129 @@ /* * Oncilla.cpp * * Created on: 15 dec. 2011 * Author: Alexandre Tuleu, Arne Nordmann */ #include "Oncilla.h" -#include "OncillaTrunk.h" -#include "OncillaL0.h" -#include "OncillaL1L2.h" -#include "OncillaL3.h" -#include "OncillaL4.h" #include "BackendFactory.h" namespace rci { namespace oncilla { OncillaSynchronizerPtr Oncilla::s_synchronizer; Oncilla::LegNames Oncilla::s_legNames; const std::string & Oncilla::nameOfLeg(Leg l){ if(s_legNames.empty()){ s_legNames.push_back("Left Fore"); s_legNames.push_back("Right Fore"); s_legNames.push_back("Left Hind"); s_legNames.push_back("Right Hind"); s_legNames.push_back("Undefined"); } if(l >= NUM_LEGS){ return s_legNames.back(); } return s_legNames[l]; } Oncilla::Oncilla(){ if(!s_synchronizer){ BackendFactory::EntryPtr e = BackendFactory::Instance().HighestPriorityBackend(); if( e == 0){ throw std::runtime_error("No backend for liboncilla were found. Are you" " sure you are linking your program with a backend like " "'liboncilla-webots' or 'liboncilla-hw' ?."); } OncillaSynchronizerPtr p((*e)()); s_synchronizer = p; init(); } } Oncilla::~Oncilla() { } #define CREATE_NODES(LegName)do{\ d_L0s.push_back(OncillaL0Ptr(new OncillaL0(LegName " Oncilla L0" )));\ d_L1s.push_back(OncillaL1Ptr(new OncillaL1L2(*s_synchronizer,\ LegName " Oncilla L1")));\ d_L2s.push_back(OncillaL2Ptr(new OncillaL1L2(*s_synchronizer,\ LegName " Oncilla L2")));\ d_L3s.push_back(OncillaL3Ptr(new OncillaL3(LegName " Oncilla L3")));\ d_L4s.push_back(OncillaL4Ptr(new OncillaL4(LegName " Oncilla L4")));\ }while(0) #define REGISTER_NODES(Type)do{\ for(std::vector < Oncilla ## Type ## Ptr >::const_iterator n = d_ ## Type ## s.begin();\ n != d_ ## Type ## s.end();\ ++n){\ - Oncilla::Leg l = static_cast(n - d_ ## Type ## s.begin());\ + Leg l = static_cast(n - d_ ## Type ## s.begin());\ s_synchronizer->register ## Type ## Node(l,*n);\ }\ }while(0) void Oncilla::init() { d_L0s.reserve(4); d_L1s.reserve(4); d_L2s.reserve(4); d_L3s.reserve(4); d_L4s.reserve(4); CREATE_NODES("Left Fore"); CREATE_NODES("Right Fore"); CREATE_NODES("Left Hind"); CREATE_NODES("Right Hind"); d_trunk = OncillaTrunkPtr(new OncillaTrunk("Oncilla Trunk")); REGISTER_NODES(L0); REGISTER_NODES(L1); REGISTER_NODES(L2); REGISTER_NODES(L3); REGISTER_NODES(L4); s_synchronizer->registerTrunkNode(d_trunk); } OncillaL0Ptr Oncilla::getL0(Leg l) const { return this->d_L0s[l]; } OncillaL1Ptr Oncilla::getL1(Leg l) const { return this->d_L1s[l]; } OncillaL2Ptr Oncilla::getL2(Leg l) const { return this->d_L2s[l]; } OncillaL3Ptr Oncilla::getL3(Leg l) const { return this->d_L3s[l]; } OncillaL4Ptr Oncilla::getL4(Leg l) const { return this->d_L4s[l]; } OncillaTrunkPtr Oncilla::getTrunk() const { return this->d_trunk; } OncillaSynchronizerPtr Oncilla::getSynchronizer() const { if(!s_synchronizer){ throw std::logic_error("Internal error : synchronizer seems unitialized" ", but it should never happen. Please report this bug."); } return this->s_synchronizer; } } } diff --git a/src/liboncilla/Oncilla.h b/src/liboncilla/Oncilla.h index 5a24a1f..40d74a8 100644 --- a/src/liboncilla/Oncilla.h +++ b/src/liboncilla/Oncilla.h @@ -1,81 +1,64 @@ /* * Oncilla.h * * Created on: 15 dec. 2011 * Author: Alexandre Tuleu, Arne Nordmann */ #pragma once #include #include #include +#include "common.h" +#include "OncillaSynchronizer.h" + namespace rci { namespace oncilla { class Oncilla; typedef boost::shared_ptr OncillaPtr; class OncillaSynchronizer; typedef boost::shared_ptr OncillaSynchronizerPtr; -class OncillaTrunk; -typedef boost::shared_ptr OncillaTrunkPtr; -class OncillaL0; -typedef boost::shared_ptr OncillaL0Ptr; -class OncillaL1L2; -typedef boost::shared_ptr OncillaL1Ptr; -typedef boost::shared_ptr OncillaL2Ptr; -class OncillaL3; -typedef boost::shared_ptr OncillaL3Ptr; -class OncillaL4; -typedef boost::shared_ptr OncillaL4Ptr; - class Oncilla { public: - enum Leg { - LEFT_FORE = 0, - RIGHT_FORE = 1, - LEFT_HIND = 2, - RIGHT_HIND = 3, - NUM_LEGS = 4 - }; - static const std::string & nameOfLeg(Leg l); Oncilla(); virtual ~Oncilla(); OncillaL0Ptr getL0(Leg l) const; OncillaL1Ptr getL1(Leg l) const; OncillaL2Ptr getL2(Leg l) const; OncillaL3Ptr getL3(Leg l) const; OncillaL4Ptr getL4(Leg l) const; OncillaTrunkPtr getTrunk() const; OncillaSynchronizerPtr getSynchronizer() const; private: typedef std::vector LegNames; static LegNames s_legNames; void init(); static OncillaSynchronizerPtr s_synchronizer; std::vector d_L0s; std::vector d_L1s; std::vector d_L2s; std::vector d_L3s; std::vector d_L4s; OncillaTrunkPtr d_trunk; }; } } diff --git a/src/liboncilla/OncillaL0.h b/src/liboncilla/OncillaL0.h index e88b312..fce8ae1 100644 --- a/src/liboncilla/OncillaL0.h +++ b/src/liboncilla/OncillaL0.h @@ -1,69 +1,68 @@ #pragma once #include #include #include #include #include #include -#include "OncillaSynchronizer.h" namespace rci { namespace oncilla { class OncillaL0; typedef boost::shared_ptr OncillaL0Ptr; /** * Node class, representing the hip node of the quadruped robot. * @todo In case of simulation, this node can also sense the power consumption. */ class OncillaL0: public rci::ResourceNode, public rci::Controlled, public rci::PositionControlled { public: /** * Special constructor to also link to webots */ OncillaL0(const std::string &name = "Oncilla Hip"); virtual ~OncillaL0(); /** * Returns, if controller is converged. * @return True, if controller is converged after last command. */ bool isConverged() const; /** * Commanding a joint position. * @param position Position command * @return Return, if successfull. (e.g. not exceeding joint limits) * @todo Check for correct control mode */ bool setJointPosition(JointAnglesPtr position); void unsafeSetCommand(double value); double unsafeGetCommand() const; /** * Returns latest position command. Note, that this is the latest valid * commanded position. If the position command exceeds limits and is * therefore rejected, it .. */ virtual JointAnglesPtr getLastPositionCommand() const; /** * Print */ std::string print() const; }; } } diff --git a/src/liboncilla/OncillaL1L2.cpp b/src/liboncilla/OncillaL1L2.cpp index 435cafd..b40c873 100644 --- a/src/liboncilla/OncillaL1L2.cpp +++ b/src/liboncilla/OncillaL1L2.cpp @@ -1,109 +1,111 @@ #include "OncillaL1L2.h" +#include "OncillaSynchronizer.h" + #include 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 std::runtime_error("Convergence check for OncillaL1 and Oncilla L2 " "node is not implemnted yet."); return false; } bool OncillaL1L2::setJointPosition(JointAnglesPtr position) { // Successfull this->_lastCommandedPosition = position; return true; } JointAnglesPtr OncillaL1L2::getJointPosition() const { return this->_latestJointPosition; } bool OncillaL1L2::setJointVelocity(JointVelocitiesPtr ) { throw std::runtime_error("Joint velocity control for OncillaL1 /L2 not is not yet implemented."); } bool OncillaL1L2::setJointTorque(JointTorquesPtr ) { throw std::runtime_error("Joint Torque control for OncillaL1/L2 is not yet implemented."); } bool OncillaL1L2::setJointImpedance(JointImpedancePtr ) { 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 << "" << 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); } } } diff --git a/src/liboncilla/OncillaL1L2.h b/src/liboncilla/OncillaL1L2.h index db4ece6..c35c6bb 100644 --- a/src/liboncilla/OncillaL1L2.h +++ b/src/liboncilla/OncillaL1L2.h @@ -1,125 +1,125 @@ #pragma once #include #include #include #include #include -#include "OncillaSynchronizer.h" - namespace rci{ namespace oncilla{ +class OncillaSynchronizer; class OncillaL1L2; typedef boost::shared_ptr OncillaL1Ptr; typedef boost::shared_ptr OncillaL2Ptr; /** * Node class, representing the hip node of the quadruped robot. * * @todo In case of simulation, this node can also sense the power consumption. */ class OncillaL1L2: public rci::ResourceNode, - public rci::Controlled, - public rci::Sensing, - public rci::PositionControlled, - public rci::PositionSensing, // Two encoder values - public rci::VelocityControlled, - public rci::TorqueControlled, - public rci::ImpedanceControlled { + public rci::Controlled, + public rci::Sensing, + public rci::PositionControlled, + public rci::PositionSensing, // Two encoder values + public rci::VelocityControlled, + public rci::TorqueControlled, + public rci::ImpedanceControlled { public: const static unsigned int MotorAxisIndex = 0; const static unsigned int MagneticEncoderIndex = 1; /** * Special constructor to also link to webots */ - OncillaL1L2(OncillaSynchronizer &s, const std::string &name = "Oncilla Hip"); + OncillaL1L2(OncillaSynchronizer & s, + const std::string & name = "Oncilla Hip"); virtual ~OncillaL1L2(); /** * Returns, if controller is converged. * @return True, if controller is converged after last command. */ bool isConverged() const; /** * Commanding a joint position. * @param position Position command * @return Return, if successfull. (e.g. not exceeding joint limits) * @todo Check for correct control mode */ bool setJointPosition(JointAnglesPtr position); void unsafeSetCommand(double value); double unsafeGetCommand() const ; /** * Returns current joint position. * @return Current joint position */ virtual JointAnglesPtr getJointPosition() const; double unsafeGetMotorAxisPosition() const; double unsafeGetMagneticEncoderPosition() const; void unsafeUpdateMotorAxisPosition(double value); void unsafeUpdateMagneticEncoderPosition(double value); /** * Commanding a joint velocity. * @param position Position command * @return Return, if successfull. (e.g. not exceeding joint limits) * @todo Check for correct control mode */ virtual bool setJointVelocity(rci::JointVelocitiesPtr vel); /** * Returns current joint torque. * @return Current joint torque */ virtual bool setJointTorque(JointTorquesPtr torque); /** * Returns current joint torque. * @return Current joint torque */ virtual bool setJointImpedance(JointImpedancePtr imped); /** * Returns latest position command. Note, that this is the latest valid * commanded position. If the position command exceeds limits and is * therefore rejected, it .. */ virtual JointAnglesPtr getLastPositionCommand() const; /** * Print */ std::string print() const; private: /** * Reference to OncillaSynchronizer * @todo This is a dangerous reference which will become invalid as soon as * OncillaSynchronizer gets deleted outside. */ OncillaSynchronizer &synchronizer; }; } } diff --git a/src/liboncilla/OncillaSynchronizer.h b/src/liboncilla/OncillaSynchronizer.h index 5f1a93a..386500e 100644 --- a/src/liboncilla/OncillaSynchronizer.h +++ b/src/liboncilla/OncillaSynchronizer.h @@ -1,86 +1,83 @@ #pragma once #include #include #include #include -#include "Oncilla.h" +#include "common.h" + +#include "OncillaL0.h" +#include "OncillaL1L2.h" +#include "OncillaL3.h" +#include "OncillaL4.h" +#include "OncillaTrunk.h" namespace rci { namespace oncilla { class OncillaSynchronizer; typedef boost::shared_ptr OncillaSynchronizerPtr; -class OncillaTrunk; -typedef boost::shared_ptr OncillaTrunkPtr; -class OncillaL0; -typedef boost::shared_ptr OncillaL0Ptr; -class OncillaL1L2; -typedef boost::shared_ptr OncillaL1Ptr; -typedef boost::shared_ptr OncillaL2Ptr; -class OncillaL3; -typedef boost::shared_ptr OncillaL3Ptr; -class OncillaL4; -typedef boost::shared_ptr OncillaL4Ptr; + + /** * OncillaSynchronizer * Different OncillaSynchronizers may be for example: * * Update with a given frequency * ** Send data every time * ** Send only if new data * * Update only if new data (event-based) * * Triggered from robot side (e.g. FRI) */ class OncillaSynchronizer: public Synchronizer { public: OncillaSynchronizer(const std::string &name); virtual ~OncillaSynchronizer(); /** * Registers a trunk node. */ virtual void registerTrunkNode(OncillaTrunkPtr node) = 0; /** * Registers an L0 node. */ - virtual void registerL0Node(Oncilla::Leg l, OncillaL0Ptr node) = 0; + virtual void registerL0Node(Leg l, OncillaL0Ptr node) = 0; /** * Registers an L1 node. */ - virtual void registerL1Node(Oncilla::Leg l, OncillaL1Ptr node) = 0; + virtual void registerL1Node(Leg l, OncillaL1Ptr node) = 0; /** * Registers an L2 node. */ - virtual void registerL2Node(Oncilla::Leg l, OncillaL2Ptr node) = 0; + virtual void registerL2Node(Leg l, OncillaL2Ptr node) = 0; /** * Registers an L3 node. */ - virtual void registerL3Node(Oncilla::Leg l, OncillaL3Ptr node) = 0; + virtual void registerL3Node(Leg l, OncillaL3Ptr node) = 0; /** * Registers an L4 node. */ - virtual void registerL4Node(Oncilla::Leg l, OncillaL4Ptr node) = 0; + virtual void registerL4Node(Leg l, OncillaL4Ptr node) = 0; /** * */ friend std::ostream & operator<<(std::ostream& os, const OncillaSynchronizer& val); double latestProcessLoopDuration() const; protected : virtual double latestProcessLoopDurationPrimpl() const = 0; }; } } diff --git a/src/liboncilla/common.h b/src/liboncilla/common.h new file mode 100644 index 0000000..040dbdd --- /dev/null +++ b/src/liboncilla/common.h @@ -0,0 +1,25 @@ +/** + * \file common.h + * + * \date Jun 28, 2012 + * \author Alexandre Tuleu + */ + +#ifndef LIBONCILLA_COMMON_H_ +#define LIBONCILLA_COMMON_H_ + + +namespace rci{ +namespace oncilla{ + + enum Leg { + LEFT_FORE = 0, + RIGHT_FORE = 1, + LEFT_HIND = 2, + RIGHT_HIND = 3, + NUM_LEGS = 4 + }; +} +} + +#endif // LIBONCILLA_COMMON_H_ diff --git a/test/nodes/OncillaL1L2Test.cpp b/test/nodes/OncillaL1L2Test.cpp index e865400..266505a 100644 --- a/test/nodes/OncillaL1L2Test.cpp +++ b/test/nodes/OncillaL1L2Test.cpp @@ -1,85 +1,85 @@ #include #include "liboncilla/OncillaL1L2.h" #include "liboncilla/OncillaSynchronizer.h" using namespace nemo; using namespace rci; using namespace rci::oncilla; class MockSynchronizer: public OncillaSynchronizer { public: MockSynchronizer() : OncillaSynchronizer("Sync") { } virtual void processAsync() { } virtual void waitForProcessAsync() { } virtual void registerTrunkNode(rci::oncilla::OncillaTrunkPtr) { } - virtual void registerL0Node(rci::oncilla::Oncilla::Leg, + virtual void registerL0Node(rci::oncilla::Leg, rci::oncilla::OncillaL0Ptr) { } - virtual void registerL1Node(rci::oncilla::Oncilla::Leg, + virtual void registerL1Node(rci::oncilla::Leg, rci::oncilla::OncillaL1Ptr) { } - virtual void registerL2Node(rci::oncilla::Oncilla::Leg, + virtual void registerL2Node(rci::oncilla::Leg, rci::oncilla::OncillaL2Ptr) { } - virtual void registerL3Node(rci::oncilla::Oncilla::Leg, + virtual void registerL3Node(rci::oncilla::Leg, rci::oncilla::OncillaL3Ptr) { } - virtual void registerL4Node(rci::oncilla::Oncilla::Leg, + virtual void registerL4Node(rci::oncilla::Leg, rci::oncilla::OncillaL4Ptr) { } virtual double latestProcessLoopDurationPrimpl() const { return 0.005; } virtual void switchToSynchronizationModeHook(){ } }; class OncillaL1L2Test: public ::testing::Test { protected: OncillaL1L2Test() : joint(), ja_values(), ji_values(), ja(), ji(), synchr() { joint = OncillaL1Ptr(new OncillaL1L2(synchr, "FooBar")); ja_values = RealVector(dim(1), 1.234); ji_values = RealVector(dim(2), 1.234); ji_values[1] = 2.345; ja = JointAngles::fromRad(ja_values); ji = JointImpedancePtr(new JointImpedance(ji_values)); } virtual ~OncillaL1L2Test() { } OncillaL1Ptr joint; RealVector ja_values, ji_values; JointAnglesPtr ja; JointImpedancePtr ji; MockSynchronizer synchr; }; TEST_F(OncillaL1L2Test, testInitialialState) { EXPECT_NO_THROW(joint->getLastPositionCommand()); EXPECT_EQ(0.0,joint->getLastPositionCommand()->rad()); } TEST_F(OncillaL1L2Test, testPositionCommand) { EXPECT_NO_THROW(joint->getLastPositionCommand()); EXPECT_EQ(0.0,joint->getLastPositionCommand()->rad()); joint->setJointPosition(ja); EXPECT_EQ(1.234, joint->getLastPositionCommand()->rad()); }