Skip to content

Commit 7f5a93e

Browse files
nim65snim65s
committed
Merge tag 'v4.11.1'
Release of version 4.11.1.
2 parents 283301d + b412444 commit 7f5a93e

File tree

105 files changed

+1932
-2158
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

105 files changed

+1932
-2158
lines changed

‎CMakeLists.txt

Lines changed: 9 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -37,20 +37,21 @@ CHECK_MINIMAL_CXX_STANDARD(11 ENFORCE)
3737
# Project dependencies
3838
ADD_PROJECT_DEPENDENCY(dynamic-graph REQUIRED PKG_CONFIG_REQUIRES dynamic-graph)
3939
ADD_PROJECT_DEPENDENCY(pinocchio REQUIRED PKG_CONFIG_REQUIRES pinocchio)
40-
ADD_PROJECT_DEPENDENCY(example-robot-data)
41-
SET(BOOST_COMPONENTS filesystem system thread program_options
42-
unit_test_framework regex)
40+
ADD_PROJECT_DEPENDENCY(Boost REQUIRED COMPONENTS regex)
41+
IF(BUILD_TESTING)
42+
ADD_PROJECT_DEPENDENCY(example-robot-data)
43+
FIND_PACKAGE(Boost REQUIRED COMPONENTS unit_test_framework program_options)
44+
ENDIF()
4345

4446
IF(BUILD_PYTHON_INTERFACE)
4547
FINDPYTHON()
4648
STRING(REGEX REPLACE "-" "_" PYTHON_DIR ${CUSTOM_HEADER_DIR})
47-
ADD_PROJECT_DEPENDENCY(dynamic-graph-python REQUIRED
49+
ADD_PROJECT_DEPENDENCY(dynamic-graph-python 4.0.0 REQUIRED
4850
PKG_CONFIG_REQUIRES dynamic-graph-python)
49-
SET(BOOST_COMPONENTS ${BOOST_COMPONENTS} python)
51+
ADD_PROJECT_DEPENDENCY(eigenpy REQUIRED PKG_CONFIG_REQUIRES eigenpy)
52+
SEARCH_FOR_BOOST_PYTHON(REQUIRED)
5053
ENDIF(BUILD_PYTHON_INTERFACE)
5154

52-
SEARCH_FOR_BOOST()
53-
5455
# Verbosity level
5556
IF(NOT (\"${CMAKE_VERBOSITY_LEVEL}\" STREQUAL \"\"))
5657
ADD_DEFINITIONS(-DVP_DEBUG_MODE=${CMAKE_VERBOSITY_LEVEL} -DVP_DEBUG)
@@ -172,9 +173,7 @@ SET(${PROJECT_NAME}_SOURCES
172173
ADD_LIBRARY(${PROJECT_NAME} SHARED
173174
${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS})
174175
TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} PUBLIC $<INSTALL_INTERFACE:include>)
175-
TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME}
176-
SYSTEM PUBLIC ${Boost_INCLUDE_DIRS})
177-
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${Boost_LIBRARIES}
176+
TARGET_LINK_LIBRARIES(${PROJECT_NAME} PUBLIC Boost::regex
178177
dynamic-graph::dynamic-graph pinocchio::pinocchio)
179178

180179
IF(SUFFIX_SO_VERSION)

‎cmake

‎include/sot/core/device.hh

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -97,6 +97,9 @@ public:
9797
void setTorqueBounds(const Vector &lower, const Vector &upper);
9898
/// \}
9999

100+
PeriodicCall& periodicCallBefore() { return periodicCallBefore_; }
101+
PeriodicCall& periodicCallAfter() { return periodicCallAfter_; }
102+
100103
public: /* --- DISPLAY --- */
101104
virtual void display(std::ostream &os) const;
102105
virtual void cmdDisplay();

