diff --git a/src/roscontrol-sot-controller.cpp b/src/roscontrol-sot-controller.cpp index 561a241..5650bbd 100644 --- a/src/roscontrol-sot-controller.cpp +++ b/src/roscontrol-sot-controller.cpp @@ -61,7 +61,7 @@ using namespace rc_sot_system; namespace sot_controller { typedef std::map::iterator it_map_rt_to_sot; typedef std::map::iterator it_control_mode; - +typedef dynamicgraph::size_type size_type; ControlPDMotorControlData::ControlPDMotorControlData() {} void ControlPDMotorControlData::read_from_xmlrpc_value( @@ -171,7 +171,7 @@ bool RCSotController::initRequest(lhi::RobotHW *robot_hw, ROS_WARN("initRequest 4"); /// Create SoT SotLoaderBasic::Initialization(); - sotController_->setControlSize((int)joints_name_.size()); + sotController_->setControlSize((size_type)joints_name_.size()); ROS_WARN("initRequest 5"); return true; } diff --git a/tests/sot-test-controller.cpp b/tests/sot-test-controller.cpp index 54e44bb..19d7886 100644 --- a/tests/sot-test-controller.cpp +++ b/tests/sot-test-controller.cpp @@ -101,7 +101,7 @@ void SoTTestController::getControl( } } -void SoTTestController::setControlSize(const int &size) { +void SoTTestController::setControlSize(const size_type &size) { device_->setControlSize(size); } diff --git a/tests/sot-test-controller.hh b/tests/sot-test-controller.hh index 87efea0..8f11e94 100644 --- a/tests/sot-test-controller.hh +++ b/tests/sot-test-controller.hh @@ -25,6 +25,7 @@ namespace dgsot = dynamicgraph::sot; class SoTTestController : public dgsot::AbstractSotExternalInterface { public: + typedef dynamicgraph::size_type size_type; static const std::string LOG_PYTHON; SoTTestController(); @@ -41,7 +42,7 @@ class SoTTestController : public dgsot::AbstractSotExternalInterface { void getControl(std::map &anglesOut, const double &period); - void setControlSize(const int &size); + void setControlSize(const size_type &size); void initialize(); void setNoIntegration(void); void setSecondOrderIntegration(void); diff --git a/tests/sot-test-device.cpp b/tests/sot-test-device.cpp index c7bf230..f9b7c5b 100644 --- a/tests/sot-test-device.cpp +++ b/tests/sot-test-device.cpp @@ -107,7 +107,7 @@ SoTTestDevice::SoTTestDevice(std::string RobotName) SoTTestDevice::~SoTTestDevice() {} void SoTTestDevice::setSensorsForce(map &SensorsIn, - int t) { + sigtime_t t) { int map_sot_2_urdf[4] = {2, 0, 3, 1}; sotDEBUGIN(15); map::iterator it; @@ -133,7 +133,7 @@ void SoTTestDevice::setSensorsForce(map &SensorsIn, } void SoTTestDevice::setSensorsIMU(map &SensorsIn, - int t) { + sigtime_t t) { map::iterator it; // TODO: Confirm if this can be made quaternion it = SensorsIn.find("attitude"); @@ -164,7 +164,7 @@ void SoTTestDevice::setSensorsIMU(map &SensorsIn, } void SoTTestDevice::setSensorsEncoders( - map &SensorsIn, int t) { + map &SensorsIn, sigtime_t t) { map::iterator it; it = SensorsIn.find("motor-angles"); @@ -195,7 +195,7 @@ void SoTTestDevice::setSensorsEncoders( } void SoTTestDevice::setSensorsVelocities( - map &SensorsIn, int t) { + map &SensorsIn, sigtime_t t) { map::iterator it; it = SensorsIn.find("velocities"); @@ -212,7 +212,7 @@ void SoTTestDevice::setSensorsVelocities( } void SoTTestDevice::setSensorsTorquesCurrents( - map &SensorsIn, int t) { + map &SensorsIn, sigtime_t t) { map::iterator it; it = SensorsIn.find("torques"); if (it != SensorsIn.end()) { @@ -235,7 +235,7 @@ void SoTTestDevice::setSensorsTorquesCurrents( } void SoTTestDevice::setSensorsGains(map &SensorsIn, - int t) { + sigtime_t t) { map::iterator it; it = SensorsIn.find("p_gains"); if (it != SensorsIn.end()) { @@ -259,7 +259,7 @@ void SoTTestDevice::setSensorsGains(map &SensorsIn, void SoTTestDevice::setSensors(map &SensorsIn) { sotDEBUGIN(25); map::iterator it; - int t = stateSOUT.getTime() + 1; + sigtime_t t = stateSOUT.getTime() + 1; setSensorsForce(SensorsIn, t); setSensorsIMU(SensorsIn, t); @@ -312,7 +312,7 @@ void SoTTestDevice::getControl(map &controlOut, for (unsigned int i = 6; i < state_.size(); ++i) anglesOut[i - 6] = state_(i); controlOut["control"].setValues(anglesOut); // Read zmp reference from input signal if plugged - int time = controlSIN.getTime(); + sigtime_t time = controlSIN.getTime(); zmpSIN.recompute(time + 1); // Express ZMP in free flyer reference frame dg::Vector zmpGlobal(4); diff --git a/tests/sot-test-device.hh b/tests/sot-test-device.hh index 760d181..38cbf8c 100644 --- a/tests/sot-test-device.hh +++ b/tests/sot-test-device.hh @@ -22,6 +22,7 @@ namespace dgsot = dynamicgraph::sot; namespace dg = dynamicgraph; +typedef dg::sigtime_t sigtime_t; class SoTTestDevice : public dgsot::Device { public: @@ -54,34 +55,34 @@ class SoTTestDevice : public dgsot::Device { std::vector baseff_; /// Accelerations measured by accelerometers - dynamicgraph::Signal accelerometerSOUT_; + dynamicgraph::Signal accelerometerSOUT_; /// Rotation velocity measured by gyrometers - dynamicgraph::Signal gyrometerSOUT_; + dynamicgraph::Signal gyrometerSOUT_; /// motor currents - dynamicgraph::Signal currentsSOUT_; + dynamicgraph::Signal currentsSOUT_; /// joint angles - dynamicgraph::Signal joint_anglesSOUT_; + dynamicgraph::Signal joint_anglesSOUT_; /// motor angles - dynamicgraph::Signal motor_anglesSOUT_; + dynamicgraph::Signal motor_anglesSOUT_; /// proportional and derivative position-control gains - dynamicgraph::Signal p_gainsSOUT_; + dynamicgraph::Signal p_gainsSOUT_; - dynamicgraph::Signal d_gainsSOUT_; + dynamicgraph::Signal d_gainsSOUT_; /// Protected methods for internal variables filling void setSensorsForce(std::map &SensorsIn, - int t); + sigtime_t t); void setSensorsIMU(std::map &SensorsIn, - int t); + sigtime_t t); void setSensorsEncoders(std::map &SensorsIn, - int t); + sigtime_t t); void setSensorsVelocities( - std::map &SensorsIn, int t); + std::map &SensorsIn, sigtime_t t); void setSensorsTorquesCurrents( - std::map &SensorsIn, int t); + std::map &SensorsIn, sigtime_t t); void setSensorsGains(std::map &SensorsIn, - int t); + sigtime_t t); /// Intermediate variables to avoid allocation during control dg::Vector dgforces_;