Skip to content

Use long int for signal time. #4

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.

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

Merged
merged 2 commits into from
Jul 28, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
2 changes: 1 addition & 1 deletion src/controller.cc
Original file line number Diff line number Diff line change
Expand Up @@ -110,7 +110,7 @@ SoTRobotArmController::~SoTRobotArmController()
{
}

void SoTRobotArmController::setControlSize(const int& size)
void SoTRobotArmController::setControlSize(const size_type& size)
{
device_->setControlSize(size);
}
Expand Down
4 changes: 2 additions & 2 deletions src/controller.hh
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ using dynamicgraph::sot::AbstractSotExternalInterface;
using dynamicgraph::sot::Device;
using dynamicgraph::sot::ControlValues;
using dynamicgraph::sot::SensorValues;

using dynamicgraph::size_type;

class SoTRobotArmController: public AbstractSotExternalInterface
{
Expand All @@ -63,7 +63,7 @@ class SoTRobotArmController: public AbstractSotExternalInterface
void setSecondOrderIntegration(void);

// Set the number of joints that are controlled
virtual void setControlSize(const int& size);
virtual void setControlSize(const size_type& size);

// Run some python commands that cannot be run at construction
virtual void initializePython();
Expand Down
2 changes: 1 addition & 1 deletion src/device.cc
Original file line number Diff line number Diff line change
Expand Up @@ -90,7 +90,7 @@ void SoTRobotArmDevice::setSensors(map<string,SensorValues> &SensorsIn)
{
sotDEBUGIN(25) ;
map<string,SensorValues>::iterator it;
int t = stateSOUT.getTime () + 1;
sigtime_t t = stateSOUT.getTime () + 1;
bool setRobotState = false;

it = SensorsIn.find("forces");
Expand Down
12 changes: 7 additions & 5 deletions src/device.hh
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@ using dynamicgraph::sot::SensorValues;
class SoTRobotArmDevice : public dynamicgraph::sot::Device
{
public:
typedef dynamicgraph::sigtime_t sigtime_t;
static const std::string CLASS_NAME;
static const double TIMESTEP_DEFAULT;

Expand Down Expand Up @@ -79,9 +80,9 @@ protected:
dynamicgraph::Vector previousState_;

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

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

/// Intermediate variables to avoid allocation during control
dynamicgraph::Vector dgforces_;
Expand All @@ -99,6 +100,7 @@ protected:
class DeviceToDynamic : public dynamicgraph::Entity
{
public:
typedef dynamicgraph::sigtime_t sigtime_t;
static const std::string CLASS_NAME;
DeviceToDynamic(const std::string& name) :
dynamicgraph::Entity(name), sinSIN(0x0, "DeviceToDynamic("+name+")::input(vector)::sin"),
Expand All @@ -112,14 +114,14 @@ public:
inputSize_ = size;
}
private:
dynamicgraph::Vector& compute(dynamicgraph::Vector& res, int time)
dynamicgraph::Vector& compute(dynamicgraph::Vector& res, sigtime_t time)
{
res.head<7>() = sinSIN(time);
res[7] = 0;
return res;
}
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> sinSIN;
dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> soutSOUT;
dynamicgraph::SignalPtr<dynamicgraph::Vector, sigtime_t> sinSIN;
dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, sigtime_t> soutSOUT;
int inputSize_;
};

Expand Down