‎include/sot/core/double-constant.hh

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -17,10 +17,10 @@ namespace dynamicgraph {
1717
namespace sot {
1818

1919
class DoubleConstant : public Entity {
20+
public:
2021
static const std::string CLASS_NAME;
2122
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
2223

23-
public:
2424
DoubleConstant(const std::string &name);
2525

2626
virtual ~DoubleConstant(void) {}

‎include/sot/core/feature-point6d.hh

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -97,15 +97,15 @@ public:
9797
int time);
9898

9999
/** Static Feature selection. */
100-
inline static Flags selectX(void) { return FLAG_LINE_1; }
101-
inline static Flags selectY(void) { return FLAG_LINE_2; }
102-
inline static Flags selectZ(void) { return FLAG_LINE_3; }
103-
inline static Flags selectRX(void) { return FLAG_LINE_4; }
104-
inline static Flags selectRY(void) { return FLAG_LINE_5; }
105-
inline static Flags selectRZ(void) { return FLAG_LINE_6; }
106-
107-
inline static Flags selectTranslation(void) { return Flags(7); }
108-
inline static Flags selectRotation(void) { return Flags(56); }
100+
inline static Flags selectX(void) { return Flags("100000"); }
101+
inline static Flags selectY(void) { return Flags("010000"); }
102+
inline static Flags selectZ(void) { return Flags("001000"); }
103+
inline static Flags selectRX(void) { return Flags("000100"); }
104+
inline static Flags selectRY(void) { return Flags("000010"); }
105+
inline static Flags selectRZ(void) { return Flags("000001"); }
106+
107+
inline static Flags selectTranslation(void) { return Flags("111000"); }
108+
inline static Flags selectRotation(void) { return Flags("000111"); }
109109

110110
virtual void display(std::ostream &os) const;
111111

‎include/sot/core/feature-pose.hh

Lines changed: 0 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -139,17 +139,6 @@ public:
139139
virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res,
140140
int time);
141141

142-
/** Static Feature selection. */
143-
inline static Flags selectX(void) { return FLAG_LINE_1; }
144-
inline static Flags selectY(void) { return FLAG_LINE_2; }
145-
inline static Flags selectZ(void) { return FLAG_LINE_3; }
146-
inline static Flags selectRX(void) { return FLAG_LINE_4; }
147-
inline static Flags selectRY(void) { return FLAG_LINE_5; }
148-
inline static Flags selectRZ(void) { return FLAG_LINE_6; }
149-
150-
inline static Flags selectTranslation(void) { return Flags(7); }
151-
inline static Flags selectRotation(void) { return Flags(56); }
152-
153142
virtual void display(std::ostream &os) const;
154143

155144
public:

‎include/sot/core/feature-visual-point.hh

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -80,8 +80,8 @@ public:
8080
int time);
8181

8282
/** Static Feature selection. */
83-
inline static Flags selectX(void) { return FLAG_LINE_1; }
84-
inline static Flags selectY(void) { return FLAG_LINE_2; }
83+
inline static Flags selectX(void) { return Flags("10"); }
84+
inline static Flags selectY(void) { return Flags("01"); }
8585

8686
virtual void display(std::ostream &os) const;
8787
};

‎include/sot/core/flags.hh

Lines changed: 9 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
#include <vector>
2020

2121
/* SOT */
22+
#include <dynamic-graph/signal-caster.h>
2223
#include "sot/core/api.hh"
2324

