Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Use long int for signal time. #43

Merged
merged 2 commits into from
Jul 28, 2023
Merged

Use long int for signal time. #43

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
e5d815e
Use int64_t as type for signal time.
florent-lamiraux Jun 16, 2023
8363648
Use dynamicgraph::size_type instead of int.
florent-lamiraux Jun 23, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions src/roscontrol-sot-controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,7 @@ using namespace rc_sot_system;
namespace sot_controller {
typedef std::map<std::string, std::string>::iterator it_map_rt_to_sot;
typedef std::map<std::string, std::string>::iterator it_control_mode;

typedef dynamicgraph::size_type size_type;
ControlPDMotorControlData::ControlPDMotorControlData() {}

void ControlPDMotorControlData::read_from_xmlrpc_value(
Expand Down Expand Up @@ -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;
}
Expand Down
2 changes: 1 addition & 1 deletion tests/sot-test-controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -101,7 +101,7 @@ void SoTTestController::getControl(
}
}

void SoTTestController::setControlSize(const int &size) {
void SoTTestController::setControlSize(const size_type &size) {
device_->setControlSize(size);
}

Expand Down
3 changes: 2 additions & 1 deletion tests/sot-test-controller.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand All @@ -41,7 +42,7 @@ class SoTTestController : public dgsot::AbstractSotExternalInterface {
void getControl(std::map<std::string, dgsot::ControlValues> &anglesOut,
const double &period);

void setControlSize(const int &size);
void setControlSize(const size_type &size);
void initialize();
void setNoIntegration(void);
void setSecondOrderIntegration(void);
Expand Down
16 changes: 8 additions & 8 deletions tests/sot-test-device.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -107,7 +107,7 @@ SoTTestDevice::SoTTestDevice(std::string RobotName)
SoTTestDevice::~SoTTestDevice() {}

void SoTTestDevice::setSensorsForce(map<string, dgsot::SensorValues> &SensorsIn,
int t) {
sigtime_t t) {
int map_sot_2_urdf[4] = {2, 0, 3, 1};
sotDEBUGIN(15);
map<string, dgsot::SensorValues>::iterator it;
Expand All @@ -133,7 +133,7 @@ void SoTTestDevice::setSensorsForce(map<string, dgsot::SensorValues> &SensorsIn,
}

void SoTTestDevice::setSensorsIMU(map<string, dgsot::SensorValues> &SensorsIn,
int t) {
sigtime_t t) {
map<string, dgsot::SensorValues>::iterator it;
// TODO: Confirm if this can be made quaternion
it = SensorsIn.find("attitude");
Expand Down Expand Up @@ -164,7 +164,7 @@ void SoTTestDevice::setSensorsIMU(map<string, dgsot::SensorValues> &SensorsIn,
}

void SoTTestDevice::setSensorsEncoders(
map<string, dgsot::SensorValues> &SensorsIn, int t) {
map<string, dgsot::SensorValues> &SensorsIn, sigtime_t t) {
map<string, dgsot::SensorValues>::iterator it;

it = SensorsIn.find("motor-angles");
Expand Down Expand Up @@ -195,7 +195,7 @@ void SoTTestDevice::setSensorsEncoders(
}

void SoTTestDevice::setSensorsVelocities(
map<string, dgsot::SensorValues> &SensorsIn, int t) {
map<string, dgsot::SensorValues> &SensorsIn, sigtime_t t) {
map<string, dgsot::SensorValues>::iterator it;

it = SensorsIn.find("velocities");
Expand All @@ -212,7 +212,7 @@ void SoTTestDevice::setSensorsVelocities(
}

void SoTTestDevice::setSensorsTorquesCurrents(
map<string, dgsot::SensorValues> &SensorsIn, int t) {
map<string, dgsot::SensorValues> &SensorsIn, sigtime_t t) {
map<string, dgsot::SensorValues>::iterator it;
it = SensorsIn.find("torques");
if (it != SensorsIn.end()) {
Expand All @@ -235,7 +235,7 @@ void SoTTestDevice::setSensorsTorquesCurrents(
}

void SoTTestDevice::setSensorsGains(map<string, dgsot::SensorValues> &SensorsIn,
int t) {
sigtime_t t) {
map<string, dgsot::SensorValues>::iterator it;
it = SensorsIn.find("p_gains");
if (it != SensorsIn.end()) {
Expand All @@ -259,7 +259,7 @@ void SoTTestDevice::setSensorsGains(map<string, dgsot::SensorValues> &SensorsIn,
void SoTTestDevice::setSensors(map<string, dgsot::SensorValues> &SensorsIn) {
sotDEBUGIN(25);
map<string, dgsot::SensorValues>::iterator it;
int t = stateSOUT.getTime() + 1;
sigtime_t t = stateSOUT.getTime() + 1;

setSensorsForce(SensorsIn, t);
setSensorsIMU(SensorsIn, t);
Expand Down Expand Up @@ -312,7 +312,7 @@ void SoTTestDevice::getControl(map<string, dgsot::ControlValues> &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);
Expand Down
27 changes: 14 additions & 13 deletions tests/sot-test-device.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@

namespace dgsot = dynamicgraph::sot;
namespace dg = dynamicgraph;
typedef dg::sigtime_t sigtime_t;

class SoTTestDevice : public dgsot::Device {
public:
Expand Down Expand Up @@ -54,34 +55,34 @@ class SoTTestDevice : public dgsot::Device {
std::vector<double> baseff_;

/// Accelerations measured by accelerometers
dynamicgraph::Signal<dg::Vector, int> accelerometerSOUT_;
dynamicgraph::Signal<dg::Vector, sigtime_t> accelerometerSOUT_;
/// Rotation velocity measured by gyrometers
dynamicgraph::Signal<dg::Vector, int> gyrometerSOUT_;
dynamicgraph::Signal<dg::Vector, sigtime_t> gyrometerSOUT_;
/// motor currents
dynamicgraph::Signal<dg::Vector, int> currentsSOUT_;
dynamicgraph::Signal<dg::Vector, sigtime_t> currentsSOUT_;
/// joint angles
dynamicgraph::Signal<dg::Vector, int> joint_anglesSOUT_;
dynamicgraph::Signal<dg::Vector, sigtime_t> joint_anglesSOUT_;
/// motor angles
dynamicgraph::Signal<dg::Vector, int> motor_anglesSOUT_;
dynamicgraph::Signal<dg::Vector, sigtime_t> motor_anglesSOUT_;

/// proportional and derivative position-control gains
dynamicgraph::Signal<dg::Vector, int> p_gainsSOUT_;
dynamicgraph::Signal<dg::Vector, sigtime_t> p_gainsSOUT_;

dynamicgraph::Signal<dg::Vector, int> d_gainsSOUT_;
dynamicgraph::Signal<dg::Vector, sigtime_t> d_gainsSOUT_;

/// Protected methods for internal variables filling
void setSensorsForce(std::map<std::string, dgsot::SensorValues> &SensorsIn,
int t);
sigtime_t t);
void setSensorsIMU(std::map<std::string, dgsot::SensorValues> &SensorsIn,
int t);
sigtime_t t);
void setSensorsEncoders(std::map<std::string, dgsot::SensorValues> &SensorsIn,
int t);
sigtime_t t);
void setSensorsVelocities(
std::map<std::string, dgsot::SensorValues> &SensorsIn, int t);
std::map<std::string, dgsot::SensorValues> &SensorsIn, sigtime_t t);
void setSensorsTorquesCurrents(
std::map<std::string, dgsot::SensorValues> &SensorsIn, int t);
std::map<std::string, dgsot::SensorValues> &SensorsIn, sigtime_t t);
void setSensorsGains(std::map<std::string, dgsot::SensorValues> &SensorsIn,
int t);
sigtime_t t);

/// Intermediate variables to avoid allocation during control
dg::Vector dgforces_;
Expand Down