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 int64_t as type for signal time. #39

Merged
merged 1 commit into from
Jul 28, 2023
Merged

Use int64_t as type for signal time. #39

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
50 changes: 0 additions & 50 deletions .travis/build
View file
Open in desktop

This file was deleted.

13 changes: 7 additions & 6 deletions include/sot/tools/cubic-interpolation-se3.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,14 +33,15 @@ class CubicInterpolationSE3 : public Entity {

protected:
virtual void doStart(const double& duration);
dynamicgraph::Signal<MatrixHomogeneous, int> soutSOUT_;
dynamicgraph::Signal<Vector, int> soutdotSOUT_;
dynamicgraph::SignalPtr<MatrixHomogeneous, int> initSIN_;
dynamicgraph::SignalPtr<MatrixHomogeneous, int> goalSIN_;
dynamicgraph::Signal<MatrixHomogeneous, sigtime_t> soutSOUT_;
dynamicgraph::Signal<Vector, sigtime_t> soutdotSOUT_;
dynamicgraph::SignalPtr<MatrixHomogeneous, sigtime_t> initSIN_;
dynamicgraph::SignalPtr<MatrixHomogeneous, sigtime_t> goalSIN_;

MatrixHomogeneous& computeSout(MatrixHomogeneous& sout, const int& inTime);
MatrixHomogeneous& computeSout(MatrixHomogeneous& sout,
const sigtime_t& inTime);

int startTime_;
sigtime_t startTime_;
double samplingPeriod_;
double duration_;
// 0: motion not started, 1: motion in progress, 2: motion finished
Expand Down
14 changes: 7 additions & 7 deletions include/sot/tools/cubic-interpolation.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,15 +32,15 @@ class CubicInterpolation : public Entity {

protected:
virtual void doStart(const double& duration);
dynamicgraph::Signal<Vector, int> soutSOUT_;
dynamicgraph::Signal<Vector, int> soutdotSOUT_;
dynamicgraph::SignalPtr<Vector, int> initSIN_;
dynamicgraph::SignalPtr<Vector, int> goalSIN_;
dynamicgraph::Signal<Vector, sigtime_t> soutSOUT_;
dynamicgraph::Signal<Vector, sigtime_t> soutdotSOUT_;
dynamicgraph::SignalPtr<Vector, sigtime_t> initSIN_;
dynamicgraph::SignalPtr<Vector, sigtime_t> goalSIN_;

Vector& computeSout(Vector& sout, const int& inTime);
Vector& computeSoutdot(Vector& sout, const int& inTime);
Vector& computeSout(Vector& sout, const sigtime_t& inTime);
Vector& computeSoutdot(Vector& sout, const sigtime_t& inTime);

int startTime_;
sigtime_t startTime_;
double samplingPeriod_;
double duration_;
// 0: motion not started, 1: motion in progress, 2: motion finished
Expand Down
2 changes: 1 addition & 1 deletion include/sot/tools/kinematic-planner.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -95,7 +95,7 @@ class KinematicPlanner : public Entity {
int nSources1; // 5
int nSources2; // 4
/*! @} */
std::list<dynamicgraph::SignalBase<int>*> genericSignalRefs;
std::list<dynamicgraph::SignalBase<sigtime_t>*> genericSignalRefs;

// Load Motion Capture outputs
template <typename Derived>
Expand Down
16 changes: 8 additions & 8 deletions include/sot/tools/oscillator.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,18 +29,18 @@ class Oscillator : public Entity {
Oscillator(const std::string name);

protected:
double& computeSignal(double& sout, const int& t);
double& computeSignal(double& sout, const sigtime_t& t);
dynamicgraph::Vector& computeVectorSignal(dynamicgraph::Vector& vsout,
const int& t);
const sigtime_t& t);
double value(double dt, double time, double omega, double phase,
double amplitude, double bias);

SignalPtr<double, int> angularFrequencySIN_;
SignalPtr<double, int> magnitudeSIN_;
SignalPtr<double, int> phaseSIN_;
SignalPtr<double, int> biasSIN_;
SignalTimeDependent<double, int> soutSOUT_;
SignalTimeDependent<dynamicgraph::Vector, int> vectorSoutSOUT_;
SignalPtr<double, sigtime_t> angularFrequencySIN_;
SignalPtr<double, sigtime_t> magnitudeSIN_;
SignalPtr<double, sigtime_t> phaseSIN_;
SignalPtr<double, sigtime_t> biasSIN_;
SignalTimeDependent<double, sigtime_t> soutSOUT_;
SignalTimeDependent<dynamicgraph::Vector, sigtime_t> vectorSoutSOUT_;

double epsilon_;
bool started_;
Expand Down
55 changes: 29 additions & 26 deletions include/sot/tools/seqplay.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,18 +26,18 @@ using dynamicgraph::Vector;
using dynamicgraph::sot::MatrixHomogeneous;

class Seqplay : public Entity {
Signal<Vector, int> postureSOUT_;
Signal<MatrixHomogeneous, int> leftAnkleSOUT_;
Signal<MatrixHomogeneous, int> rightAnkleSOUT_;
Signal<Vector, int> leftAnkleVelSOUT_;
Signal<Vector, int> rightAnkleVelSOUT_;
Signal<Vector, int> comSOUT_;
Signal<Vector, int> comdotSOUT_;
Signal<Vector, int> comddotSOUT_;
Signal<Vector, int> forceLeftFootSOUT_;
Signal<Vector, int> forceRightFootSOUT_;

Signal<Vector, int> zmpSOUT_;
Signal<Vector, sigtime_t> postureSOUT_;
Signal<MatrixHomogeneous, sigtime_t> leftAnkleSOUT_;
Signal<MatrixHomogeneous, sigtime_t> rightAnkleSOUT_;
Signal<Vector, sigtime_t> leftAnkleVelSOUT_;
Signal<Vector, sigtime_t> rightAnkleVelSOUT_;
Signal<Vector, sigtime_t> comSOUT_;
Signal<Vector, sigtime_t> comdotSOUT_;
Signal<Vector, sigtime_t> comddotSOUT_;
Signal<Vector, sigtime_t> forceLeftFootSOUT_;
Signal<Vector, sigtime_t> forceRightFootSOUT_;

Signal<Vector, sigtime_t> zmpSOUT_;

DYNAMIC_GRAPH_ENTITY_DECL();
Seqplay(const std::string& name);
Expand All @@ -47,29 +47,32 @@ class Seqplay : public Entity {
virtual std::string getDocString() const;

private:
Vector& computePosture(Vector& pos, const int& t);
MatrixHomogeneous& computeLeftAnkle(MatrixHomogeneous& la, const int& t);
MatrixHomogeneous& computeRightAnkle(MatrixHomogeneous& ra, const int& t);
Vector& computePosture(Vector& pos, const sigtime_t& t);
MatrixHomogeneous& computeLeftAnkle(MatrixHomogeneous& la,
const sigtime_t& t);
MatrixHomogeneous& computeRightAnkle(MatrixHomogeneous& ra,
const sigtime_t& t);
Vector& computeAnkleVelocity(
Vector& velocity, const std::vector<MatrixHomogeneous>& ankleVector,
const int& t);
Vector& computeLeftAnkleVel(Vector& velocity, const int& t);
Vector& computeRightAnkleVel(Vector& velocity, const int& t);
Vector& computeCom(Vector& com, const int& t);
Vector& computeComdot(Vector& comdot, const int& t);
Vector& computeComddot(Vector& comdot, const int& t);
Vector& computeZMP(Vector& comdot, const int& t);
Vector& computeForceFoot(Vector&, const std::vector<Vector>&, const int&);
Vector& computeForceLeftFoot(Vector& force, const int& t);
Vector& computeForceRightFoot(Vector& force, const int& t);
const sigtime_t& t);
Vector& computeLeftAnkleVel(Vector& velocity, const sigtime_t& t);
Vector& computeRightAnkleVel(Vector& velocity, const sigtime_t& t);
Vector& computeCom(Vector& com, const sigtime_t& t);
Vector& computeComdot(Vector& comdot, const sigtime_t& t);
Vector& computeComddot(Vector& comdot, const sigtime_t& t);
Vector& computeZMP(Vector& comdot, const sigtime_t& t);
Vector& computeForceFoot(Vector&, const std::vector<Vector>&,
const sigtime_t&);
Vector& computeForceLeftFoot(Vector& force, const sigtime_t& t);
Vector& computeForceRightFoot(Vector& force, const sigtime_t& t);

void readAnkleFile(std::ifstream&, std::vector<MatrixHomogeneous>&,
const std::string&);
void readForceFile(std::ifstream&, std::vector<Vector>&, const std::string&);
// 0: motion not started, 1: motion in progress, 2: motion finished
unsigned int state_;
unsigned int configId_;
int startTime_;
sigtime_t startTime_;

std::vector<Vector> posture_;
std::vector<MatrixHomogeneous> leftAnkle_;
Expand Down
8 changes: 4 additions & 4 deletions include/sot/tools/simpleseqplay.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,10 +28,10 @@ namespace dg = dynamicgraph;
class SimpleSeqPlay : public dg::Entity {
public:
typedef int Dummy;
dg::SignalTimeDependent<Dummy, int> firstSINTERN;
dg::SignalTimeDependent<dg::Vector, int> postureSOUT_;
dg::SignalTimeDependent<Dummy, sigtime_t> firstSINTERN;
dg::SignalTimeDependent<dg::Vector, sigtime_t> postureSOUT_;

dg::SignalPtr<dg::Vector, int> currentPostureSIN_;
dg::SignalPtr<dg::Vector, sigtime_t> currentPostureSIN_;

DYNAMIC_GRAPH_ENTITY_DECL();
SimpleSeqPlay(const std::string& name);
Expand All @@ -51,7 +51,7 @@ class SimpleSeqPlay : public dg::Entity {
// 1: going to the current position to the first position.
// 2: motion in progress, 3: motion finished
unsigned int state_;
int startTime_;
sigtime_t startTime_;

std::vector<dg::Vector> posture_;
dg::Vector currentPosture_;
Expand Down
2 changes: 1 addition & 1 deletion src/cubic-interpolation-se3.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -93,7 +93,7 @@ std::string CubicInterpolationSE3::getDocString() const {
void CubicInterpolationSE3::reset() { state_ = 0; }

sot::MatrixHomogeneous& CubicInterpolationSE3::computeSout(
sot::MatrixHomogeneous& sout, const int& inTime) {
sot::MatrixHomogeneous& sout, const sigtime_t& inTime) {
double t;
switch (state_) {
case 0:
Expand Down
9 changes: 5 additions & 4 deletions src/cubic-interpolation.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -83,14 +83,14 @@ std::string CubicInterpolation::getDocString() const {

void CubicInterpolation::reset() { state_ = 0; }

Vector& CubicInterpolation::computeSout(Vector& sout, const int& inTime) {
Vector& CubicInterpolation::computeSout(Vector& sout, const sigtime_t& inTime) {
double t;
switch (state_) {
case 0:
sout = initSIN_.accessCopy();
break;
case 1:
t = (inTime - startTime_) * samplingPeriod_;
t = (double)(inTime - startTime_) * samplingPeriod_;
sout = p0_ + (p1_ + (p2_ + p3_ * t) * t) * t;
if (t >= duration_) {
state_ = 2;
Expand All @@ -104,15 +104,16 @@ Vector& CubicInterpolation::computeSout(Vector& sout, const int& inTime) {
return sout;
}

Vector& CubicInterpolation::computeSoutdot(Vector& soutdot, const int& inTime) {
Vector& CubicInterpolation::computeSoutdot(Vector& soutdot,
const sigtime_t& inTime) {
soutdot.resize(initSIN_.accessCopy().size());
double t;
switch (state_) {
case 0:
soutdot.setZero();
break;
case 1:
t = (inTime - startTime_) * samplingPeriod_;
t = (double)(inTime - startTime_) * samplingPeriod_;
soutdot = p1_ + (p2_ * 2 + p3_ * (3 * t)) * t;
if (t >= duration_) {
state_ = 2;
Expand Down
6 changes: 3 additions & 3 deletions src/oscillator.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ Oscillator::Oscillator(const std::string name)
vectorSoutSOUT_.setFunction(
boost::bind(&Oscillator::computeVectorSignal, this, _1, _2));
soutSOUT_.setNeedUpdateFromAllChildren(true);
soutSOUT_.setDependencyType(TimeDependency<int>::ALWAYS_READY);
soutSOUT_.setDependencyType(TimeDependency<sigtime_t>::ALWAYS_READY);

addCommand(
"setTimePeriod",
Expand Down Expand Up @@ -104,13 +104,13 @@ double Oscillator::value(double dt, double t, double omega, double phase,
}

dynamicgraph::Vector& Oscillator::computeVectorSignal(
dynamicgraph::Vector& vsout, const int& t) {
dynamicgraph::Vector& vsout, const sigtime_t& t) {
vsout.resize(1);
vsout(0) = soutSOUT_.access(t);
return vsout;
}

double& Oscillator::computeSignal(double& sout, const int& t) {
double& Oscillator::computeSignal(double& sout, const sigtime_t& t) {
double eps;

if (continuous_)
Expand Down
Loading