2425
/* --------------------------------------------------------------------- */
@@ -30,57 +31,37 @@ namespace sot {
3031

3132
class SOT_CORE_EXPORT Flags {
3233
protected:
33-
std::vector<char> flags;
34-
bool reverse;
35-
36-
char operator[](const unsigned int &i) const;
34+
std::vector<bool> flags;
35+
bool outOfRangeFlag;
3736

3837
public:
3938
Flags(const bool &b = false);
40-
Flags(const char &c);
41-
Flags(const int &c4);
39+
Flags(const char *flags);
40+
Flags(const std::vector<bool> &flags);
4241

43-
void add(const char &c);
44-
void add(const int &c4);
42+
void add(const bool &b);
4543

4644
Flags operator!(void) const;
4745
SOT_CORE_EXPORT friend Flags operator&(const Flags &f1, const Flags &f2);
4846
SOT_CORE_EXPORT friend Flags operator|(const Flags &f1, const Flags &f2);
4947
Flags &operator&=(const Flags &f2);
5048
Flags &operator|=(const Flags &f2);
5149

52-
SOT_CORE_EXPORT friend Flags operator&(const Flags &f1, const bool &b);
53-
SOT_CORE_EXPORT friend Flags operator|(const Flags &f1, const bool &b);
54-
Flags &operator&=(const bool &b);
55-
Flags &operator|=(const bool &b);
56-
5750
SOT_CORE_EXPORT friend std::ostream &operator<<(std::ostream &os,
5851
const Flags &fl);
59-
SOT_CORE_EXPORT friend char operator>>(const Flags &flags, const int &i);
6052
SOT_CORE_EXPORT friend std::istream &operator>>(std::istream &is, Flags &fl);
6153
bool operator()(const int &i) const;
6254

6355
operator bool(void) const;
6456

6557
void unset(const unsigned int &i);
6658
void set(const unsigned int &i);
67-
68-
public: /* Selec "matlab-style" : 1:15, 1:, :45 ... */
69-
static void readIndexMatlab(std::istream &iss, unsigned int &indexStart,
70-
unsigned int &indexEnd, bool &unspecifiedEnd);
71-
static Flags readIndexMatlab(std::istream &iss);
7259
};
7360

74-
SOT_CORE_EXPORT extern const Flags FLAG_LINE_1;
75-
SOT_CORE_EXPORT extern const Flags FLAG_LINE_2;
76-
SOT_CORE_EXPORT extern const Flags FLAG_LINE_3;
77-
SOT_CORE_EXPORT extern const Flags FLAG_LINE_4;
78-
SOT_CORE_EXPORT extern const Flags FLAG_LINE_5;
79-
SOT_CORE_EXPORT extern const Flags FLAG_LINE_6;
80-
SOT_CORE_EXPORT extern const Flags FLAG_LINE_7;
81-
SOT_CORE_EXPORT extern const Flags FLAG_LINE_8;
82-
8361
} // namespace sot
62+
63+
template <>
64+
struct signal_io<sot::Flags> : signal_io_unimplemented<sot::Flags> {};
8465
} // namespace dynamicgraph
8566

8667
#endif /* #ifndef __SOT_FLAGS_H */

‎include/sot/core/integrator-abstract.hh

Lines changed: 24 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -66,7 +66,7 @@ public:
6666
"pushDenomCoef",
6767
makeCommandVoid1(
6868
*this, &IntegratorAbstract::pushDenomCoef,
69-
docCommandVoid1("Push a new denomicator coefficient", typeName)));
69+
docCommandVoid1("Push a new denominator coefficient", typeName)));
7070

7171
addCommand(
7272
"popNumCoef",
@@ -75,7 +75,7 @@ public:
7575
addCommand(
7676
"popDenomCoef",
7777
makeCommandVoid0(*this, &IntegratorAbstract::popDenomCoef,
78-
docCommandVoid0("Pop a new denomicator coefficient")));
78+
docCommandVoid0("Pop a new denominator coefficient")));
7979
}
8080

8181
virtual ~IntegratorAbstract() {}
@@ -90,11 +90,33 @@ public:
9090
void popNumCoef() { numerator.pop_back(); }
9191
void popDenomCoef() { denominator.pop_back(); }
9292

93+
const std::vector<coefT>& numCoeffs() const { return numerator; }
94+
void numCoeffs(const std::vector<coefT>& coeffs) { numerator = coeffs; }
95+
96+
const std::vector<coefT>& denomCoeffs() const { return denominator; }
97+
void denomCoeffs(const std::vector<coefT>& coeffs) { denominator = coeffs; }
98+
9399
public:
94100
dynamicgraph::SignalPtr<sigT, int> SIN;
95101

96102
dynamicgraph::SignalTimeDependent<sigT, int> SOUT;
97103

104+
virtual void display(std::ostream &os) const
105+
{
106+
os << this->getClassName() << ": " << getName() << '\n'
107+
<< " ";
108+
if (numerator.empty() || denominator.empty()) {
109+
os << "ill-formed.";
110+
return;
111+
}
112+
os << numerator[0];
113+
for (std::size_t i = 1; i < numerator.size(); ++i)
114+
os << " + " << numerator[i] << " s^" << i;
115+
os << "\n " << denominator[0];
116+
for (std::size_t i = 1; i < denominator.size(); ++i)
117+
os << " + " << denominator[i] << " s^" << i;
118+
}
119+
98120
protected:
99121
std::vector<coefT> numerator;
100122
std::vector<coefT> denominator;

‎include/sot/core/integrator-euler.hh

Lines changed: 12 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -168,6 +168,18 @@ public:
168168
double getSamplingPeriod() const { return dt; }
169169

170170
void initialize() {
171+
if (denominator.empty() || numerator.empty())
172+
throw dynamicgraph::ExceptionSignal(
173+
dynamicgraph::ExceptionSignal::GENERIC,
174+
"The numerator or the denominator is empty.");
175+
176+
// Check that denominator.back is the identity
177+
if (!internal::integratorEulerCoeffIsIdentity(denominator.back()))
178+
throw dynamicgraph::ExceptionSignal(
179+
dynamicgraph::ExceptionSignal::GENERIC,
180+
"The coefficient of the highest order derivative of denominator "
181+
"should be 1 (the last pushDenomCoef should be the identity).");
182+
171183
std::size_t numsize = numerator.size();
172184
inputMemory.resize(numsize);
173185

@@ -181,13 +193,6 @@ public:
181193
for (std::size_t i = 0; i < denomsize; ++i) {
182194
outputMemory[i] = inputMemory[0];
183195
}
184-
185-
// Check that denominator.back is the identity
186-
if (!internal::integratorEulerCoeffIsIdentity(denominator.back()))
187-
throw dynamicgraph::ExceptionSignal(
188-
dynamicgraph::ExceptionSignal::GENERIC,
189-
"The coefficient of the highest order derivative of denominator "
190-
"should be 1 (the last pushDenomCoef should be the identity).");
191196
}
192197
};
193198

‎include/sot/core/mailbox.hh

Lines changed: 16 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -31,16 +31,21 @@
3131
namespace dynamicgraph {
3232
namespace sot {
3333

34-
template <class Object> class Mailbox : public dynamicgraph::Entity {
34+
namespace dg = dynamicgraph;
35+
36+
template <class Object> struct MailboxTimestampedObject
37+
{
38+
Object obj;
39+
struct timeval timestamp;
40+
};
41+
42+
template <class Object> class Mailbox : public dg::Entity {
3543
public:
3644
static const std::string CLASS_NAME;
3745
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
3846

3947
public:
40-
struct sotTimestampedObject {
41-
Object obj;
42-
struct timeval timestamp;
43-
};
48+
typedef MailboxTimestampedObject<Object> sotTimestampedObject;
4449

4550
public:
4651
Mailbox(const std::string &name);
@@ -61,12 +66,17 @@ protected:
6166
bool update;
6267

6368
public: /* --- SIGNALS --- */
64-
dynamicgraph::SignalTimeDependent<struct sotTimestampedObject, int> SOUT;
69+
dynamicgraph::SignalTimeDependent<sotTimestampedObject, int> SOUT;
6570
dynamicgraph::SignalTimeDependent<Object, int> objSOUT;
6671
dynamicgraph::SignalTimeDependent<struct timeval, int> timeSOUT;
6772
};
6873

6974
} /* namespace sot */
75+
76+
template <class Object> struct signal_io<sot::MailboxTimestampedObject<Object> >
77+
: signal_io_unimplemented<sot::MailboxTimestampedObject<Object> > {};
78+
79+
template <> struct signal_io<timeval> : signal_io_unimplemented<timeval > {};
7080
} /* namespace dynamicgraph */
7181

7282
#endif // #ifndef __SOT_MAILBOX_HH

‎include/sot/core/multi-bound.hh

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
#include <vector>
2020

2121
/* SOT */
22+
#include <dynamic-graph/signal-caster.h>
2223
#include "sot/core/api.hh"
2324
#include <sot/core/exception-task.hh>
2425

@@ -71,6 +72,8 @@ SOT_CORE_EXPORT std::ostream &operator<<(std::ostream &os,
7172
SOT_CORE_EXPORT std::istream &operator>>(std::istream &os, VectorMultiBound &v);
7273

7374
} /* namespace sot */
75+
76+
template <> struct signal_io<sot::MultiBound> : signal_io_unimplemented<sot::MultiBound> {};
7477
} /* namespace dynamicgraph */
7578

7679
#endif // #ifndef __SOT_MultiBound_H__

‎include/sot/core/periodic-call.hh

Lines changed: 0 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -77,9 +77,6 @@ public:
7777
void clear(void) { signalMap.clear(); }
7878

7979
void display(std::ostream &os) const;
80-
void addSpecificCommands(dynamicgraph::Entity &ent,
81-
dynamicgraph::Entity::CommandMap_t &commap,
82-
const std::string &prefix = "");
8380
};
8481

8582
} // namespace sot

0 commit comments

Comments
 (0)