From 2a9fbd70d4fc47ed41e0ad25fb90e241865a341b Mon Sep 17 00:00:00 2001
From: Guilhem Saurel <guilhem.saurel@laas.fr>
Date: Wed, 27 Jul 2022 22:44:21 +0200
Subject: [PATCH] pre-commit run -a
---
.gitignore | 2 +-
INSTALL | 2 +-
NEWS | 1 -
doc/pictures/feature.fig | 2 +-
doc/pictures/task.fig | 2 +-
.../core/abstract-sot-external-interface.hh | 25 +-
include/sot/core/additional-functions.hh | 6 +-
.../sot/core/admittance-control-op-point.hh | 18 +-
include/sot/core/binary-int-to-uint.hh | 7 +-
include/sot/core/binary-op.hh | 19 +-
include/sot/core/causal-filter.hh | 10 +-
include/sot/core/clamp-workspace.hh | 9 +-
include/sot/core/com-freezer.hh | 12 +-
include/sot/core/contiifstream.hh | 8 +-
include/sot/core/control-gr.hh | 23 +-
include/sot/core/control-pd.hh | 23 +-
include/sot/core/debug.hh | 160 ++++++------
include/sot/core/derivator-impl.hh | 12 +-
include/sot/core/derivator.hh | 31 +--
include/sot/core/device.hh | 28 +-
include/sot/core/double-constant.hh | 9 +-
include/sot/core/event.hh | 39 +--
include/sot/core/exception-abstract.hh | 30 +--
include/sot/core/exception-dynamic.hh | 7 +-
include/sot/core/exception-factory.hh | 9 +-
include/sot/core/exception-feature.hh | 5 +-
include/sot/core/exception-signal.hh | 7 +-
include/sot/core/exception-task.hh | 5 +-
include/sot/core/exception-tools.hh | 11 +-
include/sot/core/exp-moving-avg.hh | 7 +-
include/sot/core/factory.hh | 4 +-
include/sot/core/feature-1d.hh | 11 +-
include/sot/core/feature-abstract.hh | 83 +++---
include/sot/core/feature-generic.hh | 11 +-
include/sot/core/feature-joint-limits.hh | 13 +-
include/sot/core/feature-line-distance.hh | 11 +-
include/sot/core/feature-point6d-relative.hh | 13 +-
include/sot/core/feature-point6d.hh | 26 +-
include/sot/core/feature-pose.hh | 19 +-
include/sot/core/feature-pose.hxx | 37 ++-
include/sot/core/feature-posture.hh | 19 +-
include/sot/core/feature-task.hh | 11 +-
include/sot/core/feature-vector3.hh | 11 +-
include/sot/core/feature-visual-point.hh | 11 +-
include/sot/core/filter-differentiator.hh | 23 +-
include/sot/core/fir-filter-impl.hh | 18 +-
include/sot/core/fir-filter.hh | 111 ++++----
include/sot/core/flags.hh | 11 +-
include/sot/core/fwd.hh | 6 +-
include/sot/core/gain-adaptive.hh | 25 +-
include/sot/core/gain-hyperbolic.hh | 23 +-
include/sot/core/gradient-ascent.hh | 7 +-
include/sot/core/gripper-control.hh | 27 +-
include/sot/core/integrator-abstract-impl.hh | 18 +-
include/sot/core/integrator-abstract.hh | 9 +-
include/sot/core/integrator-euler-impl.hh | 20 +-
include/sot/core/integrator-euler.hh | 17 +-
include/sot/core/joint-limitator.hh | 9 +-
include/sot/core/joint-trajectory-entity.hh | 17 +-
include/sot/core/kalman.hh | 41 +--
include/sot/core/latch.hh | 13 +-
include/sot/core/macros-signal.hh | 152 +++++------
include/sot/core/macros.hh | 8 +-
include/sot/core/madgwickahrs.hh | 20 +-
include/sot/core/mailbox-vector.hh | 12 +-
include/sot/core/mailbox.hh | 21 +-
include/sot/core/mailbox.hxx | 52 ++--
include/sot/core/matrix-constant.hh | 10 +-
include/sot/core/matrix-geometry.hh | 48 ++--
include/sot/core/matrix-svd.hh | 5 +-
include/sot/core/memory-task-sot.hh | 18 +-
include/sot/core/motion-period.hh | 12 +-
include/sot/core/multi-bound.hh | 18 +-
include/sot/core/neck-limitation.hh | 20 +-
include/sot/core/op-point-modifier.hh | 11 +-
include/sot/core/parameter-server.hh | 37 +--
include/sot/core/periodic-call-entity.hh | 12 +-
include/sot/core/periodic-call.hh | 11 +-
include/sot/core/pool.hh | 12 +-
include/sot/core/reader.hh | 9 +-
include/sot/core/robot-simu.hh | 11 +-
include/sot/core/robot-utils.hh | 26 +-
include/sot/core/sequencer.hh | 27 +-
include/sot/core/smooth-reach.hh | 12 +-
include/sot/core/sot-loader.hh | 4 +-
include/sot/core/sot.hh | 23 +-
include/sot/core/stop-watch.hh | 22 +-
include/sot/core/switch.hh | 20 +-
include/sot/core/task-abstract.hh | 23 +-
include/sot/core/task-conti.hh | 11 +-
include/sot/core/task-pd.hh | 6 +-
include/sot/core/task-unilateral.hh | 11 +-
include/sot/core/task.hh | 11 +-
include/sot/core/time-stamp.hh | 13 +-
include/sot/core/timer.hh | 27 +-
include/sot/core/trajectory.hh | 92 ++++---
include/sot/core/unary-op.hh | 18 +-
include/sot/core/utils-windows.hh | 4 +-
include/sot/core/variadic-op.hh | 30 +--
include/sot/core/vector-constant.hh | 13 +-
include/sot/core/vector-to-rotation.hh | 10 +-
include/sot/core/visual-point-projecter.hh | 15 +-
src/control/admittance-control-op-point.cpp | 41 ++-
src/control/control-gr.cpp | 6 +-
src/control/control-pd.cpp | 20 +-
src/debug/contiifstream.cpp | 5 +-
src/debug/debug.cpp | 24 +-
src/dynamic_graph/__init__.py | 2 +-
.../sot/core/feature_position.py | 44 ++--
.../sot/core/feature_position_relative.py | 81 +++---
.../sot/core/math_small_entities.py | 64 ++++-
src/dynamic_graph/sot/core/matrix_util.py | 53 ++--
src/dynamic_graph/sot/core/meta_task_6d.py | 47 ++--
.../sot/core/meta_task_posture.py | 12 +-
.../sot/core/meta_task_visual_point.py | 52 ++--
src/dynamic_graph/sot/core/meta_tasks.py | 24 +-
src/dynamic_graph/sot/core/meta_tasks_kine.py | 11 +-
.../sot/core/meta_tasks_kine_relative.py | 65 +++--
src/dynamic_graph/sot/core/utils/attime.py | 27 +-
src/dynamic_graph/sot/core/utils/history.py | 63 +++--
.../core/utils/thread_interruptible_loop.py | 5 +-
.../sot/core/utils/viewer_helper.py | 40 +--
.../sot/core/utils/viewer_loger.py | 24 +-
src/exception/exception-abstract.cpp | 11 +-
src/exception/exception-dynamic.cpp | 3 +-
src/exception/exception-factory.cpp | 3 +-
src/exception/exception-feature.cpp | 3 +-
src/exception/exception-signal.cpp | 3 +-
src/exception/exception-task.cpp | 3 +-
src/exception/exception-tools.cpp | 3 +-
src/factory/additional-functions.cpp | 1 +
src/factory/pool.cpp | 9 +-
src/feature/feature-1d.cpp | 3 +-
src/feature/feature-abstract.cpp | 31 +--
src/feature/feature-generic.cpp | 18 +-
src/feature/feature-joint-limits-command.h | 16 +-
src/feature/feature-joint-limits.cpp | 14 +-
src/feature/feature-line-distance.cpp | 7 +-
src/feature/feature-point6d-relative.cpp | 21 +-
src/feature/feature-point6d.cpp | 138 +++++-----
src/feature/feature-pose.cpp | 16 +-
src/feature/feature-posture.cpp | 50 ++--
src/feature/feature-task.cpp | 4 +-
src/feature/feature-vector3.cpp | 15 +-
src/feature/feature-visual-point.cpp | 19 +-
src/feature/visual-point-projecter.cpp | 14 +-
src/filters/causal-filter.cpp | 16 +-
src/filters/filter-differentiator.cpp | 40 ++-
src/filters/madgwickahrs.cpp | 28 +-
src/math/op-point-modifier.cpp | 27 +-
src/math/vector-quaternion.cpp | 4 +-
src/matrix/derivator.cpp | 44 ++--
src/matrix/double-constant.cpp | 21 +-
src/matrix/fir-filter.cpp | 16 +-
.../integrator-euler-python-module-py.cc | 41 +--
src/matrix/integrator-euler.cpp | 5 +-
src/matrix/integrator-euler.t.cpp | 8 +-
src/matrix/matrix-constant-command.h | 18 +-
src/matrix/matrix-constant.cpp | 30 ++-
src/matrix/matrix-svd.cpp | 2 +-
src/matrix/operator-python-module-py.cc | 19 +-
src/matrix/operator.cpp | 40 +--
src/matrix/operator.hh | 247 +++++++++---------
src/matrix/vector-constant-command.h | 14 +-
src/matrix/vector-constant.cpp | 27 +-
src/matrix/vector-to-rotation.cpp | 55 ++--
src/python-module.cc | 39 +--
src/signal/signal-cast.cpp | 13 +-
src/sot/flags.cpp | 65 +++--
src/sot/memory-task-sot.cpp | 5 +-
src/sot/sot-command.h | 38 +--
src/sot/sot.cpp | 213 +++++++--------
src/task/gain-adaptive.cpp | 50 ++--
src/task/multi-bound.cpp | 131 +++++-----
src/task/task-abstract.cpp | 3 +-
src/task/task-command.h | 14 +-
src/task/task-conti.cpp | 22 +-
src/task/task-pd.cpp | 5 +-
src/task/task-unilateral.cpp | 11 +-
src/task/task.cpp | 47 ++--
src/tools/binary-int-to-uint.cpp | 8 +-
src/tools/clamp-workspace.cpp | 46 ++--
src/tools/com-freezer.cpp | 10 +-
src/tools/device.cpp | 202 +++++++-------
src/tools/event.cpp | 12 +-
src/tools/exp-moving-avg.cpp | 13 +-
src/tools/gradient-ascent.cpp | 3 +-
src/tools/gripper-control.cpp | 61 +++--
src/tools/joint-limitator.cpp | 3 +-
src/tools/joint-trajectory-command.hh | 14 +-
src/tools/joint-trajectory-entity.cpp | 52 ++--
src/tools/kalman.cpp | 33 +--
src/tools/latch.cpp | 4 +-
src/tools/mailbox-vector.cpp | 1 +
src/tools/motion-period.cpp | 63 ++---
src/tools/neck-limitation.cpp | 20 +-
src/tools/parameter-server.cpp | 49 ++--
src/tools/periodic-call-entity.cpp | 5 +-
src/tools/periodic-call.cpp | 3 +-
src/tools/robot-simu.cpp | 27 +-
src/tools/robot-utils-py.cpp | 1 -
src/tools/robot-utils.cpp | 45 ++--
src/tools/sequencer.cpp | 38 ++-
src/tools/smooth-reach.cpp | 36 +--
src/tools/sot-loader.cpp | 19 +-
src/tools/switch-python-module-py.cc | 6 +-
src/tools/switch.cpp | 14 +-
src/tools/time-stamp.cpp | 8 +-
src/tools/timer.cpp | 10 +-
src/tools/trajectory.cpp | 53 ++--
src/tools/type-name-helper.hh | 10 +-
src/tools/utils-windows.cpp | 3 +-
src/traces/reader.cpp | 32 +--
src/utils/stop-watch.cpp | 52 ++--
tests/CMakeLists.txt | 2 +-
tests/control/test_control_admittance.cpp | 1 +
tests/control/test_control_pd.cpp | 1 +
tests/factory/test_factory.cpp | 12 +-
tests/features/test_feature_generic.cpp | 141 +++++-----
tests/features/test_feature_point6d.cpp | 19 +-
tests/filters/test_filter_differentiator.cpp | 37 +--
tests/filters/test_madgwick_ahrs.cpp | 10 +-
tests/filters/test_madgwick_arhs.cpp | 34 +--
tests/math/matrix-homogeneous.cpp | 52 ++--
tests/math/matrix-twist.cpp | 22 +-
tests/matrix/test_operator.cpp | 61 +++--
tests/python/parameter_server_conf.py | 14 +-
tests/python/test-imports.py | 2 +-
tests/python/test-initialize-euler.py | 6 +-
tests/python/test-matrix-util.py | 20 +-
tests/python/test-op-point-modifier.py | 144 +++++++---
tests/python/test-parameter-server.py | 8 +-
tests/python/test-smooth-reach.py | 2 +-
tests/signal/test_dep.cpp | 15 +-
tests/signal/test_depend.cpp | 17 +-
tests/signal/test_ptr.cpp | 18 +-
tests/signal/test_ptrcast.cpp | 1 +
tests/signal/test_signal.cpp | 4 +-
tests/sot/test_solverSoth.cpp | 44 ++--
tests/sot/tsot.cpp | 4 +-
tests/task/test_flags.cpp | 3 +-
tests/task/test_gain.cpp | 3 +-
tests/task/test_task.cpp | 7 +-
tests/tools/dummy-sot-external-interface.cc | 6 +-
tests/tools/dummy-sot-external-interface.hh | 4 +-
tests/tools/plugin.cc | 8 +-
tests/tools/plugin.hh | 2 +-
tests/tools/test_abstract_interface.cpp | 8 +-
tests/tools/test_boost.cpp | 45 ++--
tests/tools/test_control_pd.cpp | 1 +
tests/tools/test_device.cpp | 16 +-
tests/tools/test_mailbox.cpp | 4 +-
tests/tools/test_matrix.cpp | 18 +-
tests/tools/test_robot_utils.cpp | 1 +
tests/tools/test_sot_loader.cpp | 31 +--
tests/traces/files.cpp | 6 +-
tests/traces/test_traces.cpp | 4 +-
257 files changed, 3385 insertions(+), 3023 deletions(-)
diff --git a/.gitignore b/.gitignore
index 264c367f..b8952e2c 100644
--- a/.gitignore
+++ b/.gitignore
@@ -4,4 +4,4 @@ include/sot-core/import-default-paths.h
unitTesting/test-paths.h
*~
_build*
-*__pycache__*
\ No newline at end of file
+*__pycache__*
diff --git a/INSTALL b/INSTALL
index 22534b78..2e93a0ec 100644
--- a/INSTALL
+++ b/INSTALL
@@ -9,7 +9,7 @@ It is recommended to create a specific directory to install this package.
mkdir build
cd build
cmake [Options] ..
-make
+make
make install
Options:
diff --git a/NEWS b/NEWS
index b079810e..5e381c14 100644
--- a/NEWS
+++ b/NEWS
@@ -163,4 +163,3 @@ New in version 1.1 - 2010/09/30
Packages libraries in Debian package.
Extend description of Debian package.
Add missing build dependencies.
-
diff --git a/doc/pictures/feature.fig b/doc/pictures/feature.fig
index 30af0c3a..ec1fdde1 100644
--- a/doc/pictures/feature.fig
+++ b/doc/pictures/feature.fig
@@ -2,7 +2,7 @@
Landscape
Center
Metric
-A4
+A4
100.00
Single
-2
diff --git a/doc/pictures/task.fig b/doc/pictures/task.fig
index 6a68b026..352e6923 100644
--- a/doc/pictures/task.fig
+++ b/doc/pictures/task.fig
@@ -2,7 +2,7 @@
Landscape
Center
Metric
-A4
+A4
100.00
Single
-2
diff --git a/include/sot/core/abstract-sot-external-interface.hh b/include/sot/core/abstract-sot-external-interface.hh
index 484ea3d3..9a139725 100644
--- a/include/sot/core/abstract-sot-external-interface.hh
+++ b/include/sot/core/abstract-sot-external-interface.hh
@@ -18,12 +18,11 @@ namespace dynamicgraph {
namespace sot {
class SOT_CORE_EXPORT NamedVector {
-
-private:
+ private:
std::string name_;
std::vector<double> values_;
-public:
+ public:
NamedVector() {}
~NamedVector() {}
@@ -39,30 +38,30 @@ typedef NamedVector SensorValues;
typedef NamedVector ControlValues;
class SOT_CORE_EXPORT AbstractSotExternalInterface {
-public:
+ public:
AbstractSotExternalInterface() {}
virtual ~AbstractSotExternalInterface() {}
- virtual void
- setupSetSensors(std::map<std::string, SensorValues> &sensorsIn) = 0;
+ virtual void setupSetSensors(
+ std::map<std::string, SensorValues> &sensorsIn) = 0;
- virtual void
- nominalSetSensors(std::map<std::string, SensorValues> &sensorsIn) = 0;
+ virtual void nominalSetSensors(
+ std::map<std::string, SensorValues> &sensorsIn) = 0;
- virtual void
- cleanupSetSensors(std::map<std::string, SensorValues> &sensorsIn) = 0;
+ virtual void cleanupSetSensors(
+ std::map<std::string, SensorValues> &sensorsIn) = 0;
virtual void getControl(std::map<std::string, ControlValues> &) = 0;
virtual void setSecondOrderIntegration(void) = 0;
virtual void setNoIntegration(void) = 0;
};
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
typedef dynamicgraph::sot::AbstractSotExternalInterface *
createSotExternalInterface_t();
typedef void destroySotExternalInterface_t(
dynamicgraph::sot::AbstractSotExternalInterface *);
-#endif // ABSTRACT_SOT_EXTERNAL_INTERFACE_HH
+#endif // ABSTRACT_SOT_EXTERNAL_INTERFACE_HH
diff --git a/include/sot/core/additional-functions.hh b/include/sot/core/additional-functions.hh
index c4f82e00..80bba781 100644
--- a/include/sot/core/additional-functions.hh
+++ b/include/sot/core/additional-functions.hh
@@ -8,12 +8,14 @@
*/
/* SOT */
-#include "sot/core/api.hh"
#include <dynamic-graph/pool.h>
#include <dynamic-graph/signal-base.h>
+
#include <sot/core/exception-factory.hh>
#include <sot/core/pool.hh>
+#include "sot/core/api.hh"
+
/* --- STD --- */
#include <map>
#include <sstream>
@@ -31,7 +33,7 @@ namespace sot {
to allow creation of tasks and features as well as entities.
*/
class AdditionalFunctions {
-public:
+ public:
AdditionalFunctions();
~AdditionalFunctions();
static void cmdFlagSet(const std::string &cmd, std::istringstream &args,
diff --git a/include/sot/core/admittance-control-op-point.hh b/include/sot/core/admittance-control-op-point.hh
index c703ecd3..79264703 100644
--- a/include/sot/core/admittance-control-op-point.hh
+++ b/include/sot/core/admittance-control-op-point.hh
@@ -31,12 +31,12 @@
#include <dynamic-graph/signal-helper.h>
+#include <sot/core/matrix-geometry.hh>
+
#include "pinocchio/spatial/force.hpp"
#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/se3.hpp"
-#include <sot/core/matrix-geometry.hh>
-
namespace dynamicgraph {
namespace sot {
namespace core {
@@ -60,7 +60,7 @@ class ADMITTANCECONTROLOPPOINT_EXPORT AdmittanceControlOpPoint
: public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
-public:
+ public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
@@ -106,7 +106,7 @@ public:
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream &os) const;
-protected:
+ protected:
/// Dimension of the force signals and of the output
int m_n;
/// True if the entity has been successfully initialized
@@ -118,10 +118,10 @@ protected:
// Weight of the end-effector
double m_mass;
-}; // class AdmittanceControlOpPoint
+}; // class AdmittanceControlOpPoint
-} // namespace core
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace core
+} // namespace sot
+} // namespace dynamicgraph
-#endif // #ifndef __sot_core_admittance_control_op_point_H__
+#endif // #ifndef __sot_core_admittance_control_op_point_H__
diff --git a/include/sot/core/binary-int-to-uint.hh b/include/sot/core/binary-int-to-uint.hh
index a073117f..cdf83032 100644
--- a/include/sot/core/binary-int-to-uint.hh
+++ b/include/sot/core/binary-int-to-uint.hh
@@ -17,6 +17,7 @@
/* SOT */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
+
#include <sot/core/exception-task.hh>
/* --------------------------------------------------------------------- */
@@ -37,16 +38,16 @@ namespace dynamicgraph {
namespace sot {
class SOTBINARYINTTOUINT_EXPORT BinaryIntToUint : public dynamicgraph::Entity {
-public:
+ public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
/* --- SIGNALS ------------------------------------------------------------ */
-public:
+ public:
dynamicgraph::SignalPtr<int, int> binaryIntSIN;
dynamicgraph::SignalTimeDependent<unsigned, int> binaryUintSOUT;
-public:
+ public:
BinaryIntToUint(const std::string &name);
virtual ~BinaryIntToUint() {}
diff --git a/include/sot/core/binary-op.hh b/include/sot/core/binary-op.hh
index 43d0277a..3f74a887 100644
--- a/include/sot/core/binary-op.hh
+++ b/include/sot/core/binary-op.hh
@@ -20,14 +20,14 @@
/* SOT */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
+
#include <sot/core/flags.hh>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/pool.hh>
/* STD */
-#include <string>
-
#include <boost/function.hpp>
+#include <string>
namespace dynamicgraph {
namespace sot {
@@ -36,14 +36,15 @@ namespace sot {
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-template <typename Operator> class BinaryOp : public Entity {
+template <typename Operator>
+class BinaryOp : public Entity {
Operator op;
typedef typename Operator::Tin1 Tin1;
typedef typename Operator::Tin2 Tin2;
typedef typename Operator::Tout Tout;
typedef BinaryOp<Operator> Self;
-public: /* --- CONSTRUCTION --- */
+ public: /* --- CONSTRUCTION --- */
static std::string getTypeIn1Name(void) { return Operator::nameTypeIn1(); }
static std::string getTypeIn2Name(void) { return Operator::nameTypeIn2(); }
static std::string getTypeOutName(void) { return Operator::nameTypeOut(); }
@@ -66,12 +67,12 @@ public: /* --- CONSTRUCTION --- */
virtual ~BinaryOp(void){};
-public: /* --- SIGNAL --- */
+ public: /* --- SIGNAL --- */
SignalPtr<Tin1, int> SIN1;
SignalPtr<Tin2, int> SIN2;
SignalTimeDependent<Tout, int> SOUT;
-protected:
+ protected:
Tout &computeOperation(Tout &res, int time) {
const Tin1 &x1 = SIN1(time);
const Tin2 &x2 = SIN2(time);
@@ -79,7 +80,7 @@ protected:
return res;
}
};
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
-#endif // #ifndef SOT_CORE_BINARYOP_HH
+#endif // #ifndef SOT_CORE_BINARYOP_HH
diff --git a/include/sot/core/causal-filter.hh b/include/sot/core/causal-filter.hh
index 8f3110d5..b372d9f8 100644
--- a/include/sot/core/causal-filter.hh
+++ b/include/sot/core/causal-filter.hh
@@ -43,7 +43,7 @@ namespace dynamicgraph {
namespace sot {
class CausalFilter {
-public:
+ public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/** --- CONSTRUCTOR ----
@@ -64,7 +64,7 @@ public:
void switch_filter(const Eigen::VectorXd &filter_numerator,
const Eigen::VectorXd &filter_denominator);
-private:
+ private:
/// sampling timestep of the input signal
double m_dt;
/// Size
@@ -84,7 +84,7 @@ private:
int m_pt_denominator;
Eigen::MatrixXd m_input_buffer;
Eigen::MatrixXd m_output_buffer;
-}; // class CausalFilter
-} // namespace sot
-} // namespace dynamicgraph
+}; // class CausalFilter
+} // namespace sot
+} // namespace dynamicgraph
#endif /* _SOT_CORE_CAUSAL_FILTER_H_ */
diff --git a/include/sot/core/clamp-workspace.hh b/include/sot/core/clamp-workspace.hh
index d1c2544c..1970068c 100644
--- a/include/sot/core/clamp-workspace.hh
+++ b/include/sot/core/clamp-workspace.hh
@@ -19,6 +19,7 @@
/* SOT */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
+
#include <sot/core/exception-task.hh>
#include <sot/core/matrix-geometry.hh>
@@ -44,19 +45,19 @@ namespace sot {
/* --------------------------------------------------------------------- */
class SOTCLAMPWORKSPACE_EXPORT ClampWorkspace : public dynamicgraph::Entity {
-public:
+ public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
/* --- SIGNALS ------------------------------------------------------------ */
-public:
+ public:
dynamicgraph::SignalPtr<MatrixHomogeneous, int> positionrefSIN;
dynamicgraph::SignalPtr<MatrixHomogeneous, int> positionSIN;
dynamicgraph::SignalTimeDependent<dynamicgraph::Matrix, int> alphaSOUT;
dynamicgraph::SignalTimeDependent<dynamicgraph::Matrix, int> alphabarSOUT;
dynamicgraph::SignalTimeDependent<MatrixHomogeneous, int> handrefSOUT;
-public:
+ public:
ClampWorkspace(const std::string &name);
virtual ~ClampWorkspace(void) {}
@@ -70,7 +71,7 @@ public:
virtual void display(std::ostream &) const;
-private:
+ private:
int timeUpdate;
dynamicgraph::Matrix alpha;
diff --git a/include/sot/core/com-freezer.hh b/include/sot/core/com-freezer.hh
index ab58d8dd..0beea541 100644
--- a/include/sot/core/com-freezer.hh
+++ b/include/sot/core/com-freezer.hh
@@ -43,29 +43,29 @@ namespace sot {
/* --------------------------------------------------------------------- */
class SOTCOMFREEZER_EXPORT CoMFreezer : public dynamicgraph::Entity {
-public:
+ public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName() const { return CLASS_NAME; }
-private:
+ private:
dynamicgraph::Vector m_lastCoM;
bool m_previousPGInProcess;
int m_lastStopTime;
-public: /* --- CONSTRUCTION --- */
+ public: /* --- CONSTRUCTION --- */
CoMFreezer(const std::string &name);
virtual ~CoMFreezer(void);
-public: /* --- SIGNAL --- */
+ public: /* --- SIGNAL --- */
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> CoMRefSIN;
dynamicgraph::SignalPtr<unsigned, int> PGInProcessSIN;
dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> freezedCoMSOUT;
-public: /* --- FUNCTION --- */
+ public: /* --- FUNCTION --- */
dynamicgraph::Vector &computeFreezedCoM(dynamicgraph::Vector &freezedCoM,
const int &time);
-public: /* --- PARAMS --- */
+ public: /* --- PARAMS --- */
virtual void display(std::ostream &os) const;
};
diff --git a/include/sot/core/contiifstream.hh b/include/sot/core/contiifstream.hh
index 3a3ebe82..8c239eec 100644
--- a/include/sot/core/contiifstream.hh
+++ b/include/sot/core/contiifstream.hh
@@ -33,7 +33,7 @@ namespace sot {
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOT_CORE_EXPORT Contiifstream {
-protected:
+ protected:
std::string filename;
std::streamoff cursor;
static const unsigned int BUFFER_SIZE = 256;
@@ -41,7 +41,7 @@ protected:
std::list<std::string> reader;
bool first;
-public: /* --- Constructor --- */
+ public: /* --- Constructor --- */
Contiifstream(const std::string &n = "");
~Contiifstream(void);
void open(const std::string &n) {
@@ -49,10 +49,10 @@ public: /* --- Constructor --- */
cursor = 0;
}
-public: /* --- READ FILE --- */
+ public: /* --- READ FILE --- */
bool loop(void);
-public: /* --- READ LIST --- */
+ public: /* --- READ LIST --- */
inline bool ready(void) { return 0 < reader.size(); }
std::string next(void);
};
diff --git a/include/sot/core/control-gr.hh b/include/sot/core/control-gr.hh
index 9a8f1409..4c72f405 100644
--- a/include/sot/core/control-gr.hh
+++ b/include/sot/core/control-gr.hh
@@ -44,40 +44,39 @@ namespace sot {
/* --------------------------------------------------------------------- */
class ControlGR_EXPORT ControlGR : public Entity {
-
-public: /* --- CONSTRUCTOR ---- */
+ public: /* --- CONSTRUCTOR ---- */
ControlGR(const std::string &name);
-public: /* --- INIT --- */
+ public: /* --- INIT --- */
void init(const double &step);
-public: /* --- CONSTANTS --- */
+ public: /* --- CONSTANTS --- */
/* Default values. */
- static const double TIME_STEP_DEFAULT; // = 0.001
+ static const double TIME_STEP_DEFAULT; // = 0.001
-public: /* --- ENTITY INHERITANCE --- */
+ public: /* --- ENTITY INHERITANCE --- */
static const std::string CLASS_NAME;
virtual void display(std::ostream &os) const;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
-protected:
+ protected:
/* Parameters of the torque-control function:
* tau = - A*qddot = g */
double TimeStep;
double _dimension;
-public: /* --- SIGNALS --- */
+ public: /* --- SIGNALS --- */
SignalPtr<dynamicgraph::Matrix, int> matrixASIN;
SignalPtr<dynamicgraph::Vector, int> accelerationSIN;
SignalPtr<dynamicgraph::Vector, int> gravitySIN;
SignalTimeDependent<dynamicgraph::Vector, int> controlSOUT;
-protected:
+ protected:
double &setsize(int dimension);
dynamicgraph::Vector &computeControl(dynamicgraph::Vector &tau, int t);
};
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
-#endif // #ifndef __SOT_Control_GR_HH__
+#endif // #ifndef __SOT_Control_GR_HH__
diff --git a/include/sot/core/control-pd.hh b/include/sot/core/control-pd.hh
index 31ad8056..a124ca12 100644
--- a/include/sot/core/control-pd.hh
+++ b/include/sot/core/control-pd.hh
@@ -44,28 +44,27 @@ namespace sot {
/* --------------------------------------------------------------------- */
class ControlPD_EXPORT ControlPD : public Entity {
-
-public: /* --- CONSTRUCTOR ---- */
+ public: /* --- CONSTRUCTOR ---- */
ControlPD(const std::string &name);
-public: /* --- INIT --- */
+ public: /* --- INIT --- */
void init(const double &step);
-public: /* --- CONSTANTS --- */
+ public: /* --- CONSTANTS --- */
/* Default values. */
- static const double TIME_STEP_DEFAULT; // = 0.001
+ static const double TIME_STEP_DEFAULT; // = 0.001
-public: /* --- ENTITY INHERITANCE --- */
+ public: /* --- ENTITY INHERITANCE --- */
static const std::string CLASS_NAME;
virtual void display(std::ostream &os) const;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
-protected:
+ protected:
/* Parameters of the torque-control function:
* tau = kp * (qd-q) + kd* (dqd-dq) */
double TimeStep;
-public: /* --- SIGNALS --- */
+ public: /* --- SIGNALS --- */
SignalPtr<dynamicgraph::Vector, int> KpSIN;
SignalPtr<dynamicgraph::Vector, int> KdSIN;
SignalPtr<dynamicgraph::Vector, int> positionSIN;
@@ -76,7 +75,7 @@ public: /* --- SIGNALS --- */
SignalTimeDependent<dynamicgraph::Vector, int> positionErrorSOUT;
SignalTimeDependent<dynamicgraph::Vector, int> velocityErrorSOUT;
-protected:
+ protected:
dynamicgraph::Vector &computeControl(dynamicgraph::Vector &tau, int t);
dynamicgraph::Vector position_error_;
dynamicgraph::Vector velocity_error_;
@@ -86,7 +85,7 @@ protected:
int t);
};
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
-#endif // #ifndef __SOT_Control_PD_HH__
+#endif // #ifndef __SOT_Control_PD_HH__
diff --git a/include/sot/core/debug.hh b/include/sot/core/debug.hh
index dee51e55..33bb0e17 100644
--- a/include/sot/core/debug.hh
+++ b/include/sot/core/debug.hh
@@ -9,33 +9,34 @@
#ifndef SOT_CORE_DEBUG_HH
#define SOT_CORE_DEBUG_HH
-#include "sot/core/api.hh"
#include <cstdarg>
#include <cstdio>
#include <fstream>
#include <sstream>
+#include "sot/core/api.hh"
+
#ifndef VP_DEBUG_MODE
#define VP_DEBUG_MODE 0
-#endif //! VP_DEBUG_MODE
+#endif //! VP_DEBUG_MODE
#ifndef VP_TEMPLATE_DEBUG_MODE
#define VP_TEMPLATE_DEBUG_MODE 0
-#endif //! VP_TEMPLATE_DEBUG_MODE
-
-#define SOT_COMMON_TRACES \
- do { \
- va_list arg; \
- va_start(arg, format); \
- vsnprintf(charbuffer, SIZE, format, arg); \
- va_end(arg); \
- outputbuffer << tmpbuffer.str() << charbuffer << std::endl; \
+#endif //! VP_TEMPLATE_DEBUG_MODE
+
+#define SOT_COMMON_TRACES \
+ do { \
+ va_list arg; \
+ va_start(arg, format); \
+ vsnprintf(charbuffer, SIZE, format, arg); \
+ va_end(arg); \
+ outputbuffer << tmpbuffer.str() << charbuffer << std::endl; \
} while (0)
namespace dynamicgraph {
namespace sot {
class SOT_CORE_EXPORT DebugTrace {
-public:
+ public:
static const int SIZE = 512;
std::stringstream tmpbuffer;
@@ -47,8 +48,7 @@ public:
DebugTrace(std::ostream &os) : outputbuffer(os) {}
inline void trace(const int level, const char *format, ...) {
- if (level <= traceLevel)
- SOT_COMMON_TRACES;
+ if (level <= traceLevel) SOT_COMMON_TRACES;
tmpbuffer.str("");
}
@@ -58,14 +58,12 @@ public:
}
inline void trace(const int level = -1) {
- if (level <= traceLevel)
- outputbuffer << tmpbuffer.str();
+ if (level <= traceLevel) outputbuffer << tmpbuffer.str();
tmpbuffer.str("");
}
inline void traceTemplate(const int level, const char *format, ...) {
- if (level <= traceLevelTemplate)
- SOT_COMMON_TRACES;
+ if (level <= traceLevelTemplate) SOT_COMMON_TRACES;
tmpbuffer.str("");
}
@@ -88,66 +86,66 @@ public:
SOT_CORE_EXPORT extern DebugTrace sotDEBUGFLOW;
SOT_CORE_EXPORT extern DebugTrace sotERRORFLOW;
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
#ifdef VP_DEBUG
-#define sotPREDEBUG \
+#define sotPREDEBUG \
__FILE__ << ": " << __FUNCTION__ << "(#" << __LINE__ << ") :"
-#define sotPREERROR \
+#define sotPREERROR \
"\t!! " << __FILE__ << ": " << __FUNCTION__ << "(#" << __LINE__ << ") :"
-#define sotDEBUG(level) \
- if ((level > VP_DEBUG_MODE) || \
- (!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good())) \
- ; \
- else \
+#define sotDEBUG(level) \
+ if ((level > VP_DEBUG_MODE) || \
+ (!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good())) \
+ ; \
+ else \
dynamicgraph::sot::sotDEBUGFLOW.outputbuffer << sotPREDEBUG
-#define sotDEBUGMUTE(level) \
- if ((level > VP_DEBUG_MODE) || \
- (!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good())) \
- ; \
- else \
+#define sotDEBUGMUTE(level) \
+ if ((level > VP_DEBUG_MODE) || \
+ (!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good())) \
+ ; \
+ else \
dynamicgraph::sot::sotDEBUGFLOW.outputbuffer
-#define sotERROR \
- if (!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good()) \
- ; \
- else \
+#define sotERROR \
+ if (!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good()) \
+ ; \
+ else \
dynamicgraph::sot::sotERRORFLOW.outputbuffer << sotPREERROR
-#define sotDEBUGF \
- if (!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good()) \
- ; \
- else \
- dynamicgraph::sot::sotDEBUGFLOW \
- .pre(dynamicgraph::sot::sotDEBUGFLOW.tmpbuffer << sotPREDEBUG, \
- VP_DEBUG_MODE) \
+#define sotDEBUGF \
+ if (!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good()) \
+ ; \
+ else \
+ dynamicgraph::sot::sotDEBUGFLOW \
+ .pre(dynamicgraph::sot::sotDEBUGFLOW.tmpbuffer << sotPREDEBUG, \
+ VP_DEBUG_MODE) \
.trace
-#define sotERRORF \
- if (!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good()) \
- ; \
- else \
+#define sotERRORF \
+ if (!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good()) \
+ ; \
+ else \
sot::sotERRORFLOW.pre(sot::sotERRORFLOW.tmpbuffer << sotPREERROR).trace
// TEMPLATE
-#define sotTDEBUG(level) \
- if ((level > VP_TEMPLATE_DEBUG_MODE) || \
- (!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good())) \
- ; \
- else \
+#define sotTDEBUG(level) \
+ if ((level > VP_TEMPLATE_DEBUG_MODE) || \
+ (!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good())) \
+ ; \
+ else \
dynamicgraph::sot::sotDEBUGFLOW.outputbuffer << sotPREDEBUG
-#define sotTDEBUGF \
- if (!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good()) \
- ; \
- else \
- dynamicgraph::sot::sotDEBUGFLOW \
- .pre(dynamicgraph::sot::sotDEBUGFLOW.tmpbuffer << sotPREDEBUG, \
- VP_TEMPLATE_DEBUG_MODE) \
+#define sotTDEBUGF \
+ if (!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good()) \
+ ; \
+ else \
+ dynamicgraph::sot::sotDEBUGFLOW \
+ .pre(dynamicgraph::sot::sotDEBUGFLOW.tmpbuffer << sotPREDEBUG, \
+ VP_TEMPLATE_DEBUG_MODE) \
.trace
namespace dynamicgraph {
@@ -157,22 +155,22 @@ inline bool sotDEBUG_ENABLE(const int &level) { return level <= VP_DEBUG_MODE; }
inline bool sotTDEBUG_ENABLE(const int &level) {
return level <= VP_TEMPLATE_DEBUG_MODE;
}
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
/* -------------------------------------------------------------------------- */
-#else // VP_DEBUG
-#define sotPREERROR \
+#else // VP_DEBUG
+#define sotPREERROR \
"\t!! " << __FILE__ << ": " << __FUNCTION__ << "(#" << __LINE__ << ") :"
-#define sotDEBUG(level) \
- if (1) \
- ; \
- else \
+#define sotDEBUG(level) \
+ if (1) \
+ ; \
+ else \
::dynamicgraph::sot::__null_stream()
-#define sotDEBUGMUTE(level) \
- if (1) \
- ; \
- else \
+#define sotDEBUGMUTE(level) \
+ if (1) \
+ ; \
+ else \
::dynamicgraph::sot::__null_stream()
#define sotERROR sotERRORFLOW.outputbuffer << sotPREERROR
@@ -188,27 +186,27 @@ inline std::ostream &__null_stream() {
static std::ostream os(NULL);
return os;
}
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
// TEMPLATE
-#define sotTDEBUG(level) \
- if (1) \
- ; \
- else \
+#define sotTDEBUG(level) \
+ if (1) \
+ ; \
+ else \
::dynamicgraph::sot::__null_stream()
namespace dynamicgraph {
namespace sot {
inline void sotTDEBUGF(const int, const char *, ...) {}
inline void sotTDEBUGF(const char *, ...) {}
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
#define sotDEBUG_ENABLE(level) false
#define sotTDEBUG_ENABLE(level) false
-#endif // VP_DEBUG
+#endif // VP_DEBUG
#define sotDEBUGIN(level) sotDEBUG(level) << "# In {" << std::endl
#define sotDEBUGOUT(level) sotDEBUG(level) << "# Out }" << std::endl
@@ -218,7 +216,7 @@ inline void sotTDEBUGF(const char *, ...) {}
#define sotTDEBUGOUT(level) sotTDEBUG(level) << "# Out }" << std::endl
#define sotTDEBUGINOUT(level) sotTDEBUG(level) << "# In/Out { }" << std::endl
-#endif //! #ifdef SOT_CORE_DEBUG_HH
+#endif //! #ifdef SOT_CORE_DEBUG_HH
// Local variables:
// c-basic-offset: 2
diff --git a/include/sot/core/derivator-impl.hh b/include/sot/core/derivator-impl.hh
index ba354eef..c5dc09c2 100644
--- a/include/sot/core/derivator-impl.hh
+++ b/include/sot/core/derivator-impl.hh
@@ -35,13 +35,13 @@ namespace dynamicgraph {
namespace sot {
#ifdef WIN32
-#define DECLARE_SPECIFICATION(className, sotSigType) \
- class DERIVATOR_EXPORT className : public Derivator<sotSigType> { \
- public: \
- className(const std::string &name); \
+#define DECLARE_SPECIFICATION(className, sotSigType) \
+ class DERIVATOR_EXPORT className : public Derivator<sotSigType> { \
+ public: \
+ className(const std::string &name); \
};
#else
-#define DECLARE_SPECIFICATION(className, sotSigType) \
+#define DECLARE_SPECIFICATION(className, sotSigType) \
typedef Derivator<sotSigType> className;
#endif
@@ -52,4 +52,4 @@ DECLARE_SPECIFICATION(DerivatorVectorQuaternion, VectorQuaternion)
} /* namespace sot */
} /* namespace dynamicgraph */
-#endif // #ifndef __SOT_DERIVATOR_H__
+#endif // #ifndef __SOT_DERIVATOR_H__
diff --git a/include/sot/core/derivator.hh b/include/sot/core/derivator.hh
index 82a7413a..ea72dcd9 100644
--- a/include/sot/core/derivator.hh
+++ b/include/sot/core/derivator.hh
@@ -20,6 +20,7 @@
/* SOT */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
+
#include <sot/core/flags.hh>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/pool.hh>
@@ -34,20 +35,23 @@ namespace sot {
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-template <class T> class Derivator : public dynamicgraph::Entity {
+template <class T>
+class Derivator : public dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
-protected:
+ protected:
T memory;
bool initialized;
double timestep;
- static const double TIMESTEP_DEFAULT; //= 1.;
+ static const double TIMESTEP_DEFAULT; //= 1.;
-public: /* --- CONSTRUCTION --- */
+ public: /* --- CONSTRUCTION --- */
static std::string getTypeName(void) { return "Unknown"; }
Derivator(const std::string &name)
- : dynamicgraph::Entity(name), memory(), initialized(false),
+ : dynamicgraph::Entity(name),
+ memory(),
+ initialized(false),
timestep(TIMESTEP_DEFAULT),
SIN(NULL, "sotDerivator<" + getTypeName() + ">(" + name + ")::input(" +
getTypeName() + ")::sin"),
@@ -63,20 +67,19 @@ public: /* --- CONSTRUCTION --- */
virtual ~Derivator(void){};
-public: /* --- SIGNAL --- */
+ public: /* --- SIGNAL --- */
dynamicgraph::SignalPtr<T, int> SIN;
dynamicgraph::SignalTimeDependent<T, int> SOUT;
dynamicgraph::Signal<double, int> timestepSIN;
-protected:
+ protected:
T &computeDerivation(T &res, int time) {
if (initialized) {
res = memory;
res *= -1;
memory = SIN(time);
res += memory;
- if (timestep != 1.)
- res *= (1. / timestep);
+ if (timestep != 1.) res *= (1. / timestep);
} else {
initialized = true;
memory = SIN(time);
@@ -88,16 +91,14 @@ protected:
};
// TODO Derivation of unit quaternion?
template <>
-VectorQuaternion &
-Derivator<VectorQuaternion>::computeDerivation(VectorQuaternion &res,
- int time) {
+VectorQuaternion &Derivator<VectorQuaternion>::computeDerivation(
+ VectorQuaternion &res, int time) {
if (initialized) {
res = memory;
res.coeffs() *= -1;
memory = SIN(time);
res.coeffs() += memory.coeffs();
- if (timestep != 1.)
- res.coeffs() *= (1. / timestep);
+ if (timestep != 1.) res.coeffs() *= (1. / timestep);
} else {
initialized = true;
memory = SIN(time);
@@ -110,4 +111,4 @@ Derivator<VectorQuaternion>::computeDerivation(VectorQuaternion &res,
} /* namespace sot */
} /* namespace dynamicgraph */
-#endif // #ifndef __SOT_DERIVATOR_H__
+#endif // #ifndef __SOT_DERIVATOR_H__
diff --git a/include/sot/core/device.hh b/include/sot/core/device.hh
index eef2d4b8..ff9c2834 100644
--- a/include/sot/core/device.hh
+++ b/include/sot/core/device.hh
@@ -19,12 +19,14 @@
#include <dynamic-graph/linear-algebra.h>
/* SOT */
-#include "sot/core/api.hh"
-#include "sot/core/periodic-call.hh"
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
+
#include <sot/core/matrix-geometry.hh>
+#include "sot/core/api.hh"
+#include "sot/core/periodic-call.hh"
+
namespace dynamicgraph {
namespace sot {
@@ -43,7 +45,7 @@ const std::string ControlInput_s[] = {"noInteg", "oneInteg", "twoInteg"};
/* --------------------------------------------------------------------- */
class SOT_CORE_EXPORT Device : public Entity {
-public:
+ public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
@@ -54,7 +56,7 @@ public:
FORCE_SIGNAL_LARM
};
-protected:
+ protected:
dynamicgraph::Vector state_;
dynamicgraph::Vector velocity_;
bool sanityCheck_;
@@ -74,7 +76,7 @@ protected:
Vector lowerVelocity_;
Vector lowerTorque_;
/// \}
-public:
+ public:
/* --- CONSTRUCTION --- */
Device(const std::string &name);
/* --- DESTRUCTION --- */
@@ -100,7 +102,7 @@ public:
PeriodicCall &periodicCallBefore() { return periodicCallBefore_; }
PeriodicCall &periodicCallAfter() { return periodicCallAfter_; }
-public: /* --- DISPLAY --- */
+ public: /* --- DISPLAY --- */
virtual void display(std::ostream &os) const;
virtual void cmdDisplay();
SOT_CORE_EXPORT friend std::ostream &operator<<(std::ostream &os,
@@ -109,7 +111,7 @@ public: /* --- DISPLAY --- */
return os;
}
-public: /* --- SIGNALS --- */
+ public: /* --- SIGNALS --- */
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> controlSIN;
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> attitudeSIN;
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> zmpSIN;
@@ -142,7 +144,7 @@ public: /* --- SIGNALS --- */
dynamicgraph::Signal<dynamicgraph::Vector, int> pseudoTorqueSOUT;
/// \}
-protected:
+ protected:
/// Compute roll pitch yaw angles of freeflyer joint.
void integrateRollPitchYaw(dynamicgraph::Vector &state,
const dynamicgraph::Vector &control, double dt);
@@ -162,20 +164,20 @@ protected:
/// the joint torques for the given acceleration.
virtual void integrate(const double &dt);
-protected:
+ protected:
/// Get freeflyer pose
const MatrixHomogeneous &freeFlyerPose() const;
-public:
+ public:
virtual void setRoot(const dynamicgraph::Matrix &root);
virtual void setRoot(const MatrixHomogeneous &worldMwaist);
-private:
+ private:
// Intermediate variable to avoid dynamic allocation
dynamicgraph::Vector forceZero6;
};
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
#endif /* #ifndef SOT_DEVICE_HH */
diff --git a/include/sot/core/double-constant.hh b/include/sot/core/double-constant.hh
index eb6fcc4d..bf43efe2 100644
--- a/include/sot/core/double-constant.hh
+++ b/include/sot/core/double-constant.hh
@@ -10,14 +10,13 @@
#define DYNAMICGRAPH_SOT_DOUBLE_CONSTANT_H
#include <dynamic-graph/entity.h>
-
#include <dynamic-graph/signal-time-dependent.h>
namespace dynamicgraph {
namespace sot {
class DoubleConstant : public Entity {
-public:
+ public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
@@ -31,7 +30,7 @@ public:
void setValue(const double &inValue);
};
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
-#endif // DYNAMICGRAPH_SOT_DOUBLE_CONSTANT_H
+#endif // DYNAMICGRAPH_SOT_DOUBLE_CONSTANT_H
diff --git a/include/sot/core/event.hh b/include/sot/core/event.hh
index 64633611..c2196aff 100644
--- a/include/sot/core/event.hh
+++ b/include/sot/core/event.hh
@@ -43,31 +43,34 @@ class SOT_CORE_DLLAPI Event : public dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
Event(const std::string &name)
- : Entity(name), checkSOUT("Event(" + name + ")::output(bool)::check"),
+ : Entity(name),
+ checkSOUT("Event(" + name + ")::output(bool)::check"),
conditionSIN(NULL, "Event(" + name + ")::input(bool)::condition"),
- lastVal_(2), // lastVal_ should be different true and false.
- timeSinceUp_(0), repeatAfterNIterations_(0)
- {
+ lastVal_(2), // lastVal_ should be different true and false.
+ timeSinceUp_(0),
+ repeatAfterNIterations_(0) {
checkSOUT.setFunction(boost::bind(&Event::check, this, _1, _2));
signalRegistration(conditionSIN);
signalRegistration(checkSOUT);
using command::makeCommandVoid1;
- std::string docstring = "\n"
- " Add a signal\n";
+ std::string docstring =
+ "\n"
+ " Add a signal\n";
addCommand("addSignal",
makeCommandVoid1(*this, &Event::addSignal, docstring));
- docstring = "\n"
- " Get list of signals\n";
+ docstring =
+ "\n"
+ " Get list of signals\n";
addCommand("list", new command::Getter<Event, std::string>(
*this, &Event::getSignalsByName, docstring));
docstring =
- "\n"
- " Repease event if input signal remains True for a while\n"
- " Input: number of iterations before repeating output\n."
- " 0 for no repetition";
+ "\n"
+ " Repease event if input signal remains True for a while\n"
+ " Input: number of iterations before repeating output\n."
+ " 0 for no repetition";
addCommand("repeat", new command::Setter<Event, int>(*this, &Event::repeat,
docstring));
}
@@ -97,11 +100,11 @@ class SOT_CORE_DLLAPI Event : public dynamicgraph::Entity {
return oss.str();
}
- void repeat(const int& nbIterations)
- {
+ void repeat(const int &nbIterations) {
repeatAfterNIterations_ = nbIterations;
}
-private:
+
+ private:
typedef SignalBase<int> *Trigger_t;
typedef std::vector<Trigger_t> Triggers_t;
@@ -115,6 +118,6 @@ private:
bool lastVal_;
int timeSinceUp_, repeatAfterNIterations_;
};
-} // namespace sot
-} // namespace dynamicgraph
-#endif // __SOT_EVENT_H__
+} // namespace sot
+} // namespace dynamicgraph
+#endif // __SOT_EVENT_H__
diff --git a/include/sot/core/exception-abstract.hh b/include/sot/core/exception-abstract.hh
index ec1de2cb..dc9c9071 100644
--- a/include/sot/core/exception-abstract.hh
+++ b/include/sot/core/exception-abstract.hh
@@ -15,11 +15,12 @@
/* --------------------------------------------------------------------- */
/* Classes standards. */
-#include "sot/core/api.hh"
#include <exception>
#include <ostream> /* Classe ostream. */
#include <string> /* Classe string. */
+#include "sot/core/api.hh"
+
// Uncomment this macros to have lines parameter on the throw display
// #define SOT_EXCEPTION_PASSING_PARAM
@@ -33,8 +34,7 @@ namespace sot {
/* \class ExceptionAbstract
*/
class SOT_CORE_EXPORT ExceptionAbstract : public std::exception {
-
-public:
+ public:
enum ExceptionEnum {
ABSTRACT = 0,
SIGNAL = 100,
@@ -52,7 +52,7 @@ public:
return EXCEPTION_NAME;
}
-protected:
+ protected:
/** Error code.
* \sa ErrorCodeEnum */
int code;
@@ -60,11 +60,11 @@ protected:
/** Error message (can be empty). */
std::string message;
-private:
+ private:
/** forbid the empty constructor (private). */
ExceptionAbstract(void);
-public:
+ public:
ExceptionAbstract(const int &code, const std::string &msg = "");
virtual ~ExceptionAbstract(void) throw() {}
@@ -85,9 +85,9 @@ public:
const ExceptionAbstract &err);
#ifdef SOT_EXCEPTION_PASSING_PARAM
-public:
+ public:
class Param {
- public:
+ public:
static const int BUFFER_SIZE = 80;
const char *functionPTR;
@@ -97,13 +97,13 @@ public:
char file[BUFFER_SIZE];
bool pointersSet, set;
- public:
+ public:
Param(const int &_line, const char *_function, const char *_file);
Param(void) : pointersSet(false), set(false) {}
Param &initCopy(const Param &p);
};
-protected:
+ protected:
mutable Param p;
template <class Exc>
@@ -116,18 +116,18 @@ protected:
e.p.initCopy(p);
return e;
}
-#endif //#ifdef SOT_EXCEPTION_PASSING_PARAM
+#endif //#ifdef SOT_EXCEPTION_PASSING_PARAM
};
-#define SOT_RETHROW \
+#define SOT_RETHROW \
(const ExceptionAbstract &err) { throw err; }
#ifdef SOT_EXCEPTION_PASSING_PARAM
-#define SOT_THROW \
+#define SOT_THROW \
throw ExceptionAbstract::Param(__LINE__, __FUNCTION__, __FILE__) +
-#else //#ifdef SOT_EXCEPTION_PASSING_PARAM
+#else //#ifdef SOT_EXCEPTION_PASSING_PARAM
#define SOT_THROW throw
-#endif //#ifdef SOT_EXCEPTION_PASSING_PARAM
+#endif //#ifdef SOT_EXCEPTION_PASSING_PARAM
} /* namespace sot */
} /* namespace dynamicgraph */
diff --git a/include/sot/core/exception-dynamic.hh b/include/sot/core/exception-dynamic.hh
index 58d78fd6..30541528 100644
--- a/include/sot/core/exception-dynamic.hh
+++ b/include/sot/core/exception-dynamic.hh
@@ -14,8 +14,9 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#include "sot/core/api.hh"
#include <sot/core/exception-abstract.hh>
+
+#include "sot/core/api.hh"
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
@@ -28,7 +29,7 @@ namespace sot {
class SOT_CORE_EXPORT ExceptionDynamic : public ExceptionAbstract
{
-public:
+ public:
enum ErrorCodeEnum {
GENERIC = ExceptionAbstract::DYNAMIC
@@ -45,7 +46,7 @@ public:
return EXCEPTION_NAME;
}
-public:
+ public:
ExceptionDynamic(const ExceptionDynamic::ErrorCodeEnum &errcode,
const std::string &msg = "");
ExceptionDynamic(const ExceptionDynamic::ErrorCodeEnum &errcode,
diff --git a/include/sot/core/exception-factory.hh b/include/sot/core/exception-factory.hh
index e863bbef..c881b10a 100644
--- a/include/sot/core/exception-factory.hh
+++ b/include/sot/core/exception-factory.hh
@@ -14,8 +14,9 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#include "sot/core/api.hh"
#include <sot/core/exception-abstract.hh>
+
+#include "sot/core/api.hh"
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
@@ -28,7 +29,7 @@ namespace sot {
class SOT_CORE_EXPORT ExceptionFactory : public ExceptionAbstract
{
-public:
+ public:
enum ErrorCodeEnum {
GENERIC = ExceptionAbstract::FACTORY,
UNREFERED_OBJECT,
@@ -38,8 +39,8 @@ public:
SIGNAL_CONFLICT,
FUNCTION_CONFLICT,
OBJECT_CONFLICT,
- SYNTAX_ERROR // j' aime bien FATAL_ERROR aussi faut que je la case qq
- // part...
+ SYNTAX_ERROR // j' aime bien FATAL_ERROR aussi faut que je la case qq
+ // part...
,
READ_FILE
};
diff --git a/include/sot/core/exception-feature.hh b/include/sot/core/exception-feature.hh
index 02ae7688..0ea70979 100644
--- a/include/sot/core/exception-feature.hh
+++ b/include/sot/core/exception-feature.hh
@@ -14,8 +14,9 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#include "sot/core/api.hh"
#include <sot/core/exception-abstract.hh>
+
+#include "sot/core/api.hh"
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
@@ -28,7 +29,7 @@ namespace sot {
class SOT_CORE_EXPORT ExceptionFeature : public ExceptionAbstract
{
-public:
+ public:
enum ErrorCodeEnum {
GENERIC = ExceptionAbstract::FEATURE,
BAD_INIT,
diff --git a/include/sot/core/exception-signal.hh b/include/sot/core/exception-signal.hh
index 800a8bda..3e7eb628 100644
--- a/include/sot/core/exception-signal.hh
+++ b/include/sot/core/exception-signal.hh
@@ -14,8 +14,9 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#include "sot/core/api.hh"
#include <sot/core/exception-abstract.hh>
+
+#include "sot/core/api.hh"
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
@@ -28,7 +29,7 @@ namespace sot {
class SOT_CORE_EXPORT ExceptionSignal : public ExceptionAbstract
{
-public:
+ public:
enum ErrorCodeEnum {
GENERIC = ExceptionAbstract::SIGNAL
@@ -46,7 +47,7 @@ public:
return EXCEPTION_NAME;
}
-public:
+ public:
ExceptionSignal(const ExceptionSignal::ErrorCodeEnum &errcode,
const std::string &msg = "");
ExceptionSignal(const ExceptionSignal::ErrorCodeEnum &errcode,
diff --git a/include/sot/core/exception-task.hh b/include/sot/core/exception-task.hh
index e8f9a0be..e8fdc09c 100644
--- a/include/sot/core/exception-task.hh
+++ b/include/sot/core/exception-task.hh
@@ -14,8 +14,9 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#include "sot/core/api.hh"
#include <sot/core/exception-abstract.hh>
+
+#include "sot/core/api.hh"
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
@@ -28,7 +29,7 @@ namespace sot {
class SOT_CORE_EXPORT ExceptionTask : public ExceptionAbstract
{
-public:
+ public:
enum ErrorCodeEnum {
GENERIC = ExceptionAbstract::TASK,
EMPTY_LIST,
diff --git a/include/sot/core/exception-tools.hh b/include/sot/core/exception-tools.hh
index 29600e17..329d85c4 100644
--- a/include/sot/core/exception-tools.hh
+++ b/include/sot/core/exception-tools.hh
@@ -14,8 +14,9 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#include "sot/core/api.hh"
#include <sot/core/exception-abstract.hh>
+
+#include "sot/core/api.hh"
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
@@ -28,7 +29,7 @@ namespace sot {
class SOT_CORE_EXPORT ExceptionTools : public ExceptionAbstract
{
-public:
+ public:
enum ErrorCodeEnum {
GENERIC = ExceptionAbstract::TOOLS
@@ -41,7 +42,7 @@ public:
static const std::string EXCEPTION_NAME;
virtual const std::string &getExceptionName() const { return EXCEPTION_NAME; }
-public:
+ public:
ExceptionTools(const ExceptionTools::ErrorCodeEnum &errcode,
const std::string &msg = "");
ExceptionTools(const ExceptionTools::ErrorCodeEnum &errcode,
@@ -49,8 +50,8 @@ public:
virtual ~ExceptionTools(void) throw() {}
};
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
#endif /* #ifndef __SOT_TOOLS_EXCEPTION_H */
diff --git a/include/sot/core/exp-moving-avg.hh b/include/sot/core/exp-moving-avg.hh
index de48d5f3..c845d2cb 100644
--- a/include/sot/core/exp-moving-avg.hh
+++ b/include/sot/core/exp-moving-avg.hh
@@ -16,6 +16,7 @@
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
+
#include <sot/core/config.hh>
namespace dynamicgraph {
@@ -32,18 +33,18 @@ using dynamicgraph::SignalTimeDependent;
class SOT_CORE_DLLAPI ExpMovingAvg : public Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
-public:
+ public:
SignalPtr<dynamicgraph::Vector, int> updateSIN;
SignalTimeDependent<int, int> refresherSINTERN;
SignalTimeDependent<dynamicgraph::Vector, int> averageSOUT;
-public:
+ public:
ExpMovingAvg(const std::string &n);
virtual ~ExpMovingAvg(void);
void setAlpha(const double &alpha_);
-protected:
+ protected:
dynamicgraph::Vector &update(dynamicgraph::Vector &res, const int &inTime);
dynamicgraph::Vector average;
diff --git a/include/sot/core/factory.hh b/include/sot/core/factory.hh
index 89e9edcb..2b7b9991 100644
--- a/include/sot/core/factory.hh
+++ b/include/sot/core/factory.hh
@@ -17,13 +17,13 @@
#include <dynamic-graph/factory.h>
-#define SOT_FACTORY_FEATURE_PLUGIN(sotFeatureType, className) \
+#define SOT_FACTORY_FEATURE_PLUGIN(sotFeatureType, className) \
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(sotFeatureType, className)
/* -------------------------------------------------------------------------- */
/* --- TASK REGISTERER ------------------------------------------------------ */
/* -------------------------------------------------------------------------- */
-#define SOT_FACTORY_TASK_PLUGIN(sotTaskType, className) \
+#define SOT_FACTORY_TASK_PLUGIN(sotTaskType, className) \
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(sotTaskType, className)
#endif /* #ifndef __SOT_FACTORY_HH__ */
diff --git a/include/sot/core/feature-1d.hh b/include/sot/core/feature-1d.hh
index ee7808d9..02791c57 100644
--- a/include/sot/core/feature-1d.hh
+++ b/include/sot/core/feature-1d.hh
@@ -47,16 +47,15 @@ namespace sot {
*/
class SOTFEATURE1D_EXPORT Feature1D : public FeatureAbstract,
FeatureReferenceHelper<Feature1D> {
-
-public:
+ public:
/*! Field storing the class name. */
static const std::string CLASS_NAME;
/*! Returns the name of the class. */
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
-protected:
+ protected:
/* --- SIGNALS ------------------------------------------------------------ */
-public:
+ public:
/*! \name Signals
@{
*/
@@ -82,7 +81,7 @@ public:
* feature. */
using FeatureAbstract::errorSOUT;
-public:
+ public:
/*! \brief Default constructor */
Feature1D(const std::string &name);
@@ -120,7 +119,7 @@ public:
} /* namespace sot */
} /* namespace dynamicgraph */
-#endif // #ifndef __SOT_FEATURE_1D_HH__
+#endif // #ifndef __SOT_FEATURE_1D_HH__
/*
* Local variables:
diff --git a/include/sot/core/feature-abstract.hh b/include/sot/core/feature-abstract.hh
index 1c77d316..18fea959 100644
--- a/include/sot/core/feature-abstract.hh
+++ b/include/sot/core/feature-abstract.hh
@@ -18,12 +18,14 @@
#include <dynamic-graph/linear-algebra.h>
/* SOT */
-#include "sot/core/api.hh"
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
+
#include <sot/core/flags.hh>
#include <sot/core/pool.hh>
+#include "sot/core/api.hh"
+
/* STD */
#include <string>
@@ -72,7 +74,7 @@ namespace sot {
*/
class SOT_CORE_EXPORT FeatureAbstract : public Entity {
-public:
+ public:
/*! \brief Store the name of the class. */
static const std::string CLASS_NAME;
@@ -84,7 +86,7 @@ public:
void initCommands(void);
-public:
+ public:
/*! \brief Default constructor: the name of the class should be given. */
FeatureAbstract(const std::string &name);
/*! \brief Default destructor */
@@ -156,7 +158,7 @@ public:
/*! @} */
/* --- SIGNALS ------------------------------------------------------------ */
-public:
+ public:
/*! \name Signals
@{
*/
@@ -204,7 +206,7 @@ public:
/*! @} */
/* --- REFERENCE VALUE S* ------------------------------------------------- */
-public:
+ public:
/*! \name Reference
@{
*/
@@ -223,11 +225,12 @@ public:
/*! @} */
};
-template <class FeatureSpecialized> class FeatureReferenceHelper {
+template <class FeatureSpecialized>
+class FeatureReferenceHelper {
FeatureSpecialized *ptr;
FeatureAbstract *ptrA;
-public:
+ public:
FeatureReferenceHelper(void) : ptr(NULL) {}
void setReference(FeatureAbstract *sdes);
@@ -245,44 +248,42 @@ void FeatureReferenceHelper<FeatureSpecialized>::setReference(
ptrA = ptr;
}
-#define DECLARE_REFERENCE_FUNCTIONS(FeatureSpecialized) \
- typedef FeatureReferenceHelper<FeatureSpecialized> SP; \
- virtual void setReference(FeatureAbstract *sdes) { \
- if (sdes == NULL) { \
- /* UNSET */ \
- if (SP::isReferenceSet()) \
- removeDependenciesFromReference(); \
- SP::unsetReference(); \
- } else { \
- /* SET */ \
- SP::setReference(sdes); \
- if (SP::isReferenceSet()) \
- addDependenciesFromReference(); \
- } \
- } \
- virtual const FeatureAbstract *getReferenceAbstract(void) const { \
- return SP::getReference(); \
- } \
- virtual FeatureAbstract *getReferenceAbstract(void) { \
- return (FeatureAbstract *)SP::getReference(); \
- } \
- bool isReferenceSet(void) const { return SP::isReferenceSet(); } \
- virtual void addDependenciesFromReference(void); \
+#define DECLARE_REFERENCE_FUNCTIONS(FeatureSpecialized) \
+ typedef FeatureReferenceHelper<FeatureSpecialized> SP; \
+ virtual void setReference(FeatureAbstract *sdes) { \
+ if (sdes == NULL) { \
+ /* UNSET */ \
+ if (SP::isReferenceSet()) removeDependenciesFromReference(); \
+ SP::unsetReference(); \
+ } else { \
+ /* SET */ \
+ SP::setReference(sdes); \
+ if (SP::isReferenceSet()) addDependenciesFromReference(); \
+ } \
+ } \
+ virtual const FeatureAbstract *getReferenceAbstract(void) const { \
+ return SP::getReference(); \
+ } \
+ virtual FeatureAbstract *getReferenceAbstract(void) { \
+ return (FeatureAbstract *)SP::getReference(); \
+ } \
+ bool isReferenceSet(void) const { return SP::isReferenceSet(); } \
+ virtual void addDependenciesFromReference(void); \
virtual void removeDependenciesFromReference(void)
/* END OF define DECLARE_REFERENCE_FUNCTIONS */
-#define DECLARE_NO_REFERENCE \
- virtual void setReference(FeatureAbstract *) {} \
- virtual const FeatureAbstract *getReferenceAbstract(void) const { \
- return NULL; \
- } \
- virtual FeatureAbstract *getReferenceAbstract(void) { return NULL; } \
- virtual void addDependenciesFromReference(void) {} \
- virtual void removeDependenciesFromReference(void) {} \
+#define DECLARE_NO_REFERENCE \
+ virtual void setReference(FeatureAbstract *) {} \
+ virtual const FeatureAbstract *getReferenceAbstract(void) const { \
+ return NULL; \
+ } \
+ virtual FeatureAbstract *getReferenceAbstract(void) { return NULL; } \
+ virtual void addDependenciesFromReference(void) {} \
+ virtual void removeDependenciesFromReference(void) {} \
/* To force a ; */ bool NO_REFERENCE
/* END OF define DECLARE_REFERENCE_FUNCTIONS */
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
-#endif // #ifndef __SOT_FEATURE_ABSTRACT_H__
+#endif // #ifndef __SOT_FEATURE_ABSTRACT_H__
diff --git a/include/sot/core/feature-generic.hh b/include/sot/core/feature-generic.hh
index 2f1d0067..70ad7154 100644
--- a/include/sot/core/feature-generic.hh
+++ b/include/sot/core/feature-generic.hh
@@ -56,18 +56,17 @@ namespace sot {
class SOTFEATUREGENERIC_EXPORT FeatureGeneric
: public FeatureAbstract,
FeatureReferenceHelper<FeatureGeneric> {
-
-public:
+ public:
/*! Field storing the class name. */
static const std::string CLASS_NAME;
/*! Returns the name of the class. */
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
-protected:
+ protected:
dynamicgraph::Vector::Index dimensionDefault;
/* --- SIGNALS ------------------------------------------------------------ */
-public:
+ public:
/*! \name dynamicgraph::Signals
@{
*/
@@ -93,7 +92,7 @@ public:
feature. */
using FeatureAbstract::errorSOUT;
-public:
+ public:
/*! \brief Default constructor */
FeatureGeneric(const std::string &name);
@@ -131,7 +130,7 @@ public:
} /* namespace sot */
} /* namespace dynamicgraph */
-#endif // #ifndef __SOT_FEATURE_GENERIC_HH__
+#endif // #ifndef __SOT_FEATURE_GENERIC_HH__
/*
* Local variables:
diff --git a/include/sot/core/feature-joint-limits.hh b/include/sot/core/feature-joint-limits.hh
index 7c670d61..33dd27c9 100644
--- a/include/sot/core/feature-joint-limits.hh
+++ b/include/sot/core/feature-joint-limits.hh
@@ -46,21 +46,20 @@ namespace sot {
class SOTFEATUREJOINTLIMITS_EXPORT FeatureJointLimits
: public FeatureAbstract,
FeatureReferenceHelper<FeatureJointLimits> {
-
-public:
+ public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
-protected:
+ protected:
double threshold;
- const static double THRESHOLD_DEFAULT; // = .9;
+ const static double THRESHOLD_DEFAULT; // = .9;
/* unsigned int freeFloatingIndex,freeFloatingSize; */
/* static const unsigned int FREE_FLOATING_INDEX = 0; */
/* static const unsigned int FREE_FLOATING_SIZE = 5; */
/* --- SIGNALS ------------------------------------------------------------ */
-public:
+ public:
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> jointSIN;
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> upperJlSIN;
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> lowerJlSIN;
@@ -77,7 +76,7 @@ public:
DECLARE_REFERENCE_FUNCTIONS(FeatureJointLimits);
/*! @} */
-public:
+ public:
FeatureJointLimits(const std::string &name);
virtual ~FeatureJointLimits(void) {}
@@ -99,7 +98,7 @@ public:
} /* namespace sot */
} /* namespace dynamicgraph */
-#endif // #ifndef __SOT_FEATURE_JOINTLIMITS_HH__
+#endif // #ifndef __SOT_FEATURE_JOINTLIMITS_HH__
/*
* Local variables:
diff --git a/include/sot/core/feature-line-distance.hh b/include/sot/core/feature-line-distance.hh
index 6aa7002f..ac124d32 100644
--- a/include/sot/core/feature-line-distance.hh
+++ b/include/sot/core/feature-line-distance.hh
@@ -45,14 +45,13 @@ namespace sot {
*/
class SOTFEATURELINEDISTANCE_EXPORT FeatureLineDistance
: public FeatureAbstract {
-
-public:
+ public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
-protected:
+ protected:
/* --- SIGNALS ------------------------------------------------------------ */
-public:
+ public:
dynamicgraph::SignalPtr<MatrixHomogeneous, int> positionSIN;
dynamicgraph::SignalPtr<dynamicgraph::Matrix, int> articularJacobianSIN;
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> positionRefSIN;
@@ -69,7 +68,7 @@ public:
DECLARE_NO_REFERENCE;
/*! @} */
-public:
+ public:
FeatureLineDistance(const std::string &name);
virtual ~FeatureLineDistance(void) {}
@@ -88,7 +87,7 @@ public:
} /* namespace sot */
} /* namespace dynamicgraph */
-#endif // #ifndef __SOT_FEATURE_LINEDISTANCE_HH__
+#endif // #ifndef __SOT_FEATURE_LINEDISTANCE_HH__
/*
* Local variables:
diff --git a/include/sot/core/feature-point6d-relative.hh b/include/sot/core/feature-point6d-relative.hh
index 3fef8089..260e675a 100644
--- a/include/sot/core/feature-point6d-relative.hh
+++ b/include/sot/core/feature-point6d-relative.hh
@@ -47,18 +47,17 @@ namespace sot {
point.
\deprecated This class was replaced by FeaturePose.
*/
-class[[deprecated("replaced by FeaturePose")]] SOTFEATUREPOINT6DRELATIVE_EXPORT
+class [[deprecated("replaced by FeaturePose")]] SOTFEATUREPOINT6DRELATIVE_EXPORT
FeaturePoint6dRelative : public FeaturePoint6d {
-
-public:
+ public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
-protected:
+ protected:
dynamicgraph::Matrix L;
/* --- SIGNALS ------------------------------------------------------------ */
-public:
+ public:
dynamicgraph::SignalPtr<MatrixHomogeneous, int> positionReferenceSIN;
dynamicgraph::SignalPtr<dynamicgraph::Matrix, int>
articularJacobianReferenceSIN;
@@ -77,7 +76,7 @@ public:
using FeaturePoint6d::getReference;
-public:
+ public:
FeaturePoint6dRelative(const std::string &name);
virtual ~FeaturePoint6dRelative(void) {}
@@ -97,7 +96,7 @@ public:
} /* namespace sot */
} /* namespace dynamicgraph */
-#endif // #ifndef __SOT_FEATURE_POINT6DRELATIVE_HH__
+#endif // #ifndef __SOT_FEATURE_POINT6DRELATIVE_HH__
/*
* Local variables:
diff --git a/include/sot/core/feature-point6d.hh b/include/sot/core/feature-point6d.hh
index 5eefad27..10c65fc4 100644
--- a/include/sot/core/feature-point6d.hh
+++ b/include/sot/core/feature-point6d.hh
@@ -46,30 +46,30 @@ namespace sot {
\brief Class that defines point-6d control feature.
\deprecated This class was replaced by FeaturePose.
*/
-class[[deprecated("replaced by FeaturePose")]] SOTFEATUREPOINT6D_EXPORT
- FeaturePoint6d : public FeatureAbstract,
- public FeatureReferenceHelper<FeaturePoint6d> {
-
-public:
+class [[deprecated(
+ "replaced by FeaturePose")]] SOTFEATUREPOINT6D_EXPORT FeaturePoint6d
+ : public FeatureAbstract,
+ public FeatureReferenceHelper<FeaturePoint6d> {
+ public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
/* --- Frame type --------------------------------------------------------- */
-protected:
+ protected:
enum ComputationFrameType { FRAME_DESIRED, FRAME_CURRENT };
static const ComputationFrameType COMPUTATION_FRAME_DEFAULT;
-public:
+ public:
/// \brief Set computation frame
void computationFrame(const std::string &inFrame);
/// \brief Get computation frame
std::string computationFrame() const;
-private:
+ private:
ComputationFrameType computationFrame_;
/* --- SIGNALS ------------------------------------------------------------ */
-public:
+ public:
dynamicgraph::SignalPtr<MatrixHomogeneous, int> positionSIN;
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> velocitySIN;
dynamicgraph::SignalPtr<dynamicgraph::Matrix, int> articularJacobianSIN;
@@ -83,7 +83,7 @@ public:
DECLARE_REFERENCE_FUNCTIONS(FeaturePoint6d);
/*! @} */
-public:
+ public:
FeaturePoint6d(const std::string &name);
virtual ~FeaturePoint6d(void) {}
@@ -109,10 +109,10 @@ public:
virtual void display(std::ostream & os) const;
-public:
+ public:
void servoCurrentPosition(void);
-private:
+ private:
// Intermediate variables for internal computations
Eigen::Vector3d v_, omega_, errordot_t_, errordot_th_, Rreftomega_, t_, tref_;
VectorUTheta error_th_;
@@ -125,7 +125,7 @@ private:
} /* namespace sot */
} /* namespace dynamicgraph */
-#endif // #ifndef __SOT_FEATURE_POINT6D_HH__
+#endif // #ifndef __SOT_FEATURE_POINT6D_HH__
/*
* Local variables:
diff --git a/include/sot/core/feature-pose.hh b/include/sot/core/feature-pose.hh
index 63e36101..3a0a168b 100644
--- a/include/sot/core/feature-pose.hh
+++ b/include/sot/core/feature-pose.hh
@@ -60,12 +60,11 @@ enum Representation_t { SE3Representation, R3xSO3Representation };
*/
template <Representation_t representation = R3xSO3Representation>
class SOT_CORE_DLLAPI FeaturePose : public FeatureAbstract {
-
-public:
+ public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
-public:
+ public:
/*! \name Input signals
@{
*/
@@ -113,7 +112,7 @@ public:
DECLARE_NO_REFERENCE();
/*! @} */
-public:
+ public:
FeaturePose(const std::string &name);
virtual ~FeaturePose(void);
@@ -141,10 +140,10 @@ public:
virtual void display(std::ostream &os) const;
-public:
+ public:
void servoCurrentPosition(const int &time);
-private:
+ private:
MatrixHomogeneous &computefaMfb(MatrixHomogeneous &res, int time);
Vector7 &computeQfaMfb(Vector7 &res, int time);
Vector7 &computeQfaMfbDes(Vector7 &res, int time);
@@ -157,8 +156,10 @@ Vector6d convertVelocity(const MatrixHomogeneous &M,
const MatrixHomogeneous &Mdes,
const Vector &faNufafbDes);
-template <> const std::string FeaturePose<SE3Representation>::CLASS_NAME;
-template <> const std::string FeaturePose<R3xSO3Representation>::CLASS_NAME;
+template <>
+const std::string FeaturePose<SE3Representation>::CLASS_NAME;
+template <>
+const std::string FeaturePose<R3xSO3Representation>::CLASS_NAME;
#if __cplusplus >= 201103L
extern template class FeaturePose<SE3Representation>;
@@ -171,7 +172,7 @@ typedef FeaturePose<SE3Representation> FeaturePoseSE3_t;
} /* namespace sot */
} /* namespace dynamicgraph */
-#endif // #ifndef __SOT_FEATURE_POSE_HH__
+#endif // #ifndef __SOT_FEATURE_POSE_HH__
/*
* Local variables:
diff --git a/include/sot/core/feature-pose.hxx b/include/sot/core/feature-pose.hxx
index 7ba9f507..d335ce40 100644
--- a/include/sot/core/feature-pose.hxx
+++ b/include/sot/core/feature-pose.hxx
@@ -11,18 +11,15 @@
/* --------------------------------------------------------------------- */
/* --- SOT --- */
-#include <boost/mpl/if.hpp>
-#include <boost/type_traits/is_same.hpp>
-
#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/command-getter.h>
#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/command.h>
-#include <pinocchio/multibody/liegroup/liegroup.hpp>
-
#include <Eigen/LU>
-
+#include <boost/mpl/if.hpp>
+#include <boost/type_traits/is_same.hpp>
+#include <pinocchio/multibody/liegroup/liegroup.hpp>
#include <sot/core/debug.hh>
#include <sot/core/factory.hh>
#include <sot/core/feature-pose.hh>
@@ -37,11 +34,12 @@ typedef pinocchio::CartesianProductOperation<
typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t;
namespace internal {
-template <Representation_t representation> struct LG_t {
+template <Representation_t representation>
+struct LG_t {
typedef typename boost::mpl::if_c<representation == SE3Representation, SE3_t,
R3xSO3_t>::type type;
};
-} // namespace internal
+} // namespace internal
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
@@ -130,8 +128,7 @@ unsigned int &FeaturePose<representation>::getDimension(unsigned int &dim,
dim = 0;
for (int i = 0; i < 6; ++i)
- if (fl(i))
- dim++;
+ if (fl(i)) dim++;
sotDEBUG(25) << "# Out }" << std::endl;
return dim;
@@ -184,8 +181,7 @@ Matrix &FeaturePose<representation>::computeJacobian(Matrix &J, int time) {
// J = Jminus * X * jbJjb;
unsigned int rJ = 0;
for (unsigned int r = 0; r < 6; ++r)
- if (fl((int)r))
- J.row(rJ++) = (Jminus * X).row(r) * _jbJjb;
+ if (fl((int)r)) J.row(rJ++) = (Jminus * X).row(r) * _jbJjb;
if (jaJja.isPlugged()) {
const Matrix &_jaJja = jaJja(time);
@@ -200,16 +196,15 @@ Matrix &FeaturePose<representation>::computeJacobian(Matrix &J, int time) {
// J -= (Jminus * X) * jaJja(time);
rJ = 0;
for (unsigned int r = 0; r < 6; ++r)
- if (fl((int)r))
- J.row(rJ++).noalias() -= (Jminus * X).row(r) * _jaJja;
+ if (fl((int)r)) J.row(rJ++).noalias() -= (Jminus * X).row(r) * _jaJja;
}
return J;
}
template <Representation_t representation>
-MatrixHomogeneous &
-FeaturePose<representation>::computefaMfb(MatrixHomogeneous &res, int time) {
+MatrixHomogeneous &FeaturePose<representation>::computefaMfb(
+ MatrixHomogeneous &res, int time) {
check(*this);
res = (oMja(time) * jaMfa(time)).inverse(Eigen::Affine) * oMjb(time) *
@@ -246,8 +241,7 @@ Vector &FeaturePose<representation>::computeError(Vector &error, int time) {
error.resize(dimensionSOUT(time));
unsigned int cursor = 0;
for (unsigned int i = 0; i < 6; ++i)
- if (fl((int)i))
- error(cursor++) = v(i);
+ if (fl((int)i)) error(cursor++) = v(i);
return error;
}
@@ -302,8 +296,7 @@ Vector &FeaturePose<representation>::computeErrorDot(Vector &errordot,
faNufafbDes.accessCopy());
unsigned int cursor = 0;
for (unsigned int i = 0; i < 6; ++i)
- if (fl((int)i))
- errordot(cursor++) = Jminus.row(i) * nu;
+ if (fl((int)i)) errordot(cursor++) = Jminus.row(i) * nu;
return errordot;
}
@@ -347,5 +340,5 @@ void FeaturePose<representation>::display(std::ostream &os) const {
}
}
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
diff --git a/include/sot/core/feature-posture.hh b/include/sot/core/feature-posture.hh
index fe78576d..d89054c9 100644
--- a/include/sot/core/feature-posture.hh
+++ b/include/sot/core/feature-posture.hh
@@ -10,11 +10,12 @@
#ifndef SOT_CORE_FEATURE_POSTURE_HH
#define SOT_CORE_FEATURE_POSTURE_HH
-#include "sot/core/api.hh"
-#include "sot/core/feature-abstract.hh"
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
#include <dynamic-graph/value.h>
+
+#include "sot/core/api.hh"
+#include "sot/core/feature-abstract.hh"
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
@@ -51,7 +52,7 @@ class SOTFEATUREPOSTURE_EXPORT FeaturePosture : public FeatureAbstract {
DYNAMIC_GRAPH_ENTITY_DECL();
-public:
+ public:
typedef dynamicgraph::SignalPtr<dynamicgraph::Vector, int> signalIn_t;
typedef dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int>
signalOut_t;
@@ -63,7 +64,7 @@ public:
virtual unsigned int &getDimension(unsigned int &res, int);
void selectDof(unsigned dofId, bool control);
-protected:
+ protected:
virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int);
virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int);
virtual dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector &res,
@@ -74,11 +75,11 @@ protected:
signalIn_t postureDot_;
signalOut_t error_;
-private:
+ private:
std::vector<bool> activeDofs_;
std::size_t nbActiveDofs_;
-}; // class FeaturePosture
-} // namespace sot
-} // namespace dynamicgraph
+}; // class FeaturePosture
+} // namespace sot
+} // namespace dynamicgraph
-#endif // SOT_CORE_FEATURE_POSTURE_HH
+#endif // SOT_CORE_FEATURE_POSTURE_HH
diff --git a/include/sot/core/feature-task.hh b/include/sot/core/feature-task.hh
index d4f1912c..2c853fd2 100644
--- a/include/sot/core/feature-task.hh
+++ b/include/sot/core/feature-task.hh
@@ -41,19 +41,18 @@ namespace dynamicgraph {
namespace sot {
class SOTFEATURETASK_EXPORT FeatureTask : public FeatureGeneric {
-
-public:
+ public:
/*! Field storing the class name. */
static const std::string CLASS_NAME;
/*! Returns the name of the class. */
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
-protected:
+ protected:
TaskAbstract *taskPtr;
/* --- SIGNALS ------------------------------------------------------------ */
-public:
-public:
+ public:
+ public:
/*! \brief Default constructor */
FeatureTask(const std::string &name);
@@ -67,7 +66,7 @@ public:
} /* namespace sot */
} /* namespace dynamicgraph */
-#endif // #ifndef __SOT_FEATURE_TASK_HH__
+#endif // #ifndef __SOT_FEATURE_TASK_HH__
/*
* Local variables:
diff --git a/include/sot/core/feature-vector3.hh b/include/sot/core/feature-vector3.hh
index 7ed5e5e7..45846ced 100644
--- a/include/sot/core/feature-vector3.hh
+++ b/include/sot/core/feature-vector3.hh
@@ -45,16 +45,15 @@ namespace sot {
\brief Class that defines point-3d control feature
*/
class SOTFEATUREVECTOR3_EXPORT FeatureVector3 : public FeatureAbstract {
-
-public:
+ public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
DECLARE_NO_REFERENCE;
-protected:
+ protected:
/* --- SIGNALS ------------------------------------------------------------ */
-public:
+ public:
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> vectorSIN;
dynamicgraph::SignalPtr<MatrixHomogeneous, int> positionSIN;
dynamicgraph::SignalPtr<dynamicgraph::Matrix, int> articularJacobianSIN;
@@ -64,7 +63,7 @@ public:
using FeatureAbstract::jacobianSOUT;
using FeatureAbstract::selectionSIN;
-public:
+ public:
FeatureVector3(const std::string &name);
virtual ~FeatureVector3(void) {}
@@ -81,7 +80,7 @@ public:
} /* namespace sot */
} /* namespace dynamicgraph */
-#endif // #ifndef __SOT_FEATURE_VECTOR3_HH__
+#endif // #ifndef __SOT_FEATURE_VECTOR3_HH__
/*
* Local variables:
diff --git a/include/sot/core/feature-visual-point.hh b/include/sot/core/feature-visual-point.hh
index aaff6c8e..b5fbe951 100644
--- a/include/sot/core/feature-visual-point.hh
+++ b/include/sot/core/feature-visual-point.hh
@@ -46,16 +46,15 @@ namespace sot {
class SOTFEATUREVISUALPOINT_EXPORT FeatureVisualPoint
: public FeatureAbstract,
public FeatureReferenceHelper<FeatureVisualPoint> {
-
-public:
+ public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
-protected:
+ protected:
dynamicgraph::Matrix L;
/* --- SIGNALS ------------------------------------------------------------ */
-public:
+ public:
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> xySIN;
/** FeatureVisualPoint depth (required to compute the interaction matrix)
* default Z = 1m. */
@@ -68,7 +67,7 @@ public:
DECLARE_REFERENCE_FUNCTIONS(FeatureVisualPoint);
-public:
+ public:
FeatureVisualPoint(const std::string &name);
virtual ~FeatureVisualPoint(void) {}
@@ -89,7 +88,7 @@ public:
} /* namespace sot */
} /* namespace dynamicgraph */
-#endif // #ifndef __SOT_FEATURE_VISUALPOINT_HH__
+#endif // #ifndef __SOT_FEATURE_VISUALPOINT_HH__
/*
* Local variables:
diff --git a/include/sot/core/filter-differentiator.hh b/include/sot/core/filter-differentiator.hh
index 634d20fe..5bbbb6bc 100644
--- a/include/sot/core/filter-differentiator.hh
+++ b/include/sot/core/filter-differentiator.hh
@@ -39,6 +39,7 @@
/* HELPER */
#include <dynamic-graph/signal-helper.h>
+
#include <sot/core/causal-filter.hh>
#include <sot/core/stop-watch.hh>
@@ -58,8 +59,8 @@ class SOTFILTERDIFFERENTIATOR_EXPORT FilterDifferentiator
: public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
-public: /* --- SIGNALS --- */
- /// Input signals
+ public: /* --- SIGNALS --- */
+ /// Input signals
DECLARE_SIGNAL_IN(x, dynamicgraph::Vector);
/// Output signal x_filtered
DECLARE_SIGNAL_OUT(x_filtered, dynamicgraph::Vector);
@@ -81,14 +82,14 @@ public: /* --- SIGNALS --- */
/// accelerations.
DECLARE_SIGNAL_INNER(x_dx_ddx, dynamicgraph::Vector);
-protected:
- double m_dt; /// sampling timestep of the input signal
+ protected:
+ double m_dt; /// sampling timestep of the input signal
int m_x_size;
/// polynomial-fitting filters
CausalFilter *m_filter;
-public:
+ public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/** --- CONSTRUCTOR ---- */
@@ -112,13 +113,13 @@ public:
void switch_filter(const Eigen::VectorXd &filter_numerator,
const Eigen::VectorXd &filter_denominator);
-protected:
-public: /* --- ENTITY INHERITANCE --- */
+ protected:
+ public: /* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream &os) const;
-}; // class FilterDifferentiator
+}; // class FilterDifferentiator
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
-#endif // #ifndef __sot_torque_control_FilterDifferentiator_H__
+#endif // #ifndef __sot_torque_control_FilterDifferentiator_H__
diff --git a/include/sot/core/fir-filter-impl.hh b/include/sot/core/fir-filter-impl.hh
index db2b84bc..0815423e 100644
--- a/include/sot/core/fir-filter-impl.hh
+++ b/include/sot/core/fir-filter-impl.hh
@@ -27,16 +27,16 @@
#endif
#ifdef WIN32
-#define DECLARE_SPECIFICATION(className, sotSigType, sotCoefType) \
- class FIL_FILTER_EXPORT className \
- : public FIRFilter<sotSigType, sotCoefType> { \
- public: \
- className(const std::string &name); \
+#define DECLARE_SPECIFICATION(className, sotSigType, sotCoefType) \
+ class FIL_FILTER_EXPORT className \
+ : public FIRFilter<sotSigType, sotCoefType> { \
+ public: \
+ className(const std::string &name); \
};
#else
-#define DECLARE_SPECIFICATION(className, sotSigType, sotCoefType) \
+#define DECLARE_SPECIFICATION(className, sotSigType, sotCoefType) \
typedef FIRFilter<sotSigType, sotCoefType> className;
-#endif // WIN32
+#endif // WIN32
namespace dynamicgraph {
namespace sot {
@@ -45,6 +45,6 @@ DECLARE_SPECIFICATION(FIRFilterDoubleDouble, double, double)
DECLARE_SPECIFICATION(FIRFilterVectorDouble, Vector, double)
DECLARE_SPECIFICATION(FIRFilterVectorMatrix, Vector, Matrix)
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
#endif
diff --git a/include/sot/core/fir-filter.hh b/include/sot/core/fir-filter.hh
index 4dc9db35..26a48a2e 100644
--- a/include/sot/core/fir-filter.hh
+++ b/include/sot/core/fir-filter.hh
@@ -10,17 +10,16 @@
#ifndef __SOT_FIRFILTER_HH__
#define __SOT_FIRFILTER_HH__
-#include <cassert>
-
-#include <algorithm>
-#include <iterator>
-#include <vector>
-
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/command-getter.h>
#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/entity.h>
+#include <algorithm>
+#include <cassert>
+#include <iterator>
+#include <vector>
+
namespace dynamicgraph {
namespace sot {
@@ -29,8 +28,9 @@ namespace detail {
// As a workaround, only the part of circular_buffer's interface used
// here is implemented.
// Ugly, fatty piece of code.
-template <class T> class circular_buffer {
-public:
+template <class T>
+class circular_buffer {
+ public:
circular_buffer() : buf(1), start(0), numel(0) {}
void push_front(const T &data) {
if (start) {
@@ -61,37 +61,41 @@ public:
}
size_t size() const { return numel; }
-private:
+ private:
std::vector<T> buf;
size_t start;
size_t numel;
-}; // class circular_buffer
-} // namespace detail
+}; // class circular_buffer
+} // namespace detail
-template <class sigT, class coefT> class FIRFilter;
+template <class sigT, class coefT>
+class FIRFilter;
namespace command {
using ::dynamicgraph::command::Command;
using ::dynamicgraph::command::Value;
-template <class sigT, class coefT> class SetElement : public Command {
-public:
+template <class sigT, class coefT>
+class SetElement : public Command {
+ public:
SetElement(FIRFilter<sigT, coefT> &entity, const std::string &docstring);
Value doExecute();
-}; // class SetElement
+}; // class SetElement
-template <class sigT, class coefT> class GetElement : public Command {
-public:
+template <class sigT, class coefT>
+class GetElement : public Command {
+ public:
GetElement(FIRFilter<sigT, coefT> &entity, const std::string &docstring);
Value doExecute();
-}; // class SetElement
-} // namespace command
+}; // class SetElement
+} // namespace command
using ::dynamicgraph::command::Getter;
using ::dynamicgraph::command::Setter;
-template <class sigT, class coefT> class FIRFilter : public Entity {
-public:
+template <class sigT, class coefT>
+class FIRFilter : public Entity {
+ public:
virtual const std::string &getClassName() const {
return Entity::getClassName();
}
@@ -114,38 +118,43 @@ public:
" - s is the input signal.\n";
}
-public:
+ public:
FIRFilter(const std::string &name)
- : Entity(name), SIN(NULL, "sotFIRFilter(" + name + ")::input(T)::sin"),
+ : Entity(name),
+ SIN(NULL, "sotFIRFilter(" + name + ")::input(T)::sin"),
SOUT(boost::bind(&FIRFilter::compute, this, _1, _2), SIN,
"sotFIRFilter(" + name + ")::output(T)::sout") {
signalRegistration(SIN << SOUT);
- std::string docstring = " Set element at rank in array of coefficients\n"
- "\n"
- " Input:\n"
- " - positive int: rank\n"
- " - element\n";
+ std::string docstring =
+ " Set element at rank in array of coefficients\n"
+ "\n"
+ " Input:\n"
+ " - positive int: rank\n"
+ " - element\n";
addCommand("setElement",
new command::SetElement<sigT, coefT>(*this, docstring));
- docstring = " Get element at rank in array of coefficients\n"
- "\n"
- " Input:\n"
- " - positive int: rank\n"
- " Return:\n"
- " - element\n";
+ docstring =
+ " Get element at rank in array of coefficients\n"
+ "\n"
+ " Input:\n"
+ " - positive int: rank\n"
+ " Return:\n"
+ " - element\n";
addCommand("getElement",
new command::GetElement<sigT, coefT>(*this, docstring));
- docstring = " Set number of coefficients\n"
- "\n"
- " Input:\n"
- " - positive int: size\n";
+ docstring =
+ " Set number of coefficients\n"
+ "\n"
+ " Input:\n"
+ " - positive int: size\n";
addCommand("setSize", new Setter<FIRFilter, unsigned>(
*this, &FIRFilter::resizeBuffer, docstring));
- docstring = " Get Number of coefficients\n"
- "\n"
- " Return:\n"
- " - positive int: size\n";
+ docstring =
+ " Get Number of coefficients\n"
+ "\n"
+ " Return:\n"
+ " - positive int: size\n";
addCommand("getSize", new Getter<FIRFilter, unsigned>(
*this, &FIRFilter::getBufferSize, docstring));
}
@@ -183,14 +192,14 @@ public:
static void reset_signal(sigT & /*res*/, const sigT & /*sample*/) {}
-public:
+ public:
SignalPtr<sigT, int> SIN;
SignalTimeDependent<sigT, int> SOUT;
-private:
+ private:
std::vector<coefT> coefs;
detail::circular_buffer<sigT> data;
-}; // class FIRFilter
+}; // class FIRFilter
namespace command {
using ::dynamicgraph::command::Command;
@@ -205,7 +214,8 @@ SetElement<sigT, coefT>::SetElement(FIRFilter<sigT, coefT> &entity,
boost::assign::list_of(Value::UNSIGNED)(ValueHelper<coefT>::TypeID),
docstring) {}
-template <class sigT, class coefT> Value SetElement<sigT, coefT>::doExecute() {
+template <class sigT, class coefT>
+Value SetElement<sigT, coefT>::doExecute() {
FIRFilter<sigT, coefT> &entity =
static_cast<FIRFilter<sigT, coefT> &>(owner());
std::vector<Value> values = getParameterValues();
@@ -220,16 +230,17 @@ GetElement<sigT, coefT>::GetElement(FIRFilter<sigT, coefT> &entity,
const std::string &docstring)
: Command(entity, boost::assign::list_of(Value::UNSIGNED), docstring) {}
-template <class sigT, class coefT> Value GetElement<sigT, coefT>::doExecute() {
+template <class sigT, class coefT>
+Value GetElement<sigT, coefT>::doExecute() {
FIRFilter<sigT, coefT> &entity =
static_cast<FIRFilter<sigT, coefT> &>(owner());
std::vector<Value> values = getParameterValues();
unsigned int rank = values[0].value();
return Value(entity.getElement(rank));
}
-} // namespace command
+} // namespace command
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
#endif
diff --git a/include/sot/core/flags.hh b/include/sot/core/flags.hh
index 2874f405..6dcb1134 100644
--- a/include/sot/core/flags.hh
+++ b/include/sot/core/flags.hh
@@ -19,9 +19,10 @@
#include <vector>
/* SOT */
-#include "sot/core/api.hh"
#include <dynamic-graph/signal-caster.h>
+#include "sot/core/api.hh"
+
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
@@ -30,11 +31,11 @@ namespace dynamicgraph {
namespace sot {
class SOT_CORE_EXPORT Flags {
-protected:
+ protected:
std::vector<bool> flags;
bool outOfRangeFlag;
-public:
+ public:
Flags(const bool &b = false);
Flags(const char *flags);
Flags(const std::vector<bool> &flags);
@@ -58,10 +59,10 @@ public:
void set(const unsigned int &i);
};
-} // namespace sot
+} // namespace sot
template <>
struct signal_io<sot::Flags> : signal_io_unimplemented<sot::Flags> {};
-} // namespace dynamicgraph
+} // namespace dynamicgraph
#endif /* #ifndef __SOT_FLAGS_H */
diff --git a/include/sot/core/fwd.hh b/include/sot/core/fwd.hh
index 65c5dfd8..4d50956b 100644
--- a/include/sot/core/fwd.hh
+++ b/include/sot/core/fwd.hh
@@ -17,7 +17,7 @@ namespace sot {
class Device;
class AbstractSotExternalInterface;
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
-#endif // SOT_CORE_FWD_HH
+#endif // SOT_CORE_FWD_HH
diff --git a/include/sot/core/gain-adaptive.hh b/include/sot/core/gain-adaptive.hh
index 7482962d..4abf52d7 100644
--- a/include/sot/core/gain-adaptive.hh
+++ b/include/sot/core/gain-adaptive.hh
@@ -51,32 +51,31 @@ namespace sot {
* - \f$ c = 0.1 \f$.
*/
class SOTGAINADAPTATIVE_EXPORT GainAdaptive : public dynamicgraph::Entity {
-
-public: /* --- CONSTANTS --- */
+ public: /* --- CONSTANTS --- */
/* Default values. */
- static const double ZERO_DEFAULT; // = 0.1
- static const double INFTY_DEFAULT; // = 0.1
- static const double TAN_DEFAULT; // = 1.
+ static const double ZERO_DEFAULT; // = 0.1
+ static const double INFTY_DEFAULT; // = 0.1
+ static const double TAN_DEFAULT; // = 1.
-public: /* --- ENTITY INHERITANCE --- */
+ public: /* --- ENTITY INHERITANCE --- */
static const std::string CLASS_NAME;
virtual void display(std::ostream &os) const;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
-protected:
+ protected:
/* Parameters of the adaptative-gain function:
* lambda (x) = a * exp (-b*x) + c. */
double coeff_a;
double coeff_b;
double coeff_c;
-public: /* --- CONSTRUCTORS ---- */
+ public: /* --- CONSTRUCTORS ---- */
GainAdaptive(const std::string &name);
GainAdaptive(const std::string &name, const double &lambda);
GainAdaptive(const std::string &name, const double &valueAt0,
const double &valueAtInfty, const double &tanAt0);
-public: /* --- INIT --- */
+ public: /* --- INIT --- */
inline void init(void) { init(ZERO_DEFAULT, INFTY_DEFAULT, TAN_DEFAULT); }
inline void init(const double &lambda) { init(lambda, lambda, 1.); }
void init(const double &valueAt0, const double &valueAtInfty,
@@ -116,18 +115,18 @@ public: /* --- INIT --- */
const double &percentage);
void forceConstant(void);
-public: /* --- SIGNALS --- */
+ public: /* --- SIGNALS --- */
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> errorSIN;
dynamicgraph::SignalTimeDependent<double, int> gainSOUT;
-protected:
+ protected:
double &computeGain(double &res, int t);
-private:
+ private:
void addCommands();
};
} /* namespace sot */
} /* namespace dynamicgraph */
-#endif // #ifndef __SOT_GAIN_ADAPTATIVE_HH__
+#endif // #ifndef __SOT_GAIN_ADAPTATIVE_HH__
diff --git a/include/sot/core/gain-hyperbolic.hh b/include/sot/core/gain-hyperbolic.hh
index abba25c4..245d70e9 100644
--- a/include/sot/core/gain-hyperbolic.hh
+++ b/include/sot/core/gain-hyperbolic.hh
@@ -51,19 +51,18 @@ namespace sot {
* - \f$ d = 0 \f$.
*/
class SOTGAINHYPERBOLIC_EXPORT GainHyperbolic : public dynamicgraph::Entity {
-
-public: /* --- CONSTANTS --- */
+ public: /* --- CONSTANTS --- */
/* Default values. */
- static const double ZERO_DEFAULT; // = 0.1
- static const double INFTY_DEFAULT; // = 0.1
- static const double TAN_DEFAULT; // = 1.
+ static const double ZERO_DEFAULT; // = 0.1
+ static const double INFTY_DEFAULT; // = 0.1
+ static const double TAN_DEFAULT; // = 1.
-public: /* --- ENTITY INHERITANCE --- */
+ public: /* --- ENTITY INHERITANCE --- */
static const std::string CLASS_NAME;
virtual void display(std::ostream &os) const;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
-protected:
+ protected:
/* Parameters of the hyperbolic-gain function:
* lambda (x) = a * exp (-b*x) + c. */
double coeff_a;
@@ -71,14 +70,14 @@ protected:
double coeff_c;
double coeff_d;
-public: /* --- CONSTRUCTORS ---- */
+ public: /* --- CONSTRUCTORS ---- */
GainHyperbolic(const std::string &name);
GainHyperbolic(const std::string &name, const double &lambda);
GainHyperbolic(const std::string &name, const double &valueAt0,
const double &valueAtInfty, const double &tanAt0,
const double &decal0);
-public: /* --- INIT --- */
+ public: /* --- INIT --- */
inline void init(void) { init(ZERO_DEFAULT, INFTY_DEFAULT, TAN_DEFAULT, 0); }
inline void init(const double &lambda) { init(lambda, lambda, 1., 0); }
/** Set the coefficients.
@@ -91,15 +90,15 @@ public: /* --- INIT --- */
const double &tanAt0, const double &decal0);
void forceConstant(void);
-public: /* --- SIGNALS --- */
+ public: /* --- SIGNALS --- */
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> errorSIN;
dynamicgraph::SignalTimeDependent<double, int> gainSOUT;
-protected:
+ protected:
double &computeGain(double &res, int t);
};
} /* namespace sot */
} /* namespace dynamicgraph */
-#endif // #ifndef __SOT_GAIN_HYPERBOLIC_HH__
+#endif // #ifndef __SOT_GAIN_HYPERBOLIC_HH__
diff --git a/include/sot/core/gradient-ascent.hh b/include/sot/core/gradient-ascent.hh
index 40df988a..9d57ffac 100644
--- a/include/sot/core/gradient-ascent.hh
+++ b/include/sot/core/gradient-ascent.hh
@@ -16,6 +16,7 @@
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
+
#include <sot/core/config.hh>
namespace dynamicgraph {
@@ -32,17 +33,17 @@ using dynamicgraph::SignalTimeDependent;
class SOT_CORE_DLLAPI GradientAscent : public Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
-public:
+ public:
SignalPtr<dynamicgraph::Vector, int> gradientSIN;
SignalPtr<double, int> learningRateSIN;
SignalTimeDependent<int, int> refresherSINTERN;
SignalTimeDependent<dynamicgraph::Vector, int> valueSOUT;
-public:
+ public:
GradientAscent(const std::string &n);
virtual ~GradientAscent(void);
-protected:
+ protected:
dynamicgraph::Vector &update(dynamicgraph::Vector &res, const int &inTime);
dynamicgraph::Vector value;
diff --git a/include/sot/core/gripper-control.hh b/include/sot/core/gripper-control.hh
index 75fef519..cbfc9587 100644
--- a/include/sot/core/gripper-control.hh
+++ b/include/sot/core/gripper-control.hh
@@ -20,6 +20,7 @@
/* SOT */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
+
#include <sot/core/flags.hh>
/* STD */
@@ -53,13 +54,13 @@ kept
*
*/
class SOTGRIPPERCONTROL_EXPORT GripperControl {
-protected:
+ protected:
double offset;
static const double OFFSET_DEFAULT;
//! \brief The multiplication
dynamicgraph::Vector factor;
-public:
+ public:
GripperControl(void);
//! \brief Computes the
@@ -70,12 +71,12 @@ public:
const dynamicgraph::Vector ¤tNormVel);
//! \brief
- dynamicgraph::Vector &
- computeDesiredPosition(const dynamicgraph::Vector ¤tPos,
- const dynamicgraph::Vector &desiredPos,
- const dynamicgraph::Vector &torques,
- const dynamicgraph::Vector &torqueLimits,
- dynamicgraph::Vector &referencePos);
+ dynamicgraph::Vector &computeDesiredPosition(
+ const dynamicgraph::Vector ¤tPos,
+ const dynamicgraph::Vector &desiredPos,
+ const dynamicgraph::Vector &torques,
+ const dynamicgraph::Vector &torqueLimits,
+ dynamicgraph::Vector &referencePos);
/*! \brief select only some of the values of the vector fullsize,
* based on the Flags vector.
@@ -95,17 +96,17 @@ class SOTGRIPPERCONTROL_EXPORT GripperControlPlugin
public GripperControl {
DYNAMIC_GRAPH_ENTITY_DECL();
-public:
+ public:
bool calibrationStarted;
-public: /* --- CONSTRUCTION --- */
+ public: /* --- CONSTRUCTION --- */
GripperControlPlugin(const std::string &name);
virtual ~GripperControlPlugin(void);
/* --- DOCUMENTATION --- */
virtual std::string getDocString() const;
-public: /* --- SIGNAL --- */
+ public: /* --- SIGNAL --- */
/* --- INPUTS --- */
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> positionSIN;
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> positionDesSIN;
@@ -127,7 +128,7 @@ public: /* --- SIGNAL --- */
dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int>
desiredPositionSOUT;
-public: /* --- COMMANDLINE --- */
+ public: /* --- COMMANDLINE --- */
void initCommands();
void setOffset(const double &value);
@@ -136,4 +137,4 @@ public: /* --- COMMANDLINE --- */
} /* namespace sot */
} /* namespace dynamicgraph */
-#endif // #ifndef __SOT_SOTGRIPPERCONTROL_H__
+#endif // #ifndef __SOT_SOTGRIPPERCONTROL_H__
diff --git a/include/sot/core/integrator-abstract-impl.hh b/include/sot/core/integrator-abstract-impl.hh
index 9054fecb..7bd8cdcc 100644
--- a/include/sot/core/integrator-abstract-impl.hh
+++ b/include/sot/core/integrator-abstract-impl.hh
@@ -32,14 +32,14 @@
/* --------------------------------------------------------------------- */
#ifdef WIN32
-#define DECLARE_SPECIFICATION(className, sotSigType, sotCoefType) \
- class INTEGRATOR_ABSTRACT_EXPORT className \
- : public IntegratorAbstract<sotSigType, sotCoefType> { \
- public: \
- className(const std::string &name); \
+#define DECLARE_SPECIFICATION(className, sotSigType, sotCoefType) \
+ class INTEGRATOR_ABSTRACT_EXPORT className \
+ : public IntegratorAbstract<sotSigType, sotCoefType> { \
+ public: \
+ className(const std::string &name); \
};
#else
-#define DECLARE_SPECIFICATION(className, sotSigType, sotCoefType) \
+#define DECLARE_SPECIFICATION(className, sotSigType, sotCoefType) \
typedef IntegratorAbstract<sotSigType, sotCoefType> className;
#endif
@@ -48,6 +48,6 @@ namespace sot {
DECLARE_SPECIFICATION(IntegratorAbstractDouble, double, double)
DECLARE_SPECIFICATION(IntegratorAbstractVector, dynamicgraph::Vector,
dynamicgraph::Matrix)
-} // namespace sot
-} // namespace dynamicgraph
-#endif // #ifndef __SOT_MAILBOX_HH
+} // namespace sot
+} // namespace dynamicgraph
+#endif // #ifndef __SOT_MAILBOX_HH
diff --git a/include/sot/core/integrator-abstract.hh b/include/sot/core/integrator-abstract.hh
index 332eacf7..840797ba 100644
--- a/include/sot/core/integrator-abstract.hh
+++ b/include/sot/core/integrator-abstract.hh
@@ -22,6 +22,7 @@
#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/pool.h>
+
#include <sot/core/debug.hh>
#include <sot/core/flags.hh>
@@ -43,7 +44,7 @@ namespace sot {
*/
template <class sigT, class coefT>
class IntegratorAbstract : public dynamicgraph::Entity {
-public:
+ public:
IntegratorAbstract(const std::string &name)
: dynamicgraph::Entity(name),
SIN(NULL, "sotIntegratorAbstract(" + name + ")::input(vector)::sin"),
@@ -82,7 +83,7 @@ public:
virtual sigT &integrate(sigT &res, int time) = 0;
-public:
+ public:
void pushNumCoef(const coefT &numCoef) { numerator.push_back(numCoef); }
void pushDenomCoef(const coefT &denomCoef) {
denominator.push_back(denomCoef);
@@ -96,7 +97,7 @@ public:
const std::vector<coefT> &denomCoeffs() const { return denominator; }
void denomCoeffs(const std::vector<coefT> &coeffs) { denominator = coeffs; }
-public:
+ public:
dynamicgraph::SignalPtr<sigT, int> SIN;
dynamicgraph::SignalTimeDependent<sigT, int> SOUT;
@@ -115,7 +116,7 @@ public:
os << " + " << denominator[i] << " s^" << i;
}
-protected:
+ protected:
std::vector<coefT> numerator;
std::vector<coefT> denominator;
};
diff --git a/include/sot/core/integrator-euler-impl.hh b/include/sot/core/integrator-euler-impl.hh
index ae22a2d0..943196a7 100644
--- a/include/sot/core/integrator-euler-impl.hh
+++ b/include/sot/core/integrator-euler-impl.hh
@@ -32,24 +32,24 @@
#endif
#ifdef WIN32
-#define DECLARE_SPECIFICATION(className, sotSigType, sotCoefType) \
- class INTEGRATOR_EULER_EXPORT className \
- : public IntegratorEuler<sotSigType, sotCoefType> { \
- public: \
- std::string getTypeName(void); \
- className(const std::string &name); \
+#define DECLARE_SPECIFICATION(className, sotSigType, sotCoefType) \
+ class INTEGRATOR_EULER_EXPORT className \
+ : public IntegratorEuler<sotSigType, sotCoefType> { \
+ public: \
+ std::string getTypeName(void); \
+ className(const std::string &name); \
};
#else
-#define DECLARE_SPECIFICATION(className, sotSigType, sotCoefType) \
+#define DECLARE_SPECIFICATION(className, sotSigType, sotCoefType) \
typedef IntegratorEuler<sotSigType, sotCoefType> className;
-#endif // WIN32
+#endif // WIN32
namespace dynamicgraph {
namespace sot {
DECLARE_SPECIFICATION(IntegratorEulerDoubleDouble, double, double)
DECLARE_SPECIFICATION(IntegratorEulerVectorDouble, Vector, double)
DECLARE_SPECIFICATION(IntegratorEulerVectorMatrix, Vector, Matrix)
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
#endif
diff --git a/include/sot/core/integrator-euler.hh b/include/sot/core/integrator-euler.hh
index d7b04d06..92b761ba 100644
--- a/include/sot/core/integrator-euler.hh
+++ b/include/sot/core/integrator-euler.hh
@@ -17,6 +17,7 @@
/* SOT */
#include <dynamic-graph/command-getter.h>
#include <dynamic-graph/command-setter.h>
+
#include <sot/core/integrator-abstract.hh>
/* --------------------------------------------------------------------- */
@@ -27,14 +28,15 @@ namespace dynamicgraph {
namespace sot {
namespace internal {
-template <class coefT> bool integratorEulerCoeffIsIdentity(const coefT c) {
+template <class coefT>
+bool integratorEulerCoeffIsIdentity(const coefT c) {
return c == 1;
}
bool integratorEulerCoeffIsIdentity(const Vector c) { return c.isOnes(); }
bool integratorEulerCoeffIsIdentity(const Matrix c) { return c.isIdentity(); }
-} // namespace internal
+} // namespace internal
/*!
* \class IntegratorEuler
@@ -47,19 +49,18 @@ bool integratorEulerCoeffIsIdentity(const Matrix c) { return c.isIdentity(); }
*/
template <class sigT, class coefT>
class IntegratorEuler : public IntegratorAbstract<sigT, coefT> {
-
-public:
+ public:
virtual const std::string &getClassName(void) const;
static std::string getTypeName(void) { return "Unknown"; }
static const std::string CLASS_NAME;
-public:
+ public:
using IntegratorAbstract<sigT, coefT>::SIN;
using IntegratorAbstract<sigT, coefT>::SOUT;
using IntegratorAbstract<sigT, coefT>::numerator;
using IntegratorAbstract<sigT, coefT>::denominator;
-public:
+ public:
IntegratorEuler(const std::string &name)
: IntegratorAbstract<sigT, coefT>(name),
derivativeSOUT(boost::bind(&IntegratorEuler<sigT, coefT>::derivative,
@@ -92,7 +93,7 @@ public:
virtual ~IntegratorEuler(void) {}
-protected:
+ protected:
std::vector<sigT> inputMemory;
std::vector<sigT> outputMemory;
@@ -101,7 +102,7 @@ protected:
double dt;
double invdt;
-public:
+ public:
sigT &integrate(sigT &res, int time) {
sotDEBUG(15) << "# In {" << std::endl;
diff --git a/include/sot/core/joint-limitator.hh b/include/sot/core/joint-limitator.hh
index efb1e914..3931689c 100644
--- a/include/sot/core/joint-limitator.hh
+++ b/include/sot/core/joint-limitator.hh
@@ -15,6 +15,7 @@
// SOT
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
+
#include <sot/core/exception-task.hh>
#if defined(WIN32)
@@ -37,7 +38,7 @@ namespace sot {
class SOTJOINTLIMITATOR_EXPORT JointLimitator : public dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
-public:
+ public:
JointLimitator(const std::string &name);
virtual ~JointLimitator() {}
@@ -58,7 +59,7 @@ public:
dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> widthJlSINTERN;
/// \}
};
-} // end of namespace sot.
-} // namespace dynamicgraph
+} // end of namespace sot.
+} // namespace dynamicgraph
-#endif //! SOT_FEATURE_JOINTLIMITS_HH
+#endif //! SOT_FEATURE_JOINTLIMITS_HH
diff --git a/include/sot/core/joint-trajectory-entity.hh b/include/sot/core/joint-trajectory-entity.hh
index d674e132..dc8854a2 100644
--- a/include/sot/core/joint-trajectory-entity.hh
+++ b/include/sot/core/joint-trajectory-entity.hh
@@ -9,9 +9,8 @@
#ifndef SOT_JOINT_TRAJECTORY_ENTITY_HH
#define SOT_JOINT_TRAJECTORY_ENTITY_HH
-#include <list>
-
#include <deque>
+#include <list>
// Maal
#include <dynamic-graph/linear-algebra.h>
@@ -19,9 +18,9 @@
// SOT
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
+
#include <sot/core/matrix-geometry.hh>
#include <sot/core/trajectory.hh>
-
#include <sstream>
// API
@@ -47,7 +46,7 @@ namespace sot {
class SOTJOINT_TRAJECTORY_ENTITY_EXPORT SotJointTrajectoryEntity
: public dynamicgraph::Entity {
-public:
+ public:
DYNAMIC_GRAPH_ENTITY_DECL();
/// \brief Constructor
@@ -74,8 +73,8 @@ public:
unsigned int &getSeqId(unsigned int &seqid, const int &time);
/// \brief Convert a xyztheta vector into an homogeneous matrix
- sot::MatrixHomogeneous
- XYZThetaToMatrixHomogeneous(const dynamicgraph::Vector &xyztheta);
+ sot::MatrixHomogeneous XYZThetaToMatrixHomogeneous(
+ const dynamicgraph::Vector &xyztheta);
/// \brief Perform one update of the signals.
int &OneStepOfUpdate(int &dummy, const int &time);
@@ -91,7 +90,7 @@ public:
}
/// @}
-public:
+ public:
typedef int Dummy;
/// @name Signals
@@ -121,7 +120,7 @@ public:
dynamicgraph::SignalPtr<Trajectory, int> trajectorySIN;
///@}
-protected:
+ protected:
/// \brief Index on the point along the trajectory.
std::deque<sot::Trajectory>::size_type index_;
@@ -162,4 +161,4 @@ protected:
} /* namespace sot */
} /* namespace dynamicgraph */
-#endif // SOT_JOINT_TRAJECTORY_ENTITY_HH
+#endif // SOT_JOINT_TRAJECTORY_ENTITY_HH
diff --git a/include/sot/core/kalman.hh b/include/sot/core/kalman.hh
index 498d7834..1e09338a 100644
--- a/include/sot/core/kalman.hh
+++ b/include/sot/core/kalman.hh
@@ -16,11 +16,12 @@
/* --- INCLUDE -------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
-#include <Eigen/LU>
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/linear-algebra.h>
+#include <Eigen/LU>
+
/* -------------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------------ */
/* -------------------------------------------------------------------------- */
@@ -43,31 +44,31 @@ namespace dynamicgraph {
namespace sot {
class SOT_KALMAN_EXPORT Kalman : public Entity {
-public:
+ public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
-protected:
+ protected:
unsigned int size_state;
unsigned int size_measure;
double dt;
-public:
- SignalPtr<Vector, int> measureSIN; // y
- SignalPtr<Matrix, int> modelTransitionSIN; // F
- SignalPtr<Matrix, int> modelMeasureSIN; // H
- SignalPtr<Matrix, int> noiseTransitionSIN; // Q
- SignalPtr<Matrix, int> noiseMeasureSIN; // R
+ public:
+ SignalPtr<Vector, int> measureSIN; // y
+ SignalPtr<Matrix, int> modelTransitionSIN; // F
+ SignalPtr<Matrix, int> modelMeasureSIN; // H
+ SignalPtr<Matrix, int> noiseTransitionSIN; // Q
+ SignalPtr<Matrix, int> noiseMeasureSIN; // R
- SignalPtr<Vector, int> statePredictedSIN; // x_{k|k-1}
- SignalPtr<Vector, int> observationPredictedSIN; // y_pred = h (x_{k|k-1})
- SignalTimeDependent<Matrix, int> varianceUpdateSOUT; // P
- SignalTimeDependent<Vector, int> stateUpdateSOUT; // X_est
+ SignalPtr<Vector, int> statePredictedSIN; // x_{k|k-1}
+ SignalPtr<Vector, int> observationPredictedSIN; // y_pred = h (x_{k|k-1})
+ SignalTimeDependent<Matrix, int> varianceUpdateSOUT; // P
+ SignalTimeDependent<Vector, int> stateUpdateSOUT; // X_est
- SignalTimeDependent<Matrix, int> gainSINTERN; // K
- SignalTimeDependent<Matrix, int> innovationSINTERN; // S
+ SignalTimeDependent<Matrix, int> gainSINTERN; // K
+ SignalTimeDependent<Matrix, int> innovationSINTERN; // S
-public:
+ public:
virtual std::string getDocString() const {
return "Implementation of extended Kalman filter \n"
"\n"
@@ -138,7 +139,7 @@ public:
" k|k\n";
}
-protected:
+ protected:
Matrix &computeVarianceUpdate(Matrix &P_k_k, const int &time);
Vector &computeStateUpdate(Vector &x_est, const int &time);
@@ -181,14 +182,14 @@ protected:
// Kalman Gain
Matrix K_;
-public:
+ public:
Kalman(const std::string &name);
/* --- Entity --- */
void display(std::ostream &os) const;
};
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
/*!
\file Kalman.h
diff --git a/include/sot/core/latch.hh b/include/sot/core/latch.hh
index fc864a6c..b1097929 100644
--- a/include/sot/core/latch.hh
+++ b/include/sot/core/latch.hh
@@ -14,6 +14,7 @@
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/entity.h>
+
#include <sot/core/pool.hh>
/* STD */
@@ -30,14 +31,13 @@ using dynamicgraph::command::docCommandVoid0;
using dynamicgraph::command::makeCommandVoid0;
class Latch : public Entity {
-
-public: /* --- SIGNAL --- */
+ public: /* --- SIGNAL --- */
DYNAMIC_GRAPH_ENTITY_DECL();
Signal<bool, int> outSOUT;
Signal<bool, int> turnOnSOUT;
Signal<bool, int> turnOffSOUT;
-protected:
+ protected:
bool signalOutput;
void turnOn() { signalOutput = true; }
bool &turnOnLatch(bool &res, int) {
@@ -56,9 +56,10 @@ protected:
return res;
}
-public:
+ public:
Latch(const std::string &name)
- : Entity(name), outSOUT("Latch(" + name + ")::output(bool)::out"),
+ : Entity(name),
+ outSOUT("Latch(" + name + ")::output(bool)::out"),
turnOnSOUT("Latch(" + name + ")::output(bool)::turnOnSout"),
turnOffSOUT("Latch(" + name + ")::output(bool)::turnOffSout") {
outSOUT.setFunction(boost::bind(&Latch::latchOutput, this, _1, _2));
@@ -79,4 +80,4 @@ public:
} /* namespace sot */
} /* namespace dynamicgraph */
-#endif // #ifndef __SOT_LATCH_H__
+#endif // #ifndef __SOT_LATCH_H__
diff --git a/include/sot/core/macros-signal.hh b/include/sot/core/macros-signal.hh
index 3b2e182f..164d021b 100644
--- a/include/sot/core/macros-signal.hh
+++ b/include/sot/core/macros-signal.hh
@@ -11,111 +11,111 @@
/* --- GENERIC MACROS ------------------------------------------------------- */
/* --- GENERIC MACROS ------------------------------------------------------- */
-#define SOT_CALL_SIG(sotName, sotType) \
+#define SOT_CALL_SIG(sotName, sotType) \
boost::bind(&Signal<sotType, int>::access, &sotName, _2)
/* --- STATIC MACROS -------------------------------------------------------- */
/* --- STATIC MACROS -------------------------------------------------------- */
/* --- STATIC MACROS -------------------------------------------------------- */
-#define SOT_INIT_SIGNAL_1(sotFunction, sotArg1, sotArg1Type) \
+#define SOT_INIT_SIGNAL_1(sotFunction, sotArg1, sotArg1Type) \
boost::bind(&sotFunction, SOT_CALL_SIG(sotArg1, sotArg1Type), _1), sotArg1
-#define SOT_INIT_SIGNAL_2(sotFunction, sotArg1, sotArg1Type, sotArg2, \
- sotArg2Type) \
- boost::bind(&sotFunction, SOT_CALL_SIG(sotArg1, sotArg1Type), \
- SOT_CALL_SIG(sotArg2, sotArg2Type), _1), \
+#define SOT_INIT_SIGNAL_2(sotFunction, sotArg1, sotArg1Type, sotArg2, \
+ sotArg2Type) \
+ boost::bind(&sotFunction, SOT_CALL_SIG(sotArg1, sotArg1Type), \
+ SOT_CALL_SIG(sotArg2, sotArg2Type), _1), \
sotArg1 << sotArg2
-#define SOT_INIT_SIGNAL_3(sotFunction, sotArg1, sotArg1Type, sotArg2, \
- sotArg2Type, sotArg3, sotArg3Type) \
- boost::bind(&sotFunction, SOT_CALL_SIG(sotArg1, sotArg1Type), \
- SOT_CALL_SIG(sotArg2, sotArg2Type), \
- SOT_CALL_SIG(sotArg3, sotArg3Type), _1), \
+#define SOT_INIT_SIGNAL_3(sotFunction, sotArg1, sotArg1Type, sotArg2, \
+ sotArg2Type, sotArg3, sotArg3Type) \
+ boost::bind(&sotFunction, SOT_CALL_SIG(sotArg1, sotArg1Type), \
+ SOT_CALL_SIG(sotArg2, sotArg2Type), \
+ SOT_CALL_SIG(sotArg3, sotArg3Type), _1), \
sotArg1 << sotArg2 << sotArg3
-#define SOT_INIT_SIGNAL_4(sotFunction, sotArg1, sotArg1Type, sotArg2, \
- sotArg2Type, sotArg3, sotArg3Type, sotArg4, \
- sotArg4Type) \
- boost::bind(&sotFunction, SOT_CALL_SIG(sotArg1, sotArg1Type), \
- SOT_CALL_SIG(sotArg2, sotArg2Type), \
- SOT_CALL_SIG(sotArg3, sotArg3Type), \
- SOT_CALL_SIG(sotArg4, sotArg4Type), _1), \
+#define SOT_INIT_SIGNAL_4(sotFunction, sotArg1, sotArg1Type, sotArg2, \
+ sotArg2Type, sotArg3, sotArg3Type, sotArg4, \
+ sotArg4Type) \
+ boost::bind(&sotFunction, SOT_CALL_SIG(sotArg1, sotArg1Type), \
+ SOT_CALL_SIG(sotArg2, sotArg2Type), \
+ SOT_CALL_SIG(sotArg3, sotArg3Type), \
+ SOT_CALL_SIG(sotArg4, sotArg4Type), _1), \
sotArg1 << sotArg2 << sotArg3 << sotArg4
-#define SOT_INIT_SIGNAL_5(sotFunction, sotArg1, sotArg1Type, sotArg2, \
- sotArg2Type, sotArg3, sotArg3Type, sotArg4, \
- sotArg4Type, sotArg5, sotArg5Type) \
- boost::bind(&sotFunction, SOT_CALL_SIG(sotArg1, sotArg1Type), \
- SOT_CALL_SIG(sotArg2, sotArg2Type), \
- SOT_CALL_SIG(sotArg3, sotArg3Type), \
- SOT_CALL_SIG(sotArg4, sotArg4Type), \
- SOT_CALL_SIG(sotArg5, sotArg5Type), _1), \
+#define SOT_INIT_SIGNAL_5(sotFunction, sotArg1, sotArg1Type, sotArg2, \
+ sotArg2Type, sotArg3, sotArg3Type, sotArg4, \
+ sotArg4Type, sotArg5, sotArg5Type) \
+ boost::bind(&sotFunction, SOT_CALL_SIG(sotArg1, sotArg1Type), \
+ SOT_CALL_SIG(sotArg2, sotArg2Type), \
+ SOT_CALL_SIG(sotArg3, sotArg3Type), \
+ SOT_CALL_SIG(sotArg4, sotArg4Type), \
+ SOT_CALL_SIG(sotArg5, sotArg5Type), _1), \
sotArg1 << sotArg2 << sotArg3 << sotArg4 << sotArg5
-#define SOT_INIT_SIGNAL_6(sotFunction, sotArg1, sotArg1Type, sotArg2, \
- sotArg2Type, sotArg3, sotArg3Type, sotArg4, \
- sotArg4Type, sotArg5, sotArg5Type, sotArg6, \
- sotArg6Type) \
- boost::bind( \
- &sotFunction, SOT_CALL_SIG(sotArg1, sotArg1Type), \
- SOT_CALL_SIG(sotArg2, sotArg2Type), SOT_CALL_SIG(sotArg3, sotArg3Type), \
- SOT_CALL_SIG(sotArg4, sotArg4Type), SOT_CALL_SIG(sotArg5, sotArg5Type), \
- SOT_CALL_SIG(sotArg6, sotArg6Type), _1), \
+#define SOT_INIT_SIGNAL_6(sotFunction, sotArg1, sotArg1Type, sotArg2, \
+ sotArg2Type, sotArg3, sotArg3Type, sotArg4, \
+ sotArg4Type, sotArg5, sotArg5Type, sotArg6, \
+ sotArg6Type) \
+ boost::bind( \
+ &sotFunction, SOT_CALL_SIG(sotArg1, sotArg1Type), \
+ SOT_CALL_SIG(sotArg2, sotArg2Type), SOT_CALL_SIG(sotArg3, sotArg3Type), \
+ SOT_CALL_SIG(sotArg4, sotArg4Type), SOT_CALL_SIG(sotArg5, sotArg5Type), \
+ SOT_CALL_SIG(sotArg6, sotArg6Type), _1), \
sotArg1 << sotArg2 << sotArg3 << sotArg4 << sotArg5 << sotArg6
-#define SOT_INIT_SIGNAL_7(sotFunction, sotArg1, sotArg1Type, sotArg2, \
- sotArg2Type, sotArg3, sotArg3Type, sotArg4, \
- sotArg4Type, sotArg5, sotArg5Type, sotArg6, \
- sotArg6Type, sotArg7, sotArg7Type) \
- boost::bind( \
- &sotFunction, SOT_CALL_SIG(sotArg1, sotArg1Type), \
- SOT_CALL_SIG(sotArg2, sotArg2Type), SOT_CALL_SIG(sotArg3, sotArg3Type), \
- SOT_CALL_SIG(sotArg4, sotArg4Type), SOT_CALL_SIG(sotArg5, sotArg5Type), \
- SOT_CALL_SIG(sotArg6, sotArg6Type), SOT_CALL_SIG(sotArg7, sotArg7Type), \
- _1), \
- sotArg1 << sotArg2 << sotArg3 << sotArg4 << sotArg5 << sotArg6 \
+#define SOT_INIT_SIGNAL_7(sotFunction, sotArg1, sotArg1Type, sotArg2, \
+ sotArg2Type, sotArg3, sotArg3Type, sotArg4, \
+ sotArg4Type, sotArg5, sotArg5Type, sotArg6, \
+ sotArg6Type, sotArg7, sotArg7Type) \
+ boost::bind( \
+ &sotFunction, SOT_CALL_SIG(sotArg1, sotArg1Type), \
+ SOT_CALL_SIG(sotArg2, sotArg2Type), SOT_CALL_SIG(sotArg3, sotArg3Type), \
+ SOT_CALL_SIG(sotArg4, sotArg4Type), SOT_CALL_SIG(sotArg5, sotArg5Type), \
+ SOT_CALL_SIG(sotArg6, sotArg6Type), SOT_CALL_SIG(sotArg7, sotArg7Type), \
+ _1), \
+ sotArg1 << sotArg2 << sotArg3 << sotArg4 << sotArg5 << sotArg6 \
<< sotArg7
/* --- MEMBERS MACROS ------------------------------------------------------- */
/* --- MEMBERS MACROS ------------------------------------------------------- */
/* --- MEMBERS MACROS ------------------------------------------------------- */
-#define SOT_MEMBER_SIGNAL_1(sotFunction, sotArg1, sotArg1Type) \
- boost::bind(&sotFunction, this, SOT_CALL_SIG(sotArg1, sotArg1Type), _1), \
+#define SOT_MEMBER_SIGNAL_1(sotFunction, sotArg1, sotArg1Type) \
+ boost::bind(&sotFunction, this, SOT_CALL_SIG(sotArg1, sotArg1Type), _1), \
sotArg1
-#define SOT_MEMBER_SIGNAL_2(sotFunction, sotArg1, sotArg1Type, sotArg2, \
- sotArg2Type) \
- boost::bind(&sotFunction, this, SOT_CALL_SIG(sotArg1, sotArg1Type), \
- SOT_CALL_SIG(sotArg2, sotArg2Type), _1), \
+#define SOT_MEMBER_SIGNAL_2(sotFunction, sotArg1, sotArg1Type, sotArg2, \
+ sotArg2Type) \
+ boost::bind(&sotFunction, this, SOT_CALL_SIG(sotArg1, sotArg1Type), \
+ SOT_CALL_SIG(sotArg2, sotArg2Type), _1), \
sotArg1 << sotArg2
-#define SOT_MEMBER_SIGNAL_4(sotFunction, sotArg1, sotArg1Type, sotArg2, \
- sotArg2Type, sotArg3, sotArg3Type, sotArg4, \
- sotArg4Type) \
- boost::bind(&sotFunction, this, SOT_CALL_SIG(sotArg1, sotArg1Type), \
- SOT_CALL_SIG(sotArg2, sotArg2Type), \
- SOT_CALL_SIG(sotArg3, sotArg3Type), \
- SOT_CALL_SIG(sotArg4, sotArg4Type), _1), \
+#define SOT_MEMBER_SIGNAL_4(sotFunction, sotArg1, sotArg1Type, sotArg2, \
+ sotArg2Type, sotArg3, sotArg3Type, sotArg4, \
+ sotArg4Type) \
+ boost::bind(&sotFunction, this, SOT_CALL_SIG(sotArg1, sotArg1Type), \
+ SOT_CALL_SIG(sotArg2, sotArg2Type), \
+ SOT_CALL_SIG(sotArg3, sotArg3Type), \
+ SOT_CALL_SIG(sotArg4, sotArg4Type), _1), \
sotArg1 << sotArg2 << sotArg3 << sotArg4
-#define SOT_MEMBER_SIGNAL_5(sotFunction, sotArg1, sotArg1Type, sotArg2, \
- sotArg2Type, sotArg3, sotArg3Type, sotArg4, \
- sotArg4Type, sotArg5, sotArg5Type) \
- boost::bind(&sotFunction, this, SOT_CALL_SIG(sotArg1, sotArg1Type), \
- SOT_CALL_SIG(sotArg2, sotArg2Type), \
- SOT_CALL_SIG(sotArg3, sotArg3Type), \
- SOT_CALL_SIG(sotArg4, sotArg4Type), \
- SOT_CALL_SIG(sotArg5, sotArg5Type), _1), \
+#define SOT_MEMBER_SIGNAL_5(sotFunction, sotArg1, sotArg1Type, sotArg2, \
+ sotArg2Type, sotArg3, sotArg3Type, sotArg4, \
+ sotArg4Type, sotArg5, sotArg5Type) \
+ boost::bind(&sotFunction, this, SOT_CALL_SIG(sotArg1, sotArg1Type), \
+ SOT_CALL_SIG(sotArg2, sotArg2Type), \
+ SOT_CALL_SIG(sotArg3, sotArg3Type), \
+ SOT_CALL_SIG(sotArg4, sotArg4Type), \
+ SOT_CALL_SIG(sotArg5, sotArg5Type), _1), \
sotArg1 << sotArg2 << sotArg3 << sotArg4 << sotArg5
-#define SOT_MEMBER_SIGNAL_6(sotFunction, sotArg1, sotArg1Type, sotArg2, \
- sotArg2Type, sotArg3, sotArg3Type, sotArg4, \
- sotArg4Type, sotArg5, sotArg5Type, sotArg6, \
- sotArg6Type) \
- boost::bind( \
- &sotFunction, this, SOT_CALL_SIG(sotArg1, sotArg1Type), \
- SOT_CALL_SIG(sotArg2, sotArg2Type), SOT_CALL_SIG(sotArg3, sotArg3Type), \
- SOT_CALL_SIG(sotArg4, sotArg4Type), SOT_CALL_SIG(sotArg5, sotArg5Type), \
- SOT_CALL_SIG(sotArg6, sotArg6Type), _1), \
+#define SOT_MEMBER_SIGNAL_6(sotFunction, sotArg1, sotArg1Type, sotArg2, \
+ sotArg2Type, sotArg3, sotArg3Type, sotArg4, \
+ sotArg4Type, sotArg5, sotArg5Type, sotArg6, \
+ sotArg6Type) \
+ boost::bind( \
+ &sotFunction, this, SOT_CALL_SIG(sotArg1, sotArg1Type), \
+ SOT_CALL_SIG(sotArg2, sotArg2Type), SOT_CALL_SIG(sotArg3, sotArg3Type), \
+ SOT_CALL_SIG(sotArg4, sotArg4Type), SOT_CALL_SIG(sotArg5, sotArg5Type), \
+ SOT_CALL_SIG(sotArg6, sotArg6Type), _1), \
sotArg1 << sotArg2 << sotArg3 << sotArg4 << sotArg5 << sotArg6
diff --git a/include/sot/core/macros.hh b/include/sot/core/macros.hh
index cc89e248..e6b4b338 100644
--- a/include/sot/core/macros.hh
+++ b/include/sot/core/macros.hh
@@ -6,7 +6,7 @@
#define SOT_CORE_DISABLE_WARNING_PUSH __pragma(warning(push))
#define SOT_CORE_DISABLE_WARNING_POP __pragma(warning(pop))
-#define SOT_CORE_DISABLE_WARNING(warningNumber) \
+#define SOT_CORE_DISABLE_WARNING(warningNumber) \
__pragma(warning(disable : warningNumber))
#define SOT_CORE_DISABLE_WARNING_DEPRECATED SOT_CORE_DISABLE_WARNING(4996)
#define SOT_CORE_DISABLE_WARNING_FALLTHROUGH
@@ -16,11 +16,11 @@
#define SOT_CORE_DO_PRAGMA(X) _Pragma(#X)
#define SOT_CORE_DISABLE_WARNING_PUSH SOT_CORE_DO_PRAGMA(GCC diagnostic push)
#define SOT_CORE_DISABLE_WARNING_POP SOT_CORE_DO_PRAGMA(GCC diagnostic pop)
-#define SOT_CORE_DISABLE_WARNING(warningName) \
+#define SOT_CORE_DISABLE_WARNING(warningName) \
SOT_CORE_DO_PRAGMA(GCC diagnostic ignored #warningName)
-#define SOT_CORE_DISABLE_WARNING_DEPRECATED \
+#define SOT_CORE_DISABLE_WARNING_DEPRECATED \
SOT_CORE_DISABLE_WARNING(-Wdeprecated - declarations)
-#define SOT_CORE_DISABLE_WARNING_FALLTHROUGH \
+#define SOT_CORE_DISABLE_WARNING_FALLTHROUGH \
SOT_CORE_DISABLE_WARNING(-Wimplicit - fallthrough)
#else
diff --git a/include/sot/core/madgwickahrs.hh b/include/sot/core/madgwickahrs.hh
index a80021db..2449d106 100644
--- a/include/sot/core/madgwickahrs.hh
+++ b/include/sot/core/madgwickahrs.hh
@@ -48,12 +48,14 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#include "boost/assign.hpp"
#include <dynamic-graph/signal-helper.h>
+
#include <map>
#include <sot/core/matrix-geometry.hh>
-#define betaDef 0.01 // 2 * proportional g
+#include "boost/assign.hpp"
+
+#define betaDef 0.01 // 2 * proportional g
namespace dynamicgraph {
namespace sot {
@@ -84,7 +86,7 @@ class SOTMADGWICKAHRS_EXPORT MadgwickAHRS : public ::dynamicgraph::Entity {
typedef MadgwickAHRS EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
-public:
+ public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
@@ -104,7 +106,7 @@ public:
/// Estimated orientation of IMU as a quaternion
DECLARE_SIGNAL_OUT(imu_quat, dynamicgraph::Vector);
-protected:
+ protected:
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream &os) const;
@@ -114,7 +116,7 @@ protected:
void madgwickAHRSupdateIMU(double gx, double gy, double gz, double ax,
double ay, double az);
-protected:
+ protected:
/// true if the entity has been successfully initialized
bool m_initSucceeded;
/// 2 * proportional gain (Kp)
@@ -124,8 +126,8 @@ protected:
/// sample frequency in Hz
double m_sampleFreq;
-}; // class MadgwickAHRS
-} // namespace sot
-} // namespace dynamicgraph
+}; // class MadgwickAHRS
+} // namespace sot
+} // namespace dynamicgraph
-#endif // #ifndef __sot_torque_control_madgwickahrs_H__
+#endif // #ifndef __sot_torque_control_madgwickahrs_H__
diff --git a/include/sot/core/mailbox-vector.hh b/include/sot/core/mailbox-vector.hh
index f1ee0ff7..8ffdcbeb 100644
--- a/include/sot/core/mailbox-vector.hh
+++ b/include/sot/core/mailbox-vector.hh
@@ -11,10 +11,10 @@
#define __SOT_MAILBOX_VECTOR_HH
/* --- SOT PLUGIN --- */
-#include <sot/core/mailbox.hh>
-
#include <dynamic-graph/linear-algebra.h>
+#include <sot/core/mailbox.hh>
+
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
@@ -38,13 +38,13 @@ namespace sot {
#ifdef WIN32
class MAILBOX_VECTOR_EXPORT MailboxVector
: public Mailbox<dynamicgraph::Vector> {
-public:
+ public:
MailboxVector(const std::string &name);
};
#else
typedef Mailbox<dynamicgraph::Vector> MailboxVector;
#endif
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
-#endif // #ifndef __SOT_MAILBOX_HH
+#endif // #ifndef __SOT_MAILBOX_HH
diff --git a/include/sot/core/mailbox.hh b/include/sot/core/mailbox.hh
index af87346b..77943002 100644
--- a/include/sot/core/mailbox.hh
+++ b/include/sot/core/mailbox.hh
@@ -33,20 +33,22 @@ namespace sot {
namespace dg = dynamicgraph;
-template <class Object> struct MailboxTimestampedObject {
+template <class Object>
+struct MailboxTimestampedObject {
Object obj;
struct timeval timestamp;
};
-template <class Object> class Mailbox : public dg::Entity {
-public:
+template <class Object>
+class Mailbox : public dg::Entity {
+ public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
-public:
+ public:
typedef MailboxTimestampedObject<Object> sotTimestampedObject;
-public:
+ public:
Mailbox(const std::string &name);
~Mailbox(void);
@@ -58,13 +60,13 @@ public:
bool hasBeenUpdated(void);
-protected:
+ protected:
boost::timed_mutex mainObjectMutex;
Object mainObject;
struct timeval mainTimeStamp;
bool update;
-public: /* --- SIGNALS --- */
+ public: /* --- SIGNALS --- */
dynamicgraph::SignalTimeDependent<sotTimestampedObject, int> SOUT;
dynamicgraph::SignalTimeDependent<Object, int> objSOUT;
dynamicgraph::SignalTimeDependent<struct timeval, int> timeSOUT;
@@ -76,7 +78,8 @@ template <class Object>
struct signal_io<sot::MailboxTimestampedObject<Object> >
: signal_io_unimplemented<sot::MailboxTimestampedObject<Object> > {};
-template <> struct signal_io<timeval> : signal_io_unimplemented<timeval> {};
+template <>
+struct signal_io<timeval> : signal_io_unimplemented<timeval> {};
} /* namespace dynamicgraph */
-#endif // #ifndef __SOT_MAILBOX_HH
+#endif // #ifndef __SOT_MAILBOX_HH
diff --git a/include/sot/core/mailbox.hxx b/include/sot/core/mailbox.hxx
index a34b4b77..110a7121 100644
--- a/include/sot/core/mailbox.hxx
+++ b/include/sot/core/mailbox.hxx
@@ -20,7 +20,10 @@ namespace sot {
/* -------------------------------------------------------------------------- */
template <class Object>
Mailbox<Object>::Mailbox(const std::string &name)
- : Entity(name), mainObjectMutex(), mainObject(), update(false)
+ : Entity(name),
+ mainObjectMutex(),
+ mainObject(),
+ update(false)
,
SOUT(boost::bind(&Mailbox::get, this, _1, _2), sotNOSIGNAL,
@@ -33,14 +36,16 @@ Mailbox<Object>::Mailbox(const std::string &name)
SOUT.setDependencyType(TimeDependency<int>::BOOL_DEPENDENT);
}
-template <class Object> Mailbox<Object>::~Mailbox(void) {
+template <class Object>
+Mailbox<Object>::~Mailbox(void) {
boost::timed_mutex::scoped_lock lockMain(mainObjectMutex);
}
/* -------------------------------------------------------------------------- */
/* --- ACCESS --------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
-template <class Object> bool Mailbox<Object>::hasBeenUpdated(void) {
+template <class Object>
+bool Mailbox<Object>::hasBeenUpdated(void) {
boost::timed_mutex::scoped_try_lock lockMain(this->mainObjectMutex);
if (lockMain.owns_lock()) {
@@ -52,9 +57,9 @@ template <class Object> bool Mailbox<Object>::hasBeenUpdated(void) {
/* -------------------------------------------------------------------------- */
template <class Object>
-typename Mailbox<Object>::sotTimestampedObject &
-Mailbox<Object>::get(typename Mailbox<Object>::sotTimestampedObject &res,
- const int & /*dummy*/) {
+typename Mailbox<Object>::sotTimestampedObject &Mailbox<Object>::get(
+ typename Mailbox<Object>::sotTimestampedObject &res,
+ const int & /*dummy*/) {
boost::timed_mutex::scoped_try_lock lockMain(this->mainObjectMutex);
if (lockMain.owns_lock()) {
@@ -69,7 +74,8 @@ Mailbox<Object>::get(typename Mailbox<Object>::sotTimestampedObject &res,
}
/* -------------------------------------------------------------------------- */
-template <class Object> void Mailbox<Object>::post(const Object &value) {
+template <class Object>
+void Mailbox<Object>::post(const Object &value) {
boost::timed_mutex::scoped_lock lockMain(this->mainObjectMutex);
mainObject = value;
gettimeofday(&this->mainTimeStamp, NULL);
@@ -98,19 +104,19 @@ timeval &Mailbox<Object>::getTimestamp(struct timeval &res, const int &time) {
} /* namespace dynamicgraph */
/* Macro for template specialization */
#ifndef WIN32
-#define MAILBOX_TEMPLATE_SPE(S) \
- namespace dynamicgraph { \
- namespace sot { \
- template void Mailbox<S>::post(const S &obj); \
- template dynamicgraph::Vector &Mailbox<S>::getObject(S &res, \
- const int &time); \
- template bool Mailbox<S>::hasBeenUpdated(void); \
- template Mailbox<S>::~Mailbox(); \
- template Mailbox<S>::sotTimestampedObject & \
- Mailbox<S>::get(Mailbox<S>::sotTimestampedObject &res, const int &dummy); \
- template Mailbox<S>::Mailbox(const std::string &name); \
- } \
- } // namespace sot namespace dynamicgraph
-#endif // WIN32
-
-#endif // #ifdef __SOT_MAILBOX_T_CPP
+#define MAILBOX_TEMPLATE_SPE(S) \
+ namespace dynamicgraph { \
+ namespace sot { \
+ template void Mailbox<S>::post(const S &obj); \
+ template dynamicgraph::Vector &Mailbox<S>::getObject(S &res, \
+ const int &time); \
+ template bool Mailbox<S>::hasBeenUpdated(void); \
+ template Mailbox<S>::~Mailbox(); \
+ template Mailbox<S>::sotTimestampedObject &Mailbox<S>::get( \
+ Mailbox<S>::sotTimestampedObject &res, const int &dummy); \
+ template Mailbox<S>::Mailbox(const std::string &name); \
+ } \
+ } // namespace sot namespace dynamicgraph
+#endif // WIN32
+
+#endif // #ifdef __SOT_MAILBOX_T_CPP
diff --git a/include/sot/core/matrix-constant.hh b/include/sot/core/matrix-constant.hh
index f8fc26be..a3711953 100644
--- a/include/sot/core/matrix-constant.hh
+++ b/include/sot/core/matrix-constant.hh
@@ -23,12 +23,12 @@ namespace command {
namespace matrixConstant {
class Resize;
}
-} // namespace command
+} // namespace command
class MatrixConstant : public Entity {
friend class command::matrixConstant::Resize;
-public:
+ public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
@@ -37,7 +37,7 @@ public:
void setValue(const dynamicgraph::Matrix &inValue);
-public:
+ public:
MatrixConstant(const std::string &name);
virtual ~MatrixConstant(void) {}
@@ -45,5 +45,5 @@ public:
SignalTimeDependent<dynamicgraph::Matrix, int> SOUT;
};
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
diff --git a/include/sot/core/matrix-geometry.hh b/include/sot/core/matrix-geometry.hh
index 52046447..aefef558 100644
--- a/include/sot/core/matrix-geometry.hh
+++ b/include/sot/core/matrix-geometry.hh
@@ -9,10 +9,11 @@
#define __SOT_MATRIX_GEOMETRY_H__
/* --- Matrix --- */
-#include <Eigen/Core>
-#include <Eigen/Geometry>
#include <dynamic-graph/eigen-io.h>
#include <dynamic-graph/linear-algebra.h>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
#include <sot/core/api.hh>
#define MRAWDATA(x) x.data()
@@ -24,29 +25,29 @@
namespace dynamicgraph {
namespace sot {
-#define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix) \
- /** \ingroup matrixtypedefs */ \
- typedef Eigen::Matrix<Type, Size, Size> Matrix##SizeSuffix##TypeSuffix; \
- /** \ingroup matrixtypedefs */ \
- typedef Eigen::Matrix<Type, Size, 1> Vector##SizeSuffix##TypeSuffix; \
- /** \ingroup matrixtypedefs */ \
+#define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix) \
+ /** \ingroup matrixtypedefs */ \
+ typedef Eigen::Matrix<Type, Size, Size> Matrix##SizeSuffix##TypeSuffix; \
+ /** \ingroup matrixtypedefs */ \
+ typedef Eigen::Matrix<Type, Size, 1> Vector##SizeSuffix##TypeSuffix; \
+ /** \ingroup matrixtypedefs */ \
typedef Eigen::Matrix<Type, 1, Size> RowVector##SizeSuffix##TypeSuffix;
-#define EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, Size) \
- /** \ingroup matrixtypedefs */ \
- typedef Eigen::Matrix<Type, Size, Eigen::Dynamic> \
- Matrix##Size##X##TypeSuffix; \
- /** \ingroup matrixtypedefs */ \
+#define EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, Size) \
+ /** \ingroup matrixtypedefs */ \
+ typedef Eigen::Matrix<Type, Size, Eigen::Dynamic> \
+ Matrix##Size##X##TypeSuffix; \
+ /** \ingroup matrixtypedefs */ \
typedef Eigen::Matrix<Type, Eigen::Dynamic, Size> Matrix##X##Size##TypeSuffix;
-#define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix) \
- EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 1, 1) \
- EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 5, 5) \
- EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 6, 6) \
- EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 7, 7) \
- EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 1) \
- EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 5) \
- EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 6) \
+#define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix) \
+ EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 1, 1) \
+ EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 5, 5) \
+ EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 6, 6) \
+ EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 7, 7) \
+ EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 1) \
+ EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 5) \
+ EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 6) \
EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 7)
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(int, i)
@@ -85,7 +86,6 @@ typedef Eigen::Quaternion<double> SOT_CORE_EXPORT Quaternion;
typedef Eigen::Map<Quaternion> SOT_CORE_EXPORT QuaternionMap;
inline void buildFrom(const MatrixHomogeneous &MH, MatrixTwist &MT) {
-
Eigen::Vector3d _t = MH.translation();
MatrixRotation R(MH.linear());
Eigen::Matrix3d Tx;
@@ -99,7 +99,7 @@ inline void buildFrom(const MatrixHomogeneous &MH, MatrixTwist &MT) {
MT.block<3, 3>(3, 3) = R;
}
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
#endif /* #ifndef __SOT_MATRIX_GEOMETRY_H__ */
diff --git a/include/sot/core/matrix-svd.hh b/include/sot/core/matrix-svd.hh
index b9f1f040..d3f768ae 100644
--- a/include/sot/core/matrix-svd.hh
+++ b/include/sot/core/matrix-svd.hh
@@ -11,9 +11,10 @@
#define __SOT_MATRIX_SVD_H__
/* --- Matrix --- */
-#include <Eigen/SVD>
#include <dynamic-graph/linear-algebra.h>
+#include <Eigen/SVD>
+
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
@@ -34,6 +35,6 @@ void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix,
void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix,
const double threshold = 1e-6);
-} // namespace dynamicgraph
+} // namespace dynamicgraph
#endif /* #ifndef __SOT_MATRIX_SVD_H__ */
diff --git a/include/sot/core/memory-task-sot.hh b/include/sot/core/memory-task-sot.hh
index 3fcb0f22..aa8fb484 100644
--- a/include/sot/core/memory-task-sot.hh
+++ b/include/sot/core/memory-task-sot.hh
@@ -10,10 +10,11 @@
#ifndef __SOT_MEMORY_TASK_HH
#define __SOT_MEMORY_TASK_HH
-#include "sot/core/api.hh"
#include <sot/core/matrix-svd.hh>
#include <sot/core/task-abstract.hh>
+#include "sot/core/api.hh"
+
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
@@ -22,7 +23,7 @@ namespace dynamicgraph {
namespace sot {
class SOT_CORE_EXPORT MemoryTaskSOT : public TaskAbstract::MemoryTaskAbstract {
-public: // protected:
+ public: // protected:
typedef Eigen::Map<Matrix, Eigen::internal::traits<Matrix>::Alignment>
Kernel_t;
typedef Eigen::Map<const Matrix, Eigen::internal::traits<Matrix>::Alignment>
@@ -30,17 +31,16 @@ public: // protected:
/* Internal memory to reduce the dynamic allocation at task resolution. */
dynamicgraph::Vector err, tmpTask, tmpVar, tmpControl;
- dynamicgraph::Matrix Jt; //( nJ,mJ );
+ dynamicgraph::Matrix Jt; //( nJ,mJ );
- dynamicgraph::Matrix JK; //(nJ,mJ);
+ dynamicgraph::Matrix JK; //(nJ,mJ);
SVD_t svd;
Kernel_t kernel;
void resizeKernel(const Matrix::Index r, const Matrix::Index c) {
if (kernel.rows() != r || kernel.cols() != c) {
- if (kernelMem.size() < r * c)
- kernelMem.resize(r, c);
+ if (kernelMem.size() < r * c) kernelMem.resize(r, c);
new (&kernel) Kernel_t(kernelMem.data(), r, c);
}
}
@@ -50,7 +50,7 @@ public: // protected:
return kernel;
}
-public:
+ public:
/**
* \param mJ is the number of joints
* \param nJ the number of feature in the task
@@ -59,7 +59,7 @@ public:
void display(std::ostream &os) const;
-private:
+ private:
void initMemory(const Matrix::Index nJ, const Matrix::Index mJ);
Matrix kernelMem;
@@ -68,4 +68,4 @@ private:
} /* namespace sot */
} /* namespace dynamicgraph */
-#endif // __SOT_MEMORY_TASK_HH
+#endif // __SOT_MEMORY_TASK_HH
diff --git a/include/sot/core/motion-period.hh b/include/sot/core/motion-period.hh
index 4adbf274..537ed410 100644
--- a/include/sot/core/motion-period.hh
+++ b/include/sot/core/motion-period.hh
@@ -20,6 +20,7 @@
/* SOT */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
+
#include <sot/core/exception-task.hh>
/* --------------------------------------------------------------------- */
@@ -47,12 +48,11 @@ namespace dynamicgraph {
namespace sot {
class SOTMOTIONPERIOD_EXPORT MotionPeriod : public dynamicgraph::Entity {
-
-public:
+ public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
-protected:
+ protected:
enum MotionPeriodType { MOTION_CONSTANT, MOTION_SIN, MOTION_COS };
struct sotMotionParam {
@@ -69,10 +69,10 @@ protected:
void resize(const unsigned int &size);
/* --- SIGNALS ------------------------------------------------------------ */
-public:
+ public:
dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> motionSOUT;
-public:
+ public:
MotionPeriod(const std::string &name);
virtual ~MotionPeriod(void) {}
@@ -85,7 +85,7 @@ public:
} /* namespace sot */
} /* namespace dynamicgraph */
-#endif // #ifndef __SOT_JOINTLIMITS_HH__
+#endif // #ifndef __SOT_JOINTLIMITS_HH__
/*
* Local variables:
diff --git a/include/sot/core/multi-bound.hh b/include/sot/core/multi-bound.hh
index 755758b7..24bde5a4 100644
--- a/include/sot/core/multi-bound.hh
+++ b/include/sot/core/multi-bound.hh
@@ -19,10 +19,12 @@
#include <vector>
/* SOT */
-#include "sot/core/api.hh"
#include <dynamic-graph/signal-caster.h>
+
#include <sot/core/exception-task.hh>
+#include "sot/core/api.hh"
+
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
@@ -31,34 +33,34 @@ namespace dynamicgraph {
namespace sot {
class SOT_CORE_EXPORT MultiBound {
-public:
+ public:
enum MultiBoundModeType { MODE_SINGLE, MODE_DOUBLE };
enum SupInfType { BOUND_SUP, BOUND_INF };
-public: // protected:
+ public: // protected:
MultiBoundModeType mode;
double boundSingle;
double boundSup, boundInf;
bool boundSupSetup, boundInfSetup;
-public:
+ public:
MultiBound(const double x = 0.);
MultiBound(const double xi, const double xs);
MultiBound(const double x, const SupInfType bound);
MultiBound(const MultiBound &clone);
-public: // Acessors
+ public: // Acessors
MultiBoundModeType getMode(void) const;
double getSingleBound(void) const;
double getDoubleBound(const SupInfType bound) const;
bool getDoubleBoundSetup(const SupInfType bound) const;
-public: // Modifiors
+ public: // Modifiors
void setDoubleBound(SupInfType boundType, double boundValue);
void unsetDoubleBound(SupInfType boundType);
void setSingleBound(double boundValue);
-public:
+ public:
SOT_CORE_EXPORT friend std::ostream &operator<<(std::ostream &os,
const MultiBound &m);
SOT_CORE_EXPORT friend std::istream &operator>>(std::istream &is,
@@ -77,4 +79,4 @@ template <>
struct signal_io<sot::MultiBound> : signal_io_unimplemented<sot::MultiBound> {};
} /* namespace dynamicgraph */
-#endif // #ifndef __SOT_MultiBound_H__
+#endif // #ifndef __SOT_MultiBound_H__
diff --git a/include/sot/core/neck-limitation.hh b/include/sot/core/neck-limitation.hh
index 3d0a1abf..40f3ccc2 100644
--- a/include/sot/core/neck-limitation.hh
+++ b/include/sot/core/neck-limitation.hh
@@ -20,6 +20,7 @@
/* SOT */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
+
#include <sot/core/task-abstract.hh>
/* STD */
@@ -49,11 +50,11 @@ namespace dynamicgraph {
namespace sot {
class NeckLimitation_EXPORT NeckLimitation : public dynamicgraph::Entity {
-public:
+ public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
-protected:
+ protected:
unsigned int panRank, tiltRank;
static const unsigned int PAN_RANK_DEFAULT;
static const unsigned int TILT_RANK_DEFAULT;
@@ -66,24 +67,23 @@ protected:
static const double COEFF_AFFINE_DEFAULT;
static const double SIGN_TILT_DEFAULT;
-public: /* --- CONSTRUCTION --- */
+ public: /* --- CONSTRUCTION --- */
NeckLimitation(const std::string &name);
virtual ~NeckLimitation(void);
-public: /* --- SIGNAL --- */
+ public: /* --- SIGNAL --- */
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> jointSIN;
dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> jointSOUT;
-public: /* --- FUNCTIONS --- */
- dynamicgraph::Vector &
- computeJointLimitation(dynamicgraph::Vector &jointLimited,
- const int &timeSpec);
+ public: /* --- FUNCTIONS --- */
+ dynamicgraph::Vector &computeJointLimitation(
+ dynamicgraph::Vector &jointLimited, const int &timeSpec);
-public: /* --- PARAMS --- */
+ public: /* --- PARAMS --- */
virtual void display(std::ostream &os) const;
};
} /* namespace sot */
} /* namespace dynamicgraph */
-#endif // #ifndef __SOT_NeckLimitation_H__
+#endif // #ifndef __SOT_NeckLimitation_H__
diff --git a/include/sot/core/op-point-modifier.hh b/include/sot/core/op-point-modifier.hh
index d0de4871..5ac5fdcd 100644
--- a/include/sot/core/op-point-modifier.hh
+++ b/include/sot/core/op-point-modifier.hh
@@ -12,6 +12,7 @@
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
+
#include <sot/core/debug.hh>
#include <sot/core/matrix-geometry.hh>
@@ -46,18 +47,18 @@ namespace sot {
/// transformation.
///
class SOTOPPOINTMODIFIER_EXPORT OpPointModifier : public dynamicgraph::Entity {
-public:
+ public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
-public:
+ public:
dynamicgraph::SignalPtr<dynamicgraph::Matrix, int> jacobianSIN;
dynamicgraph::SignalPtr<MatrixHomogeneous, int> positionSIN;
dynamicgraph::SignalTimeDependent<dynamicgraph::Matrix, int> jacobianSOUT;
dynamicgraph::SignalTimeDependent<MatrixHomogeneous, int> positionSOUT;
-public:
+ public:
OpPointModifier(const std::string &name);
virtual ~OpPointModifier(void) {}
@@ -69,7 +70,7 @@ public:
void setTransformationBySignalName(std::istringstream &cmdArgs);
const Eigen::Matrix4d &getTransformation(void);
-private:
+ private:
MatrixHomogeneous transformation;
/* This bool tunes the effect of the modifier for end-effector Jacobian (ie
@@ -82,4 +83,4 @@ private:
} /* namespace sot */
} /* namespace dynamicgraph */
-#endif // __SOT_OP_POINT_MODIFIOR_H__
+#endif // __SOT_OP_POINT_MODIFIOR_H__
diff --git a/include/sot/core/parameter-server.hh b/include/sot/core/parameter-server.hh
index 8b71d887..004cd93c 100644
--- a/include/sot/core/parameter-server.hh
+++ b/include/sot/core/parameter-server.hh
@@ -35,12 +35,14 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#include "boost/assign.hpp"
#include <dynamic-graph/signal-helper.h>
+
#include <map>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/robot-utils.hh>
+#include "boost/assign.hpp"
+
namespace dynamicgraph {
namespace sot {
@@ -56,7 +58,7 @@ class SOTParameterServer_EXPORT ParameterServer
typedef ParameterServer EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
-public:
+ public:
/* --- CONSTRUCTOR ---- */
ParameterServer(const std::string &name);
@@ -94,7 +96,7 @@ public:
void setRightFootSoleXYZ(const dynamicgraph::Vector &);
void setRightFootForceSensorXYZ(const dynamicgraph::Vector &);
void setFootFrameName(const std::string &, const std::string &);
- void setHandFrameName(const std::string&, const std::string&);
+ void setHandFrameName(const std::string &, const std::string &);
void setImuJointName(const std::string &);
void displayRobotUtil();
/// @}
@@ -113,8 +115,8 @@ public:
m_robot_util->set_parameter<Type>(ParameterName, ParameterValue);
}
- template <typename Type> Type getParameter(const std::string &ParameterName) {
-
+ template <typename Type>
+ Type getParameter(const std::string &ParameterName) {
if (!m_initSucceeded) {
DYNAMIC_GRAPH_ENTITY_WARNING(*this)
<< "Cannot get parameter " << ParameterName
@@ -131,25 +133,26 @@ public:
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream &os) const;
-protected:
+ protected:
RobotUtilShrPtr m_robot_util;
- bool m_initSucceeded; /// true if the entity has been successfully initialized
+ bool
+ m_initSucceeded; /// true if the entity has been successfully initialized
double m_dt; /// control loop time period
- bool m_emergency_stop_triggered; /// true if an emergency condition as been
- /// triggered either by an other entity, or
- /// by control limit violation
- bool m_is_first_iter; /// true at the first iteration, false otherwise
+ bool m_emergency_stop_triggered; /// true if an emergency condition as been
+ /// triggered either by an other entity, or
+ /// by control limit violation
+ bool m_is_first_iter; /// true at the first iteration, false otherwise
int m_iter;
- double m_sleep_time; /// time to sleep at every iteration (to slow down
- /// simulation)
+ double m_sleep_time; /// time to sleep at every iteration (to slow down
+ /// simulation)
bool convertJointNameToJointId(const std::string &name, unsigned int &id);
bool isJointInRange(unsigned int id, double q);
void updateJointCtrlModesOutputSignal();
-}; // class ParameterServer
+}; // class ParameterServer
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
-#endif // #ifndef __sot_torque_control_control_manager_H__
+#endif // #ifndef __sot_torque_control_control_manager_H__
diff --git a/include/sot/core/periodic-call-entity.hh b/include/sot/core/periodic-call-entity.hh
index eb7bc82c..2bfe83a2 100644
--- a/include/sot/core/periodic-call-entity.hh
+++ b/include/sot/core/periodic-call-entity.hh
@@ -17,6 +17,7 @@
/* SOT */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
+
#include <sot/core/periodic-call-entity.hh>
#include <sot/core/periodic-call.hh>
/* STD */
@@ -55,8 +56,7 @@ namespace sot {
class PeriodicCallEntity_EXPORT PeriodicCallEntity
: public Entity,
protected sot::PeriodicCall {
-
-public:
+ public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
@@ -68,17 +68,17 @@ public:
/* --- FUNCTIONS ------------------------------------------------------------
*/
-public:
+ public:
PeriodicCallEntity(const std::string &name);
virtual ~PeriodicCallEntity(void) {}
virtual void display(std::ostream &os) const;
};
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
-#endif // #ifndef __SOT_PERIODICCALL_ENTITY_HH__
+#endif // #ifndef __SOT_PERIODICCALL_ENTITY_HH__
/*
* Local variables:
diff --git a/include/sot/core/periodic-call.hh b/include/sot/core/periodic-call.hh
index b5b223ed..1fa1c1a3 100644
--- a/include/sot/core/periodic-call.hh
+++ b/include/sot/core/periodic-call.hh
@@ -17,6 +17,7 @@
/* SOT */
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal-base.h>
+
#include <sot/core/api.hh>
/* STD */
#include <list>
@@ -34,7 +35,7 @@ namespace sot {
\class PeriodicCall
*/
class SOT_CORE_EXPORT PeriodicCall {
-protected:
+ protected:
struct SignalToCall {
dynamicgraph::SignalBase<int> *signal;
unsigned int downsamplingFactor;
@@ -57,7 +58,7 @@ protected:
/* --- FUNCTIONS ------------------------------------------------------------
*/
-public:
+ public:
PeriodicCall(void);
virtual ~PeriodicCall(void) {}
@@ -79,10 +80,10 @@ public:
void display(std::ostream &os) const;
};
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
-#endif // #ifndef __SOT_PERIODICCALL_HH__
+#endif // #ifndef __SOT_PERIODICCALL_HH__
/*
* Local variables:
diff --git a/include/sot/core/pool.hh b/include/sot/core/pool.hh
index 87a92a67..f68118de 100644
--- a/include/sot/core/pool.hh
+++ b/include/sot/core/pool.hh
@@ -20,11 +20,13 @@
#include <string>
/* --- SOT --- */
-#include "sot/core/api.hh"
#include <dynamic-graph/pool.h>
#include <dynamic-graph/signal-base.h>
+
#include <sot/core/exception-factory.hh>
+#include "sot/core/api.hh"
+
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
@@ -59,7 +61,7 @@ class TaskAbstract;
It also returns references to signals from their fully-qualified names.
*/
class SOT_CORE_EXPORT PoolStorage {
-public:
+ public:
/*! \name Define types to simplify the writing
@{
*/
@@ -70,7 +72,7 @@ public:
typedef std::map<std::string, FeatureAbstract *> Features;
/*! @} */
-protected:
+ protected:
/*! \name Fields of the class to manage the three entities.
Also the name is singular, those are true sets.
@{
@@ -83,7 +85,7 @@ protected:
Features feature;
/*! @} */
-public:
+ public:
/*! \brief Default destructor */
~PoolStorage(void);
@@ -117,7 +119,7 @@ public:
void writeGraph(const std::string &aFileName);
void writeCompletionList(std::ostream &os);
-private:
+ private:
PoolStorage();
static PoolStorage *instance_;
};
diff --git a/include/sot/core/reader.hh b/include/sot/core/reader.hh
index fee84b22..296f69da 100644
--- a/include/sot/core/reader.hh
+++ b/include/sot/core/reader.hh
@@ -30,6 +30,7 @@
#include <dynamic-graph/signal-base.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
+
#include <sot/core/flags.hh>
/* --------------------------------------------------------------------- */
@@ -58,12 +59,12 @@ using dynamicgraph::sot::Flags;
class SOTREADER_EXPORT sotReader : public Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
-public:
+ public:
SignalPtr<Flags, int> selectionSIN;
SignalTimeDependent<dynamicgraph::Vector, int> vectorSOUT;
SignalTimeDependent<dynamicgraph::Matrix, int> matrixSOUT;
-public:
+ public:
sotReader(const std::string n);
virtual ~sotReader(void) {}
@@ -71,7 +72,7 @@ public:
void clear(void);
void rewind(void);
-protected:
+ protected:
typedef std::list<std::vector<double> > DataType;
DataType dataSet;
DataType::const_iterator currentData;
@@ -85,7 +86,7 @@ protected:
const unsigned int time);
void resize(const int &nbRow, const int &nbCol);
-public:
+ public:
/* --- PARAMS --- */
void display(std::ostream &os) const;
virtual void initCommands();
diff --git a/include/sot/core/robot-simu.hh b/include/sot/core/robot-simu.hh
index 50d232d0..bc9bd092 100644
--- a/include/sot/core/robot-simu.hh
+++ b/include/sot/core/robot-simu.hh
@@ -19,11 +19,12 @@
#include <dynamic-graph/linear-algebra.h>
/* SOT */
-#include "sot/core/api.hh"
-#include "sot/core/device.hh"
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
+#include "sot/core/api.hh"
+#include "sot/core/device.hh"
+
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
@@ -46,12 +47,12 @@ namespace sot {
/* --------------------------------------------------------------------- */
class SOT_ROBOT_SIMU_EXPORT RobotSimu : public Device {
-public:
+ public:
RobotSimu(const std::string &inName);
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
};
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
#endif /* #ifndef DYNAMICGRAPH_SOT_ROBOT_SIMU_HH */
diff --git a/include/sot/core/robot-utils.hh b/include/sot/core/robot-utils.hh
index 03c7d421..fa256e76 100644
--- a/include/sot/core/robot-utils.hh
+++ b/include/sot/core/robot-utils.hh
@@ -13,16 +13,15 @@
/* --------------------------------------------------------------------- */
#include <map>
-
#include <pinocchio/fwd.hpp>
// keep pinocchio before boost
-#include <boost/assign.hpp>
-#include <boost/property_tree/ptree.hpp>
-
#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/logger.h>
#include <dynamic-graph/signal-helper.h>
+
+#include <boost/assign.hpp>
+#include <boost/property_tree/ptree.hpp>
#include <sot/core/matrix-geometry.hh>
namespace dynamicgraph {
@@ -40,15 +39,14 @@ struct SOT_CORE_EXPORT JointLimits {
typedef Eigen::VectorXd::Index Index;
class SOT_CORE_EXPORT ExtractJointMimics {
-
-public:
+ public:
/// Constructor
ExtractJointMimics(std::string &robot_model);
/// Get mimic joints.
const std::vector<std::string> &get_mimic_joints();
-private:
+ private:
void go_through(boost::property_tree::ptree &pt, int level, int stage);
// Create empty property tree object
@@ -112,7 +110,7 @@ struct SOT_CORE_EXPORT ForceUtil {
void display(std::ostream &out) const;
-}; // struct ForceUtil
+}; // struct ForceUtil
struct SOT_CORE_EXPORT FootUtil {
/// Position of the foot soles w.r.t. the frame of the foot
@@ -133,7 +131,7 @@ struct SOT_CORE_EXPORT HandUtil {
};
struct SOT_CORE_EXPORT RobotUtil {
-public:
+ public:
RobotUtil();
/// Forces data
@@ -287,7 +285,7 @@ public:
/** Access to property tree directly */
boost::property_tree::ptree &get_property_tree();
-protected:
+ protected:
Logger logger_;
/** \brief Map of the parameters: map of strings. */
@@ -295,7 +293,7 @@ protected:
/** \brief Property tree */
boost::property_tree::ptree property_tree_;
-}; // struct RobotUtil
+}; // struct RobotUtil
/// Accessors - This should be changed to RobotUtilPtrShared
typedef std::shared_ptr<RobotUtil> RobotUtilShrPtr;
@@ -308,7 +306,7 @@ std::shared_ptr<std::vector<std::string> > getListOfRobots();
bool base_se3_to_sot(ConstRefVector pos, ConstRefMatrix R, RefVector q_sot);
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
-#endif // sot_torque_control_common_h_
+#endif // sot_torque_control_common_h_
diff --git a/include/sot/core/sequencer.hh b/include/sot/core/sequencer.hh
index 0a57cc3c..96599521 100644
--- a/include/sot/core/sequencer.hh
+++ b/include/sot/core/sequencer.hh
@@ -20,6 +20,7 @@
/* SOT */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
+
#include <sot/core/task-abstract.hh>
/* STD */
@@ -53,17 +54,17 @@ class Sot;
class SOTSEQUENCER_EXPORT Sequencer : public dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
-public:
+ public:
class sotEventAbstract {
- public:
+ public:
enum sotEventType { EVENT_ADD, EVENT_RM, EVENT_CMD };
- protected:
+ protected:
std::string name;
void setName(const std::string &name_) { name = name_; }
int eventType;
- public:
+ public:
sotEventAbstract(const std::string &name) : name(name){};
virtual ~sotEventAbstract(void) {}
virtual const std::string &getName() const { return name; }
@@ -72,7 +73,7 @@ public:
virtual void display(std::ostream &os) const { os << name; }
};
-protected:
+ protected:
Sot *sotPtr;
typedef std::list<sotEventAbstract *> TaskList;
typedef std::map<unsigned int, TaskList> TaskMap;
@@ -85,26 +86,26 @@ protected:
std::ostream *outputStreamPtr;
bool noOutput; /*! if true, display nothing standard output on except errors*/
-public: /* --- CONSTRUCTION --- */
+ public: /* --- CONSTRUCTION --- */
Sequencer(const std::string &name);
virtual ~Sequencer(void);
-public: /* --- TASK MANIP --- */
+ public: /* --- TASK MANIP --- */
void setSotRef(Sot *sot) { sotPtr = sot; }
void addTask(sotEventAbstract *task, const unsigned int time);
void rmTask(int eventType, const std::string &name, const unsigned int time);
void clearAll();
-public: /* --- SIGNAL --- */
+ public: /* --- SIGNAL --- */
dynamicgraph::SignalTimeDependent<int, int> triggerSOUT;
-public: /* --- FUNCTIONS --- */
+ public: /* --- FUNCTIONS --- */
int &trigger(int &dummy, const int &time);
-public: /* --- PARAMS --- */
+ public: /* --- PARAMS --- */
virtual void display(std::ostream &os) const;
};
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
-#endif // #ifndef __SOT_SOTSEQUENCER_H__
+#endif // #ifndef __SOT_SOTSEQUENCER_H__
diff --git a/include/sot/core/smooth-reach.hh b/include/sot/core/smooth-reach.hh
index 3f64dbc5..327383f9 100644
--- a/include/sot/core/smooth-reach.hh
+++ b/include/sot/core/smooth-reach.hh
@@ -43,11 +43,11 @@ namespace sot {
/* --------------------------------------------------------------------- */
class SOTSMOOTHREACH_EXPORT SmoothReach : public dynamicgraph::Entity {
-public:
+ public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName() const { return CLASS_NAME; }
-private:
+ private:
dynamicgraph::Vector start, goal;
int startTime, lengthTime;
bool isStarted, isParam;
@@ -56,15 +56,15 @@ private:
double smoothFunction(double x);
-public: /* --- CONSTRUCTION --- */
+ public: /* --- CONSTRUCTION --- */
SmoothReach(const std::string &name);
virtual ~SmoothReach(void){};
-public: /* --- SIGNAL --- */
+ public: /* --- SIGNAL --- */
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> startSIN;
dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> goalSOUT;
-public: /* --- FUNCTION --- */
+ public: /* --- FUNCTION --- */
dynamicgraph::Vector &goalSOUT_function(dynamicgraph::Vector &goal,
const int &time);
@@ -75,7 +75,7 @@ public: /* --- FUNCTION --- */
void setSmoothing(const int &mode, const double ¶m);
-public: /* --- PARAMS --- */
+ public: /* --- PARAMS --- */
virtual void display(std::ostream &os) const;
void initCommands(void);
};
diff --git a/include/sot/core/sot-loader.hh b/include/sot/core/sot-loader.hh
index 135c5ce5..782db395 100644
--- a/include/sot/core/sot-loader.hh
+++ b/include/sot/core/sot-loader.hh
@@ -45,7 +45,7 @@ namespace sot {
* And then use the oneIteration to execute the graph.
*/
class SotLoader {
-protected:
+ protected:
/// \brief Check if the dynamic graph is running or not.
bool dynamic_graph_stopped_;
@@ -72,7 +72,7 @@ protected:
/// is not responsible for it's life time.
std::string device_name_;
-public:
+ public:
/// \brief Default constructor.
SotLoader();
/// \brief Default destructor.
diff --git a/include/sot/core/sot.hh b/include/sot/core/sot.hh
index 30d3ba0c..349d6521 100644
--- a/include/sot/core/sot.hh
+++ b/include/sot/core/sot.hh
@@ -22,6 +22,7 @@
/* SOT */
#include <dynamic-graph/entity.h>
+
#include <sot/core/flags.hh>
#include <sot/core/task-abstract.hh>
@@ -54,18 +55,18 @@ namespace sot {
through the shell.
*/
class SOTSOT_CORE_EXPORT Sot : public Entity {
-public:
+ public:
/*! \brief Specify the name of the class entity. */
static const std::string CLASS_NAME;
-public:
+ public:
/*! \brief Returns the name of this class. */
virtual const std::string &getClassName() const { return CLASS_NAME; }
/*! \brief Defines a type for a list of tasks */
typedef std::list<TaskAbstract *> StackType;
-protected:
+ protected:
/*! \brief This field is a list of controllers
managed by the stack of tasks. */
StackType stack;
@@ -86,9 +87,9 @@ protected:
*/
double maxControlIncrementSquaredNorm;
-public:
+ public:
/*! \brief Threshold to compute the dumped pseudo inverse. */
- static const double INVERSION_THRESHOLD_DEFAULT; // = 1e-4;
+ static const double INVERSION_THRESHOLD_DEFAULT; // = 1e-4;
/* static const double DIRECTIONAL_THRESHOLD_DEFAULT = 1e-2; */
/* static const bool USE_CONTI_INVERSE_DEFAULT = false; */
@@ -97,7 +98,7 @@ public:
static void taskVectorToMlVector(const VectorMultiBound &taskVector,
Vector &err);
-public:
+ public:
/*! \brief Default constructor */
Sot(const std::string &name);
~Sot(void) { /* TODO!! */
@@ -151,7 +152,7 @@ public:
virtual const unsigned int &getNbDof() const { return nbJoints; }
/*! @} */
-public: /* --- CONTROL --- */
+ public: /* --- CONTROL --- */
/*! \name Methods to compute the control law following the
recursive definition of the stack of tasks.
@{
@@ -163,7 +164,7 @@ public: /* --- CONTROL --- */
/*! @} */
-public: /* --- DISPLAY --- */
+ public: /* --- DISPLAY --- */
/*! \name Methods to display the stack of tasks.
@{
*/
@@ -173,7 +174,7 @@ public: /* --- DISPLAY --- */
SOTSOT_CORE_EXPORT friend std::ostream &operator<<(std::ostream &os,
const Sot &sot);
/*! @} */
-public: /* --- SIGNALS --- */
+ public: /* --- SIGNALS --- */
/*! \name Methods to handle signals
@{
*/
@@ -199,7 +200,7 @@ public: /* --- SIGNALS --- */
* os. */
virtual std::ostream &writeGraph(std::ostream &os) const;
};
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
#endif /* #ifndef __SOT_SOT_HH */
diff --git a/include/sot/core/stop-watch.hh b/include/sot/core/stop-watch.hh
index 3fb95407..ec270e04 100644
--- a/include/sot/core/stop-watch.hh
+++ b/include/sot/core/stop-watch.hh
@@ -39,15 +39,15 @@ WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
// Generic stopwatch exception class
struct StopwatchException {
-public:
+ public:
StopwatchException(std::string error) : error(error) {}
std::string error;
};
enum StopwatchMode {
- NONE = 0, // Clock is not initialized
- CPU_TIME = 1, // Clock calculates time ranges using ctime and CLOCKS_PER_SEC
- REAL_TIME = 2 // Clock calculates time by asking the operating system how
+ NONE = 0, // Clock is not initialized
+ CPU_TIME = 1, // Clock calculates time ranges using ctime and CLOCKS_PER_SEC
+ REAL_TIME = 2 // Clock calculates time by asking the operating system how
// much real time passed
};
@@ -145,7 +145,7 @@ enum StopwatchMode {
*/
class Stopwatch {
-public:
+ public:
/** Constructor */
Stopwatch(StopwatchMode _mode = NONE);
@@ -209,13 +209,17 @@ public:
/** Take time, depends on mode */
long double take_time();
-protected:
+ protected:
/** Struct to hold the performance data */
struct PerformanceData {
-
PerformanceData()
- : clock_start(0), total_time(0), min_time(0), max_time(0), last_time(0),
- paused(false), stops(0) {}
+ : clock_start(0),
+ total_time(0),
+ min_time(0),
+ max_time(0),
+ last_time(0),
+ paused(false),
+ stops(0) {}
/** Start time */
long double clock_start;
diff --git a/include/sot/core/switch.hh b/include/sot/core/switch.hh
index 804d059e..09864415 100644
--- a/include/sot/core/switch.hh
+++ b/include/sot/core/switch.hh
@@ -22,7 +22,7 @@ template <typename Value, typename Time = int>
class SOT_CORE_DLLAPI Switch : public VariadicAbstract<Value, Value, Time> {
DYNAMIC_GRAPH_ENTITY_DECL();
-public:
+ public:
typedef VariadicAbstract<Value, Value, Time> Base;
Switch(const std::string &name)
@@ -36,14 +36,16 @@ public:
this->SOUT.addDependency(boolSelectionSIN);
using command::makeCommandVoid1;
- std::string docstring = "\n"
- " Set number of input signals\n";
+ std::string docstring =
+ "\n"
+ " Set number of input signals\n";
this->addCommand(
"setSignalNumber",
makeCommandVoid1(*(Base *)this, &Base::setSignalNumber, docstring));
- docstring = "\n"
- " Get number of input signals\n";
+ docstring =
+ "\n"
+ " Get number of input signals\n";
this->addCommand("getSignalNumber",
new command::Getter<Base, int>(
*this, &Base::getSignalNumber, docstring));
@@ -59,7 +61,7 @@ public:
SignalPtr<int, Time> selectionSIN;
SignalPtr<bool, Time> boolSelectionSIN;
-private:
+ private:
Value &signal(Value &ret, const Time &time) {
int sel;
if (selectionSIN.isPlugged()) {
@@ -75,6 +77,6 @@ private:
return ret;
}
};
-} // namespace sot
-} // namespace dynamicgraph
-#endif // __SOT_SWITCH_H__
+} // namespace sot
+} // namespace dynamicgraph
+#endif // __SOT_SWITCH_H__
diff --git a/include/sot/core/task-abstract.hh b/include/sot/core/task-abstract.hh
index 248f1ec5..99000b0e 100644
--- a/include/sot/core/task-abstract.hh
+++ b/include/sot/core/task-abstract.hh
@@ -15,18 +15,21 @@
/* --------------------------------------------------------------------- */
/* Matrix */
-#include <Eigen/SVD>
#include <dynamic-graph/linear-algebra.h>
+#include <Eigen/SVD>
+
/* STD */
#include <string>
/* SOT */
-#include "sot/core/api.hh"
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
+
#include <sot/core/multi-bound.hh>
+#include "sot/core/api.hh"
+
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
@@ -46,17 +49,17 @@ namespace sot {
/// features that are stored in the task.
class SOT_CORE_EXPORT TaskAbstract : public dynamicgraph::Entity {
-public:
+ public:
/* Use a derivative of this class to store computational memory. */
class MemoryTaskAbstract {
- public:
+ public:
int timeLastChange;
- public:
+ public:
MemoryTaskAbstract(void) : timeLastChange(0){};
virtual ~MemoryTaskAbstract(void){};
- public:
+ public:
virtual void display(std::ostream &os) const = 0;
friend std::ostream &operator<<(std::ostream &os,
const MemoryTaskAbstract &tcm) {
@@ -65,16 +68,16 @@ public:
}
};
-public:
+ public:
MemoryTaskAbstract *memoryInternal;
-protected:
+ protected:
void taskRegistration(void);
-public:
+ public:
TaskAbstract(const std::string &n);
-public: /* --- SIGNALS --- */
+ public: /* --- SIGNALS --- */
dynamicgraph::SignalTimeDependent<VectorMultiBound, int> taskSOUT;
dynamicgraph::SignalTimeDependent<dynamicgraph::Matrix, int> jacobianSOUT;
};
diff --git a/include/sot/core/task-conti.hh b/include/sot/core/task-conti.hh
index 4098ea32..43edcf5a 100644
--- a/include/sot/core/task-conti.hh
+++ b/include/sot/core/task-conti.hh
@@ -21,12 +21,11 @@
#include <string>
/* SOT */
+#include <sot/core/exception-task.hh>
#include <sot/core/feature-abstract.hh>
#include <sot/core/flags.hh>
#include <sot/core/task.hh>
-#include <sot/core/exception-task.hh>
-
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
@@ -49,18 +48,18 @@ namespace dynamicgraph {
namespace sot {
class SOTTASKCONTI_EXPORT TaskConti : public Task {
-protected:
+ protected:
enum TimeRefValues { TIME_REF_UNSIGNIFICANT = -1, TIME_REF_TO_BE_SET = -2 };
int timeRef;
double mu;
dynamicgraph::Vector q0;
-public:
+ public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
-public:
+ public:
TaskConti(const std::string &n);
void referenceTime(const unsigned int &t) { timeRef = t; }
@@ -71,7 +70,7 @@ public:
const int &time);
/* --- SIGNALS ------------------------------------------------------------ */
-public:
+ public:
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> controlPrevSIN;
/* --- DISPLAY ------------------------------------------------------------ */
diff --git a/include/sot/core/task-pd.hh b/include/sot/core/task-pd.hh
index 8c674924..a9ade836 100644
--- a/include/sot/core/task-pd.hh
+++ b/include/sot/core/task-pd.hh
@@ -39,14 +39,14 @@ namespace dynamicgraph {
namespace sot {
class SOTTASKPD_EXPORT TaskPD : public Task {
-public:
+ public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
dynamicgraph::Vector previousError;
double beta;
-public:
+ public:
TaskPD(const std::string &n);
/* --- COMPUTATION --- */
@@ -54,7 +54,7 @@ public:
VectorMultiBound &computeTaskModif(VectorMultiBound &error, int time);
/* --- SIGNALS ------------------------------------------------------------ */
-public:
+ public:
dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> errorDotSOUT;
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> errorDotSIN;
diff --git a/include/sot/core/task-unilateral.hh b/include/sot/core/task-unilateral.hh
index 3a704a52..055632c2 100644
--- a/include/sot/core/task-unilateral.hh
+++ b/include/sot/core/task-unilateral.hh
@@ -21,12 +21,11 @@
#include <string>
/* SOT */
+#include <sot/core/exception-task.hh>
#include <sot/core/feature-abstract.hh>
#include <sot/core/flags.hh>
#include <sot/core/task.hh>
-#include <sot/core/exception-task.hh>
-
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
@@ -49,21 +48,21 @@ namespace dynamicgraph {
namespace sot {
class SOTTASKUNILATERAL_EXPORT TaskUnilateral : public Task {
-protected:
+ protected:
std::list<FeatureAbstract *> featureList;
-public:
+ public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
-public:
+ public:
TaskUnilateral(const std::string &n);
/* --- COMPUTATION --- */
VectorMultiBound &computeTaskUnilateral(VectorMultiBound &res, int time);
/* --- SIGNALS ------------------------------------------------------------ */
-public:
+ public:
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> positionSIN;
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> referenceInfSIN;
dynamicgraph::SignalPtr<dynamicgraph::Vector, int> referenceSupSIN;
diff --git a/include/sot/core/task.hh b/include/sot/core/task.hh
index 53bbfb9b..8fd4772e 100644
--- a/include/sot/core/task.hh
+++ b/include/sot/core/task.hh
@@ -21,12 +21,11 @@
#include <string>
/* SOT */
+#include <sot/core/exception-task.hh>
#include <sot/core/feature-abstract.hh>
#include <sot/core/flags.hh>
#include <sot/core/task-abstract.hh>
-#include <sot/core/exception-task.hh>
-
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
@@ -71,16 +70,16 @@ namespace dynamicgraph {
namespace sot {
class SOTTASK_EXPORT Task : public TaskAbstract {
-public:
+ public:
typedef std::list<FeatureAbstract *> FeatureList_t;
-protected:
+ protected:
FeatureList_t featureList;
bool withDerivative;
DYNAMIC_GRAPH_ENTITY_DECL();
-public:
+ public:
Task(const std::string &n);
void initCommands(void);
@@ -105,7 +104,7 @@ public:
int time);
/* --- SIGNALS ------------------------------------------------------------ */
-public:
+ public:
dynamicgraph::SignalPtr<double, int> controlGainSIN;
dynamicgraph::SignalPtr<double, int> dampingGainSINOUT;
dynamicgraph::SignalPtr<Flags, int> controlSelectionSIN;
diff --git a/include/sot/core/time-stamp.hh b/include/sot/core/time-stamp.hh
index ceb6c696..46167159 100644
--- a/include/sot/core/time-stamp.hh
+++ b/include/sot/core/time-stamp.hh
@@ -27,6 +27,7 @@
/* SOT */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
+
#include <sot/core/debug.hh>
/* --------------------------------------------------------------------- */
@@ -51,23 +52,23 @@ namespace dynamicgraph {
namespace sot {
class TimeStamp_EXPORT TimeStamp : public dynamicgraph::Entity {
-public:
+ public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
-protected:
+ protected:
struct timeval val;
unsigned int offsetValue;
bool offsetSet;
-public:
+ public:
/* --- CONSTRUCTION --- */
TimeStamp(const std::string &name);
-public: /* --- DISPLAY --- */
+ public: /* --- DISPLAY --- */
virtual void display(std::ostream &os) const;
-public: /* --- SIGNALS --- */
+ public: /* --- SIGNALS --- */
/* These signals can be called several time per period, given
* each time a different results. Useful for chronos. */
dynamicgraph::Signal<dynamicgraph::Vector, int> timeSOUT;
@@ -78,7 +79,7 @@ public: /* --- SIGNALS --- */
dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> timeOnceSOUT;
dynamicgraph::SignalTimeDependent<double, int> timeOnceDoubleSOUT;
-protected: /* --- SIGNAL FUNCTIONS --- */
+ protected: /* --- SIGNAL FUNCTIONS --- */
dynamicgraph::Vector &getTimeStamp(dynamicgraph::Vector &res,
const int &time);
double &getTimeStampDouble(const dynamicgraph::Vector &vect, double &res);
diff --git a/include/sot/core/timer.hh b/include/sot/core/timer.hh
index 0913968e..dd995dee 100644
--- a/include/sot/core/timer.hh
+++ b/include/sot/core/timer.hh
@@ -22,12 +22,14 @@
// When including Winsock2.h, the MAL must be included first
#include <Winsock2.h>
#include <dynamic-graph/linear-algebra.h>
+
#include <sot/core/utils-windows.hh>
#endif /*WIN32*/
/* SOT */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
+
#include <sot/core/debug.hh>
/* --------------------------------------------------------------------- */
@@ -48,21 +50,22 @@
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-template <class T> class Timer_EXPORT Timer : public dynamicgraph::Entity {
-public:
+template <class T>
+class Timer_EXPORT Timer : public dynamicgraph::Entity {
+ public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
-protected:
+ protected:
struct timeval t0, t1;
clock_t c0, c1;
double dt;
-public:
+ public:
/* --- CONSTRUCTION --- */
Timer(const std::string &name);
-public: /* --- DISPLAY --- */
+ public: /* --- DISPLAY --- */
virtual void display(std::ostream &os) const;
Timer_EXPORT friend std::ostream &operator<<(std::ostream &os,
const Timer<T> &timer) {
@@ -70,19 +73,20 @@ public: /* --- DISPLAY --- */
return os;
}
-public: /* --- SIGNALS --- */
+ public: /* --- SIGNALS --- */
dynamicgraph::SignalPtr<T, int> sigSIN;
dynamicgraph::SignalTimeDependent<T, int> sigSOUT;
dynamicgraph::SignalTimeDependent<T, int> sigClockSOUT;
dynamicgraph::Signal<double, int> timerSOUT;
-protected: /* --- SIGNAL FUNCTIONS --- */
+ protected: /* --- SIGNAL FUNCTIONS --- */
void plug(dynamicgraph::Signal<T, int> &sig) {
sigSIN = &sig;
dt = 0.;
}
- template <bool UseClock> T &compute(T &t, const int &time) {
+ template <bool UseClock>
+ T &compute(T &t, const int &time) {
sotDEBUGIN(15);
if (UseClock) {
c0 = clock();
@@ -131,7 +135,9 @@ void cmdChrono(const std::string &cmd, std::istringstream &args,
/* --- CONSTRUCTION ---------------------------------------------------- */
template <class T>
Timer<T>::Timer(const std::string &name)
- : Entity(name), dt(0.), sigSIN(NULL, "Timer(" + name + ")::input(T)::sin"),
+ : Entity(name),
+ dt(0.),
+ sigSIN(NULL, "Timer(" + name + ")::input(T)::sin"),
sigSOUT(boost::bind(&Timer::compute<false>, this, _1, _2), sigSIN,
"Timer(" + name + ")::output(T)::sout"),
sigClockSOUT(boost::bind(&Timer::compute<true>, this, _1, _2), sigSIN,
@@ -145,7 +151,8 @@ Timer<T>::Timer(const std::string &name)
}
/* --- DISPLAY --------------------------------------------------------- */
-template <class T> void Timer<T>::display(std::ostream &os) const {
+template <class T>
+void Timer<T>::display(std::ostream &os) const {
os << "Timer <" << sigSIN << "> : " << dt << "ms." << std::endl;
}
diff --git a/include/sot/core/trajectory.hh b/include/sot/core/trajectory.hh
index b5bb1e4f..f1edeea5 100644
--- a/include/sot/core/trajectory.hh
+++ b/include/sot/core/trajectory.hh
@@ -11,13 +11,12 @@
// Matrix
#include <dynamic-graph/linear-algebra.h>
-#include <sot/core/api.hh>
+#include <dynamic-graph/signal-caster.h>
#include <boost/array.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/regex.hpp>
-
-#include <dynamic-graph/signal-caster.h>
+#include <sot/core/api.hh>
namespace dg = dynamicgraph;
namespace ba = boost::assign;
@@ -28,10 +27,10 @@ namespace sot {
class Trajectory;
class RulesJointTrajectory {
-protected:
+ protected:
Trajectory &TrajectoryToFill_;
-public:
+ public:
unsigned int dbg_level;
/// \brief Strings specifying the grammar of the structure.
@@ -51,13 +50,13 @@ public:
/// \brief parse_string will fill TrajectoryToFill with string atext.
void parse_string(std::string &atext);
-protected:
+ protected:
/// \brief General parsing method of text with regexp e. The results are given
/// in what. The remaining text is left in sub_text.
- bool
- search_exp_sub_string(std::string &text,
- boost::match_results<std::string::const_iterator> &what,
- boost::regex &e, std::string &sub_text);
+ bool search_exp_sub_string(
+ std::string &text,
+ boost::match_results<std::string::const_iterator> &what, boost::regex &e,
+ std::string &sub_text);
/// \brief Find and store the header.
/// This method is looking for:
/// unsigned int seq.
@@ -83,7 +82,7 @@ protected:
};
class SOT_CORE_EXPORT timestamp {
-public:
+ public:
unsigned long int secs_;
unsigned long int nsecs_;
timestamp() : secs_(0), nsecs_(0) {}
@@ -96,8 +95,7 @@ public:
nsecs_ = lnsecs;
}
bool operator==(const timestamp &other) const {
- if ((secs_ != other.secs_) || (nsecs_ != other.nsecs_))
- return false;
+ if ((secs_ != other.secs_) || (nsecs_ != other.nsecs_)) return false;
return true;
}
friend std::ostream &operator<<(std::ostream &stream, const timestamp &ats) {
@@ -107,7 +105,7 @@ public:
};
class SOT_CORE_EXPORT Header {
-public:
+ public:
unsigned int seq_;
timestamp stamp_;
std::string frame_id_;
@@ -115,8 +113,7 @@ public:
};
class SOT_CORE_EXPORT JointTrajectoryPoint {
-
-public:
+ public:
std::vector<double> positions_;
std::vector<double> velocities_;
std::vector<double> accelerations_;
@@ -132,20 +129,20 @@ public:
for (std::size_t arrayId = 0; arrayId < names.size(); ++arrayId) {
switch (arrayId) {
- case (0):
- points = &positions_;
- break;
- case (1):
- points = &velocities_;
- break;
- case (2):
- points = &accelerations_;
- break;
- case (3):
- points = &efforts_;
- break;
- default:
- assert(0);
+ case (0):
+ points = &positions_;
+ break;
+ case (1):
+ points = &velocities_;
+ break;
+ case (2):
+ points = &accelerations_;
+ break;
+ case (3):
+ points = &efforts_;
+ break;
+ default:
+ assert(0);
}
std::vector<double>::const_iterator it_db;
@@ -158,27 +155,26 @@ public:
void transfer(const std::vector<double> &src, unsigned int vecId) {
switch (vecId) {
- case (0):
- positions_ = src;
- break;
- case (1):
- velocities_ = src;
- break;
- case (2):
- accelerations_ = src;
- break;
- case (3):
- efforts_ = src;
- break;
- default:
- assert(0);
+ case (0):
+ positions_ = src;
+ break;
+ case (1):
+ velocities_ = src;
+ break;
+ case (2):
+ accelerations_ = src;
+ break;
+ case (3):
+ efforts_ = src;
+ break;
+ default:
+ assert(0);
}
}
};
class SOT_CORE_EXPORT Trajectory {
-
-public:
+ public:
Trajectory();
Trajectory(const Trajectory ©);
virtual ~Trajectory();
@@ -193,11 +189,11 @@ public:
int deserialize(std::istringstream &is);
void display(std::ostream &) const;
};
-} // namespace sot
+} // namespace sot
template <>
struct signal_io<sot::Trajectory> : signal_io_unimplemented<sot::Trajectory> {};
-} // namespace dynamicgraph
+} // namespace dynamicgraph
#endif /* #ifndef SOT_TRAJECTORY_H__ */
diff --git a/include/sot/core/unary-op.hh b/include/sot/core/unary-op.hh
index 650a5c94..a43ab908 100644
--- a/include/sot/core/unary-op.hh
+++ b/include/sot/core/unary-op.hh
@@ -25,13 +25,14 @@
namespace dynamicgraph {
namespace sot {
-template <typename Operator> class UnaryOp : public Entity {
+template <typename Operator>
+class UnaryOp : public Entity {
Operator op;
typedef typename Operator::Tin Tin;
typedef typename Operator::Tout Tout;
typedef UnaryOp<Operator> Self;
-public: /* --- CONSTRUCTION --- */
+ public: /* --- CONSTRUCTION --- */
static std::string getTypeInName(void) { return Operator::nameTypeIn(); }
static std::string getTypeOutName(void) { return Operator::nameTypeOut(); }
static const std::string CLASS_NAME;
@@ -41,8 +42,9 @@ public: /* --- CONSTRUCTION --- */
std::string getDocString() const { return op.getDocString(); }
UnaryOp(const std::string &name)
- : Entity(name), SIN(NULL, Self::CLASS_NAME + "(" + name + ")::input(" +
- Self::getTypeInName() + ")::sin"),
+ : Entity(name),
+ SIN(NULL, Self::CLASS_NAME + "(" + name + ")::input(" +
+ Self::getTypeInName() + ")::sin"),
SOUT(boost::bind(&Self::computeOperation, this, _1, _2), SIN,
Self::CLASS_NAME + "(" + name + ")::output(" +
Self::getTypeOutName() + ")::sout") {
@@ -52,20 +54,20 @@ public: /* --- CONSTRUCTION --- */
virtual ~UnaryOp(void){};
-public: /* --- SIGNAL --- */
+ public: /* --- SIGNAL --- */
SignalPtr<Tin, int> SIN;
SignalTimeDependent<Tout, int> SOUT;
-protected:
+ protected:
Tout &computeOperation(Tout &res, int time) {
const Tin &x1 = SIN(time);
op(x1, res);
return res;
}
-public: /* --- PARAMS --- */
+ public: /* --- PARAMS --- */
};
} /* namespace sot */
} /* namespace dynamicgraph */
-#endif // #ifndef SOT_CORE_UNARYOP_HH
+#endif // #ifndef SOT_CORE_UNARYOP_HH
diff --git a/include/sot/core/utils-windows.hh b/include/sot/core/utils-windows.hh
index c891b9a8..67364453 100644
--- a/include/sot/core/utils-windows.hh
+++ b/include/sot/core/utils-windows.hh
@@ -12,9 +12,9 @@
#ifdef WIN32
-#include "sot/core/api.hh"
-
#include < time.h >
+
+#include "sot/core/api.hh"
#define NOMINMAX
#include <Winsock2.h>
diff --git a/include/sot/core/variadic-op.hh b/include/sot/core/variadic-op.hh
index c03018f5..034660e7 100644
--- a/include/sot/core/variadic-op.hh
+++ b/include/sot/core/variadic-op.hh
@@ -19,14 +19,14 @@
/* SOT */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
+
#include <sot/core/flags.hh>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/pool.hh>
/* STD */
-#include <string>
-
#include <boost/function.hpp>
+#include <string>
namespace dynamicgraph {
namespace sot {
@@ -37,13 +37,14 @@ namespace sot {
template <typename Tin, typename Tout, typename Time>
class VariadicAbstract : public Entity {
-public: /* --- CONSTRUCTION --- */
+ public: /* --- CONSTRUCTION --- */
static std::string getTypeInName(void);
static std::string getTypeOutName(void);
VariadicAbstract(const std::string &name, const std::string &className)
- : Entity(name), SOUT(className + "(" + name + ")::output(" +
- getTypeOutName() + ")::sout"),
+ : Entity(name),
+ SOUT(className + "(" + name + ")::output(" + getTypeOutName() +
+ ")::sout"),
baseSigname(className + "(" + name + ")::input(" + getTypeInName() +
")::") {
signalRegistration(SOUT);
@@ -55,7 +56,7 @@ public: /* --- CONSTRUCTION --- */
}
};
-public: /* --- SIGNAL --- */
+ public: /* --- SIGNAL --- */
typedef SignalPtr<Tin, int> signal_t;
SignalTimeDependent<Tout, int> SOUT;
@@ -88,8 +89,7 @@ public: /* --- SIGNAL --- */
void setSignalNumber(const int &n) {
assert(n >= 0);
const std::size_t oldSize = signalsIN.size();
- for (std::size_t i = n; i < oldSize; ++i)
- _removeSignal(i);
+ for (std::size_t i = n; i < oldSize; ++i) _removeSignal(i);
signalsIN.resize(n, NULL);
// names.resize(n);
@@ -114,14 +114,14 @@ public: /* --- SIGNAL --- */
return signalsIN[i];
}
-protected:
+ protected:
std::vector<signal_t *> signalsIN;
// Use signal->shortName instead
// std::vector< std::string > names;
virtual void updateSignalNumber(int n) { (void)n; };
-private:
+ private:
void _removeSignal(const std::size_t i) {
// signalDeregistration(names[i]);
signalDeregistration(signalsIN[i]->shortName());
@@ -142,7 +142,7 @@ class VariadicOp : public VariadicAbstract<typename Operator::Tin,
typedef typename Operator::Tout Tout;
typedef VariadicOp<Operator> Self;
-public: /* --- CONSTRUCTION --- */
+ public: /* --- CONSTRUCTION --- */
Operator op;
typedef VariadicAbstract<Tin, Tout, int> Base;
@@ -161,7 +161,7 @@ public: /* --- CONSTRUCTION --- */
virtual ~VariadicOp(void){};
-protected:
+ protected:
Tout &computeOperation(Tout &res, int time) {
std::vector<const Tin *> in(this->signalsIN.size());
for (std::size_t i = 0; i < this->signalsIN.size(); ++i) {
@@ -174,7 +174,7 @@ protected:
inline void updateSignalNumber(int n) { op.updateSignalNumber(n); }
};
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
-#endif // #ifndef SOT_CORE_VARIADICOP_HH
+#endif // #ifndef SOT_CORE_VARIADICOP_HH
diff --git a/include/sot/core/vector-constant.hh b/include/sot/core/vector-constant.hh
index 82506e4b..b78297bd 100644
--- a/include/sot/core/vector-constant.hh
+++ b/include/sot/core/vector-constant.hh
@@ -10,9 +10,8 @@
#ifndef DYNAMICGRAPH_SOT_VECTOR_CONSTANT_H
#define DYNAMICGRAPH_SOT_VECTOR_CONSTANT_H
-#include <dynamic-graph/entity.h>
-
#include <dynamic-graph/all-signals.h>
+#include <dynamic-graph/entity.h>
/* Matrix */
#include <dynamic-graph/linear-algebra.h>
@@ -27,13 +26,13 @@ namespace command {
namespace vectorConstant {
class Resize;
}
-} // namespace command
+} // namespace command
class VectorConstant : public Entity {
friend class command::vectorConstant::Resize;
int rows;
-public:
+ public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
@@ -47,7 +46,7 @@ public:
void setValue(const dynamicgraph::Vector &inValue);
};
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
-#endif // DYNAMICGRAPH_SOT_VECTOR_CONSTANT_H
+#endif // DYNAMICGRAPH_SOT_VECTOR_CONSTANT_H
diff --git a/include/sot/core/vector-to-rotation.hh b/include/sot/core/vector-to-rotation.hh
index d68bc540..95756e2e 100644
--- a/include/sot/core/vector-to-rotation.hh
+++ b/include/sot/core/vector-to-rotation.hh
@@ -12,6 +12,7 @@
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
+
#include <sot/core/matrix-geometry.hh>
/* Matrix */
@@ -40,14 +41,15 @@
namespace dynamicgraph {
namespace sot {
-class[[deprecated("use RPYToMatrix")]] SOTVECTORTOROTATION_EXPORT
- VectorToRotation : public dynamicgraph::Entity {
+class [[deprecated(
+ "use RPYToMatrix")]] SOTVECTORTOROTATION_EXPORT VectorToRotation
+ : public dynamicgraph::Entity {
enum sotAxis { AXIS_X, AXIS_Y, AXIS_Z };
unsigned int size;
std::vector<sotAxis> axes;
-public:
+ public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
@@ -65,4 +67,4 @@ public:
} /* namespace sot */
} /* namespace dynamicgraph */
-#endif // #ifndef __SOTVECTORTOMATRIX_HH
+#endif // #ifndef __SOTVECTORTOMATRIX_HH
diff --git a/include/sot/core/visual-point-projecter.hh b/include/sot/core/visual-point-projecter.hh
index 98659a73..4d0ddd99 100644
--- a/include/sot/core/visual-point-projecter.hh
+++ b/include/sot/core/visual-point-projecter.hh
@@ -42,16 +42,15 @@ namespace sot {
class SOTVISUALPOINTPROJECTER_EXPORT VisualPointProjecter
: public ::dynamicgraph::Entity,
public ::dynamicgraph::EntityHelper<VisualPointProjecter> {
-
-public: /* --- CONSTRUCTOR ---- */
+ public: /* --- CONSTRUCTOR ---- */
VisualPointProjecter(const std::string &name);
-public: /* --- ENTITY INHERITANCE --- */
+ public: /* --- ENTITY INHERITANCE --- */
static const std::string CLASS_NAME;
virtual void display(std::ostream &os) const;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
-public: /* --- SIGNALS --- */
+ public: /* --- SIGNALS --- */
DECLARE_SIGNAL_IN(point3D, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(transfo, MatrixHomogeneous);
@@ -59,9 +58,9 @@ public: /* --- SIGNALS --- */
DECLARE_SIGNAL_OUT(depth, double);
DECLARE_SIGNAL_OUT(point2D, dynamicgraph::Vector);
-}; // class VisualPointProjecter
+}; // class VisualPointProjecter
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
-#endif // #ifndef __sot_core_VisualPointProjecter_H__
+#endif // #ifndef __sot_core_VisualPointProjecter_H__
diff --git a/src/control/admittance-control-op-point.cpp b/src/control/admittance-control-op-point.cpp
index 7cabe9ac..5b0b01d7 100644
--- a/src/control/admittance-control-op-point.cpp
+++ b/src/control/admittance-control-op-point.cpp
@@ -9,8 +9,10 @@
*/
#include "sot/core/admittance-control-op-point.hh"
+
#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>
+
#include <sot/core/debug.hh>
#include <sot/core/stop-watch.hh>
@@ -22,17 +24,17 @@ using namespace dg;
using namespace pinocchio;
using namespace dg::command;
-#define PROFILE_ADMITTANCECONTROLOPPOINT_WFORCE_COMPUTATION \
+#define PROFILE_ADMITTANCECONTROLOPPOINT_WFORCE_COMPUTATION \
"AdmittanceControlOpPoint: w_force computation "
-#define PROFILE_ADMITTANCECONTROLOPPOINT_WDQ_COMPUTATION \
+#define PROFILE_ADMITTANCECONTROLOPPOINT_WDQ_COMPUTATION \
"AdmittanceControlOpPoint: w_dq computation "
-#define PROFILE_ADMITTANCECONTROLOPPOINT_DQ_COMPUTATION \
+#define PROFILE_ADMITTANCECONTROLOPPOINT_DQ_COMPUTATION \
"AdmittanceControlOpPoint: dq computation "
-#define INPUT_SIGNALS \
- m_KpSIN << m_KdSIN << m_dqSaturationSIN << m_forceSIN << m_w_forceDesSIN \
+#define INPUT_SIGNALS \
+ m_KpSIN << m_KdSIN << m_dqSaturationSIN << m_forceSIN << m_w_forceDesSIN \
<< m_opPoseSIN << m_sensorPoseSIN
#define INNER_SIGNALS m_w_forceSINNER << m_w_dqSINNER
@@ -51,7 +53,8 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControlOpPoint,
/* --- CONSTRUCTION -------------------------------------------------- */
/* ------------------------------------------------------------------- */
AdmittanceControlOpPoint::AdmittanceControlOpPoint(const std::string &name)
- : Entity(name), CONSTRUCT_SIGNAL_IN(Kp, dynamicgraph::Vector),
+ : Entity(name),
+ CONSTRUCT_SIGNAL_IN(Kp, dynamicgraph::Vector),
CONSTRUCT_SIGNAL_IN(Kd, dynamicgraph::Vector),
CONSTRUCT_SIGNAL_IN(dqSaturation, dynamicgraph::Vector),
CONSTRUCT_SIGNAL_IN(force, dynamicgraph::Vector),
@@ -114,8 +117,7 @@ DEFINE_SIGNAL_INNER_FUNCTION(w_force, dynamicgraph::Vector) {
"Cannot compute signal w_force before initialization!");
return s;
}
- if (s.size() != 6)
- s.resize(6);
+ if (s.size() != 6) s.resize(6);
getProfiler().start(PROFILE_ADMITTANCECONTROLOPPOINT_WFORCE_COMPUTATION);
@@ -123,7 +125,7 @@ DEFINE_SIGNAL_INNER_FUNCTION(w_force, dynamicgraph::Vector) {
const MatrixHomogeneous &sensorPose = m_sensorPoseSIN(iter);
assert(force.size() == m_n && "Unexpected size of signal force");
pinocchio::SE3 sensorPlacement(
- sensorPose.matrix()); // homogeneous matrix to SE3
+ sensorPose.matrix()); // homogeneous matrix to SE3
s = sensorPlacement.act(pinocchio::Force(force)).toVector();
getProfiler().stop(PROFILE_ADMITTANCECONTROLOPPOINT_WFORCE_COMPUTATION);
@@ -137,8 +139,7 @@ DEFINE_SIGNAL_INNER_FUNCTION(w_dq, dynamicgraph::Vector) {
"Cannot compute signal w_dq before initialization!");
return s;
}
- if (s.size() != 6)
- s.resize(6);
+ if (s.size() != 6) s.resize(6);
getProfiler().start(PROFILE_ADMITTANCECONTROLOPPOINT_WDQ_COMPUTATION);
@@ -158,10 +159,8 @@ DEFINE_SIGNAL_INNER_FUNCTION(w_dq, dynamicgraph::Vector) {
Kd.cwiseProduct(m_w_dq);
for (int i = 0; i < m_n; i++) {
- if (m_w_dq[i] > dqSaturation[i])
- m_w_dq[i] = dqSaturation[i];
- if (m_w_dq[i] < -dqSaturation[i])
- m_w_dq[i] = -dqSaturation[i];
+ if (m_w_dq[i] > dqSaturation[i]) m_w_dq[i] = dqSaturation[i];
+ if (m_w_dq[i] < -dqSaturation[i]) m_w_dq[i] = -dqSaturation[i];
}
s = m_w_dq;
@@ -176,15 +175,15 @@ DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector) {
SEND_WARNING_STREAM_MSG("Cannot compute signal dq before initialization!");
return s;
}
- if (s.size() != 6)
- s.resize(6);
+ if (s.size() != 6) s.resize(6);
getProfiler().start(PROFILE_ADMITTANCECONTROLOPPOINT_DQ_COMPUTATION);
const Vector &w_dq = m_w_dqSINNER(iter);
const MatrixHomogeneous &opPose = m_opPoseSIN(iter);
assert(w_dq.size() == m_n && "Unexpected size of signal w_dq");
- pinocchio::SE3 opPointPlacement(opPose.matrix()); // homogeneous matrix to SE3
+ pinocchio::SE3 opPointPlacement(
+ opPose.matrix()); // homogeneous matrix to SE3
s = opPointPlacement.actInv(pinocchio::Motion(w_dq)).toVector();
getProfiler().stop(PROFILE_ADMITTANCECONTROLOPPOINT_DQ_COMPUTATION);
@@ -204,6 +203,6 @@ void AdmittanceControlOpPoint::display(std::ostream &os) const {
} catch (ExceptionSignal e) {
}
}
-} // namespace core
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace core
+} // namespace sot
+} // namespace dynamicgraph
diff --git a/src/control/control-gr.cpp b/src/control/control-gr.cpp
index dbb7a1e5..2f757cc6 100644
--- a/src/control/control-gr.cpp
+++ b/src/control/control-gr.cpp
@@ -11,7 +11,7 @@
#include <sot/core/debug.hh>
class ControlGR__INIT {
-public:
+ public:
ControlGR__INIT(void) { dynamicgraph::sot::DebugTrace::openFile(); }
};
ControlGR__INIT ControlGR_initiator;
@@ -19,6 +19,7 @@ ControlGR__INIT ControlGR_initiator;
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/factory.h>
+
#include <sot/core/binary-op.hh>
#include <sot/core/control-gr.hh>
@@ -36,7 +37,8 @@ const double ControlGR::TIME_STEP_DEFAULT = .001;
#define __SOT_ControlGR_INIT
ControlGR::ControlGR(const std::string &name)
- : Entity(name), TimeStep(0),
+ : Entity(name),
+ TimeStep(0),
matrixASIN(NULL, "ControlGR(" + name + ")::input(matrix)::matrixA"),
accelerationSIN(NULL,
"ControlGR(" + name + ")::input(vector)::acceleration"),
diff --git a/src/control/control-pd.cpp b/src/control/control-pd.cpp
index eb57a6b7..13132ff8 100644
--- a/src/control/control-pd.cpp
+++ b/src/control/control-pd.cpp
@@ -14,6 +14,7 @@
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/factory.h>
+
#include <sot/core/debug.hh>
using namespace dynamicgraph::sot;
@@ -30,15 +31,16 @@ const double ControlPD::TIME_STEP_DEFAULT = .001;
#define __SOT_ControlPD_INIT
ControlPD::ControlPD(const std::string &name)
- : Entity(name), TimeStep(0),
+ : Entity(name),
+ TimeStep(0),
KpSIN(NULL, "ControlPD(" + name + ")::input(vector)::Kp"),
KdSIN(NULL, "ControlPD(" + name + ")::input(vector)::Kd"),
positionSIN(NULL, "ControlPD(" + name + ")::input(vector)::position"),
- desiredpositionSIN(NULL, "ControlPD(" + name +
- ")::input(vector)::desired_position"),
+ desiredpositionSIN(
+ NULL, "ControlPD(" + name + ")::input(vector)::desired_position"),
velocitySIN(NULL, "ControlPD(" + name + ")::input(vector)::velocity"),
- desiredvelocitySIN(NULL, "ControlPD(" + name +
- ")::input(vector)::desired_velocity"),
+ desiredvelocitySIN(
+ NULL, "ControlPD(" + name + ")::input(vector)::desired_velocity"),
controlSOUT(boost::bind(&ControlPD::computeControl, this, _1, _2),
KpSIN << KdSIN << positionSIN << desiredpositionSIN
<< velocitySIN << desiredvelocitySIN,
@@ -104,16 +106,16 @@ dynamicgraph::Vector &ControlPD::computeControl(dynamicgraph::Vector &tau,
return tau;
}
-dynamicgraph::Vector &
-ControlPD::getPositionError(dynamicgraph::Vector &position_error, int t) {
+dynamicgraph::Vector &ControlPD::getPositionError(
+ dynamicgraph::Vector &position_error, int t) {
// sotDEBUGOUT(15) ??
controlSOUT(t);
position_error = position_error_;
return position_error;
}
-dynamicgraph::Vector &
-ControlPD::getVelocityError(dynamicgraph::Vector &velocity_error, int t) {
+dynamicgraph::Vector &ControlPD::getVelocityError(
+ dynamicgraph::Vector &velocity_error, int t) {
controlSOUT(t);
velocity_error = velocity_error_;
return velocity_error;
diff --git a/src/debug/contiifstream.cpp b/src/debug/contiifstream.cpp
index eff881b5..3774a84f 100644
--- a/src/debug/contiifstream.cpp
+++ b/src/debug/contiifstream.cpp
@@ -31,11 +31,10 @@ bool Contiifstream::loop(void) {
if (file.gcount()) {
res = true;
std::string line(buffer);
- if (!first)
- reader.push_back(line);
+ if (!first) reader.push_back(line);
cursor = file.tellg();
cursor++;
- file.get(*buffer); // get the last char ( = '\n')
+ file.get(*buffer); // get the last char ( = '\n')
sotDEBUG(15) << "line: " << line << std::endl;
} else {
break;
diff --git a/src/debug/debug.cpp b/src/debug/debug.cpp
index 22f3486b..c6e44bc1 100644
--- a/src/debug/debug.cpp
+++ b/src/debug/debug.cpp
@@ -17,43 +17,41 @@ namespace dynamicgraph {
namespace sot {
#ifdef WIN32
const char *DebugTrace::DEBUG_FILENAME_DEFAULT = "c:/tmp/sot-core-traces.txt";
-#else // WIN32
+#else // WIN32
const char *DebugTrace::DEBUG_FILENAME_DEFAULT = "/tmp/sot-core-traces.txt";
-#endif // WIN32
+#endif // WIN32
#ifdef VP_DEBUG
#ifdef WIN32
std::ofstream debugfile("C:/tmp/sot-core-traces.txt",
std::ios::trunc &std::ios::out);
-#else // WIN32
+#else // WIN32
std::ofstream debugfile("/tmp/sot-core-traces.txt",
std::ios::trunc &std::ios::out);
-#endif // WIN32
-#else // VP_DEBUG
+#endif // WIN32
+#else // VP_DEBUG
std::ofstream debugfile;
class __sotDebug_init {
-public:
+ public:
__sotDebug_init() { debugfile.setstate(std::ios::failbit); }
};
__sotDebug_init __sotDebug_initialisator;
-#endif // VP_DEBUG
+#endif // VP_DEBUG
} /* namespace sot */
} /* namespace dynamicgraph */
void DebugTrace::openFile(const char *filename) {
- if (debugfile.good() && debugfile.is_open())
- debugfile.close();
+ if (debugfile.good() && debugfile.is_open()) debugfile.close();
debugfile.clear();
debugfile.open(filename, std::ios::trunc & std::ios::out);
}
void DebugTrace::closeFile(const char *) {
- if (debugfile.good() && debugfile.is_open())
- debugfile.close();
+ if (debugfile.good() && debugfile.is_open()) debugfile.close();
debugfile.setstate(std::ios::failbit);
}
@@ -61,5 +59,5 @@ namespace dynamicgraph {
namespace sot {
DebugTrace sotDEBUGFLOW(debugfile);
DebugTrace sotERRORFLOW(debugfile);
-} // namespace sot.
-} // namespace dynamicgraph
+} // namespace sot.
+} // namespace dynamicgraph
diff --git a/src/dynamic_graph/__init__.py b/src/dynamic_graph/__init__.py
index 69e3be50..8db66d3d 100644
--- a/src/dynamic_graph/__init__.py
+++ b/src/dynamic_graph/__init__.py
@@ -1 +1 @@
-__path__ = __import__('pkgutil').extend_path(__path__, __name__)
+__path__ = __import__("pkgutil").extend_path(__path__, __name__)
diff --git a/src/dynamic_graph/sot/core/feature_position.py b/src/dynamic_graph/sot/core/feature_position.py
index 36d6bbab..3cc632e9 100644
--- a/src/dynamic_graph/sot/core/feature_position.py
+++ b/src/dynamic_graph/sot/core/feature_position.py
@@ -28,35 +28,37 @@ class FeaturePosition(Entity):
signalMap = dict()
- def __init__(self, name, signalPosition=None, signalJacobian=None, referencePosition=None):
+ def __init__(
+ self, name, signalPosition=None, signalJacobian=None, referencePosition=None
+ ):
self._feature = FeaturePoint6d(name)
self.obj = self._feature.obj
- self._reference = FeaturePoint6d(name + '_ref')
+ self._reference = FeaturePoint6d(name + "_ref")
if referencePosition:
- self._reference.signal('position').value = tuple(referencePosition)
+ self._reference.signal("position").value = tuple(referencePosition)
if signalPosition:
- plug(signalPosition, self._feature.signal('position'))
+ plug(signalPosition, self._feature.signal("position"))
if signalJacobian:
- plug(signalJacobian, self._feature.signal('Jq'))
+ plug(signalJacobian, self._feature.signal("Jq"))
self._feature.setReference(self._reference.name)
- self._feature.signal('selec').value = Flags('111111')
- self._feature.frame('current')
+ self._feature.signal("selec").value = Flags("111111")
+ self._feature.frame("current")
# Signals stored in members
- self.position = self._feature.signal('position')
- self.reference = self._reference.signal('position')
- self.velocity = self._reference.signal('velocity')
- self.Jq = self._feature.signal('Jq')
- self.error = self._feature.signal('error')
- self.errordot = self._feature.signal('errordot')
- self.selec = self._feature.signal('selec')
+ self.position = self._feature.signal("position")
+ self.reference = self._reference.signal("position")
+ self.velocity = self._reference.signal("velocity")
+ self.Jq = self._feature.signal("Jq")
+ self.error = self._feature.signal("error")
+ self.errordot = self._feature.signal("errordot")
+ self.selec = self._feature.signal("selec")
self.signalMap = {
- 'position': self.position,
- 'reference': self.reference,
- 'Jq': self.Jq,
- 'error': self.error,
- 'selec': self.selec
+ "position": self.position,
+ "reference": self.reference,
+ "Jq": self.Jq,
+ "error": self.error,
+ "selec": self.selec,
}
@property
@@ -70,7 +72,7 @@ class FeaturePosition(Entity):
if name in self.signalMap.keys():
return self.signalMap[name]
else:
- raise RuntimeError('No signal with this name')
+ raise RuntimeError("No signal with this name")
def signals(self):
"""
@@ -82,7 +84,7 @@ class FeaturePosition(Entity):
"""
Return the list of commands.
"""
- return ('frame', 'getFrame', 'keep')
+ return ("frame", "getFrame", "keep")
def frame(self, f):
return self._feature.frame(f)
diff --git a/src/dynamic_graph/sot/core/feature_position_relative.py b/src/dynamic_graph/sot/core/feature_position_relative.py
index 3f77612e..04bb37ff 100644
--- a/src/dynamic_graph/sot/core/feature_position_relative.py
+++ b/src/dynamic_graph/sot/core/feature_position_relative.py
@@ -11,7 +11,7 @@ from dynamic_graph.sot.core import Flags
from dynamic_graph.sot.core.feature_point6d_relative import FeaturePoint6dRelative
# Identity matrix of order 4
-I4 = reduce(lambda m, i: m + (i * (0., ) + (1., ) + (3 - i) * (0., ), ), range(4), ())
+I4 = reduce(lambda m, i: m + (i * (0.0,) + (1.0,) + (3 - i) * (0.0,),), range(4), ())
class FeaturePositionRelative(Entity):
@@ -57,21 +57,23 @@ class FeaturePositionRelative(Entity):
jacobian : jacobian output signal with respect to the robot
configuration,
selec: : selection flag "RzRyRxTzTyTx" (string).
- """
+ """
signalMap = dict()
- def __init__(self,
- name,
- basePosition=None,
- otherPosition=None,
- baseReference=None,
- otherReference=None,
- JqBase=None,
- JqOther=None):
+ def __init__(
+ self,
+ name,
+ basePosition=None,
+ otherPosition=None,
+ baseReference=None,
+ otherReference=None,
+ JqBase=None,
+ JqOther=None,
+ ):
self._feature = FeaturePoint6dRelative(name)
self.obj = self._feature.obj
- self._reference = FeaturePoint6dRelative(name + '_ref')
+ self._reference = FeaturePoint6dRelative(name + "_ref")
# Set undefined input parameters as identity matrix
if basePosition is None:
basePosition = I4
@@ -83,45 +85,46 @@ class FeaturePositionRelative(Entity):
otherReference = I4
# If input positions are signals, plug them, otherwise set values
- for (sout, sin) in \
- ((basePosition, self._feature.signal('positionRef')),
- (otherPosition, self._feature.signal('position')),
- (baseReference, self._reference.signal('positionRef')),
- (otherReference, self._reference.signal('position'))):
+ for (sout, sin) in (
+ (basePosition, self._feature.signal("positionRef")),
+ (otherPosition, self._feature.signal("position")),
+ (baseReference, self._reference.signal("positionRef")),
+ (otherReference, self._reference.signal("position")),
+ ):
if isinstance(sout, SignalBase):
plug(sout, sin)
else:
sin.value = sout
if JqBase:
- plug(JqBase, self._feature.signal('JqRef'))
+ plug(JqBase, self._feature.signal("JqRef"))
if JqOther:
- plug(JqOther, self._feature.signal('Jq'))
+ plug(JqOther, self._feature.signal("Jq"))
self._feature.setReference(self._reference.name)
- self._feature.signal('selec').value = Flags('111111')
- self._feature.frame('current')
+ self._feature.signal("selec").value = Flags("111111")
+ self._feature.frame("current")
# Signals stored in members
- self.basePosition = self._feature.signal('positionRef')
- self.otherPosition = self._feature.signal('position')
- self.baseReference = self._reference.signal('positionRef')
- self.otherReference = self._reference.signal('position')
- self.JqBase = self._feature.signal('JqRef')
- self.JqOther = self._feature.signal('Jq')
- self.error = self._feature.signal('error')
- self.jacobian = self._feature.signal('jacobian')
- self.selec = self._feature.signal('selec')
+ self.basePosition = self._feature.signal("positionRef")
+ self.otherPosition = self._feature.signal("position")
+ self.baseReference = self._reference.signal("positionRef")
+ self.otherReference = self._reference.signal("position")
+ self.JqBase = self._feature.signal("JqRef")
+ self.JqOther = self._feature.signal("Jq")
+ self.error = self._feature.signal("error")
+ self.jacobian = self._feature.signal("jacobian")
+ self.selec = self._feature.signal("selec")
self.signalMap = {
- 'basePosition': self.basePosition,
- 'otherPosition': self.otherPosition,
- 'baseReference': self.baseReference,
- 'otherReference': self.otherReference,
- 'JqBase': self.JqBase,
- 'JqOther': self.JqOther,
- 'error': self.error,
- 'jacobian': self.jacobian,
- 'selec': self.selec
+ "basePosition": self.basePosition,
+ "otherPosition": self.otherPosition,
+ "baseReference": self.baseReference,
+ "otherReference": self.otherReference,
+ "JqBase": self.JqBase,
+ "JqOther": self.JqOther,
+ "error": self.error,
+ "jacobian": self.jacobian,
+ "selec": self.selec,
}
@property
@@ -135,7 +138,7 @@ class FeaturePositionRelative(Entity):
if name in self.signalMap.keys():
return self.signalMap[name]
else:
- raise RuntimeError('No signal with this name')
+ raise RuntimeError("No signal with this name")
def signals(self):
"""
diff --git a/src/dynamic_graph/sot/core/math_small_entities.py b/src/dynamic_graph/sot/core/math_small_entities.py
index 94796659..14f5e910 100644
--- a/src/dynamic_graph/sot/core/math_small_entities.py
+++ b/src/dynamic_graph/sot/core/math_small_entities.py
@@ -6,17 +6,59 @@
# from operator import WeightDir
# from operator import Nullificator
from .operator import (
- Add_of_double, Add_of_matrix, Add_of_vector, Component_of_vector, Compose_R_and_T, ConvolutionTemporal,
- HomoToMatrix, HomoToRotation, HomoToTwist, Inverse_of_matrix, Inverse_of_matrixHomo, Inverse_of_matrixrotation,
- Inverse_of_matrixtwist, Inverse_of_unitquat, MatrixDiagonal, MatrixHomoToPose, MatrixHomoToPoseQuaternion,
- PoseQuaternionToMatrixHomo, MatrixHomoToPoseRollPitchYaw, MatrixHomoToPoseUTheta, MatrixToHomo, MatrixToQuaternion,
- MatrixToRPY, MatrixToUTheta, MatrixHomoToSE3Vector, SE3VectorToMatrixHomo, MatrixTranspose, Multiply_double_vector,
- Multiply_matrix_vector, Multiply_matrixTwist_vector, Multiply_matrixHomo_vector, Multiply_of_double,
- Multiply_of_matrix, Multiply_of_matrixHomo, Multiply_of_matrixrotation, Multiply_of_matrixtwist,
- Multiply_of_quaternion, Multiply_of_vector, PoseRollPitchYawToMatrixHomo, PoseRollPitchYawToPoseUTheta,
- PoseUThetaToMatrixHomo, QuaternionToMatrix, RPYToMatrix, Selec_column_of_matrix, Selec_of_matrix, Selec_of_vector,
- SkewSymToVector, Stack_of_vector, Substract_of_double, Substract_of_matrix, Substract_of_vector,
- UThetaToQuaternion)
+ Add_of_double,
+ Add_of_matrix,
+ Add_of_vector,
+ Component_of_vector,
+ Compose_R_and_T,
+ ConvolutionTemporal,
+ HomoToMatrix,
+ HomoToRotation,
+ HomoToTwist,
+ Inverse_of_matrix,
+ Inverse_of_matrixHomo,
+ Inverse_of_matrixrotation,
+ Inverse_of_matrixtwist,
+ Inverse_of_unitquat,
+ MatrixDiagonal,
+ MatrixHomoToPose,
+ MatrixHomoToPoseQuaternion,
+ PoseQuaternionToMatrixHomo,
+ MatrixHomoToPoseRollPitchYaw,
+ MatrixHomoToPoseUTheta,
+ MatrixToHomo,
+ MatrixToQuaternion,
+ MatrixToRPY,
+ MatrixToUTheta,
+ MatrixHomoToSE3Vector,
+ SE3VectorToMatrixHomo,
+ MatrixTranspose,
+ Multiply_double_vector,
+ Multiply_matrix_vector,
+ Multiply_matrixTwist_vector,
+ Multiply_matrixHomo_vector,
+ Multiply_of_double,
+ Multiply_of_matrix,
+ Multiply_of_matrixHomo,
+ Multiply_of_matrixrotation,
+ Multiply_of_matrixtwist,
+ Multiply_of_quaternion,
+ Multiply_of_vector,
+ PoseRollPitchYawToMatrixHomo,
+ PoseRollPitchYawToPoseUTheta,
+ PoseUThetaToMatrixHomo,
+ QuaternionToMatrix,
+ RPYToMatrix,
+ Selec_column_of_matrix,
+ Selec_of_matrix,
+ Selec_of_vector,
+ SkewSymToVector,
+ Stack_of_vector,
+ Substract_of_double,
+ Substract_of_matrix,
+ Substract_of_vector,
+ UThetaToQuaternion,
+)
from .derivator import Derivator_of_Matrix, Derivator_of_Vector
from .matrix_constant import MatrixConstant
diff --git a/src/dynamic_graph/sot/core/matrix_util.py b/src/dynamic_graph/sot/core/matrix_util.py
index 2f905562..2d90144b 100644
--- a/src/dynamic_graph/sot/core/matrix_util.py
+++ b/src/dynamic_graph/sot/core/matrix_util.py
@@ -1,6 +1,6 @@
-'''
+"""
Tiny matrix functions, taken from Oscar source code.
-'''
+"""
from math import atan2
from random import random
@@ -29,7 +29,7 @@ def vectorToTuple(M):
# Convert from Roll, Pitch, Yaw to transformation Matrix
def rpy2tr(r, p, y):
- mat = matrix(rotate('z', y)) * matrix(rotate('y', p)) * matrix(rotate('x', r))
+ mat = matrix(rotate("z", y)) * matrix(rotate("y", p)) * matrix(rotate("x", r))
return matrixToTuple(mat)
@@ -63,13 +63,17 @@ def generateOrthonormalM(v1):
e1 = e1.tolist()
e2 = e2.tolist()
e3 = e3.tolist()
- M = ((e1[0][0], e2[0][0], e3[0][0]), (e1[0][1], e2[0][1], e3[0][1]), (e1[0][2], e2[0][2], e3[0][2]))
+ M = (
+ (e1[0][0], e2[0][0], e3[0][0]),
+ (e1[0][1], e2[0][1], e3[0][1]),
+ (e1[0][2], e2[0][2], e3[0][2]),
+ )
return M
# Convert from Transformation Matrix to Roll,Pitch,Yaw
def tr2rpy(M):
- m = sqrt(M[2][1]**2 + M[2][2]**2)
+ m = sqrt(M[2][1] ** 2 + M[2][2] ** 2)
p = atan2(-M[2][0], m)
if abs(p - pi / 2) < 0.001:
@@ -86,17 +90,17 @@ def tr2rpy(M):
def matrixToRPY(M):
- '''
+ """
Convert a 4x4 homogeneous matrix to a 6x1 rpy pose vector.
- '''
+ """
rot = tr2rpy(M)
return [M[0][3], M[1][3], M[2][3], rot[2], rot[1], rot[0]]
def RPYToMatrix(pr):
- '''
+ """
Convert a 6x1 rpy pose vector to a 4x4 homogeneous matrix.
- '''
+ """
M = array(rpy2tr(pr[3], pr[4], pr[5]))
M[0:3, 3] = pr[0:3]
return M
@@ -104,24 +108,37 @@ def RPYToMatrix(pr):
# Transformation Matrix corresponding to a rotation about x,y or z
def rotate(axis, ang):
- ''' eg. T=rot('x',pi/4): rotate pi/4 rad about x axis
- '''
+ """eg. T=rot('x',pi/4): rotate pi/4 rad about x axis"""
ca = cos(ang)
sa = sin(ang)
- if axis == 'x':
+ if axis == "x":
mat = ((1, 0, 0, 0), (0, ca, -sa, 0), (0, sa, ca, 0), (0, 0, 0, 1))
- elif axis == 'y':
+ elif axis == "y":
mat = ((ca, 0, sa, 0), (0, 1, 0, 0), (-sa, 0, ca, 0), (0, 0, 0, 1))
- elif axis == 'z':
+ elif axis == "z":
mat = ((ca, -sa, 0, 0), (sa, ca, 0, 0), (0, 0, 1, 0), (0, 0, 0, 1))
else:
- print('Axis should be: x,y or z only')
+ print("Axis should be: x,y or z only")
return mat
def quaternionToMatrix(q):
[qx, qy, qz, qw] = q
- R = [[1 - 2 * qy**2 - 2 * qz**2, 2 * qx * qy - 2 * qz * qw, 2 * qx * qz + 2 * qy * qw],
- [2 * qx * qy + 2 * qz * qw, 1 - 2 * qx**2 - 2 * qz**2, 2 * qy * qz - 2 * qx * qw],
- [2 * qx * qz - 2 * qy * qw, 2 * qy * qz + 2 * qx * qw, 1 - 2 * qx**2 - 2 * qy**2]]
+ R = [
+ [
+ 1 - 2 * qy**2 - 2 * qz**2,
+ 2 * qx * qy - 2 * qz * qw,
+ 2 * qx * qz + 2 * qy * qw,
+ ],
+ [
+ 2 * qx * qy + 2 * qz * qw,
+ 1 - 2 * qx**2 - 2 * qz**2,
+ 2 * qy * qz - 2 * qx * qw,
+ ],
+ [
+ 2 * qx * qz - 2 * qy * qw,
+ 2 * qy * qz + 2 * qx * qw,
+ 1 - 2 * qx**2 - 2 * qy**2,
+ ],
+ ]
return R
diff --git a/src/dynamic_graph/sot/core/meta_task_6d.py b/src/dynamic_graph/sot/core/meta_task_6d.py
index 4c0cc0d8..17d17cfb 100644
--- a/src/dynamic_graph/sot/core/meta_task_6d.py
+++ b/src/dynamic_graph/sot/core/meta_task_6d.py
@@ -8,55 +8,60 @@ from dynamic_graph.sot.core.task import Task
def toFlags(arr):
from warnings import warn
+
warn("This function is deprecated. Please, use Flags directly.")
return Flags(arr)
class MetaTask6d(object):
- name = ''
- opPoint = ''
+ name = ""
+ opPoint = ""
dyn = 0
task = 0
feature = 0
featureDes = 0
def opPointExist(self, opPoint):
- sigsP = [x for x in self.dyn.signals() if x.getName().split(':')[-1] == opPoint]
- sigsJ = [x for x in self.dyn.signals() if x.getName().split(':')[-1] == 'J' + opPoint]
+ sigsP = [x for x in self.dyn.signals() if x.getName().split(":")[-1] == opPoint]
+ sigsJ = [
+ x for x in self.dyn.signals() if x.getName().split(":")[-1] == "J" + opPoint
+ ]
return len(sigsP) == 1 & len(sigsJ) == 1
def defineDynEntities(self, dyn):
self.dyn = dyn
- def createOpPoint(self, opPoint, opPointRef='right-wrist'):
+ def createOpPoint(self, opPoint, opPointRef="right-wrist"):
self.opPoint = opPoint
if self.opPointExist(opPoint):
return
self.dyn.createOpPoint(opPoint, opPointRef)
def createOpPointModif(self):
- self.opPointModif = OpPointModifier('opmodif' + self.name)
- plug(self.dyn.signal(self.opPoint), self.opPointModif.signal('positionIN'))
- plug(self.dyn.signal('J' + self.opPoint), self.opPointModif.signal('jacobianIN'))
+ self.opPointModif = OpPointModifier("opmodif" + self.name)
+ plug(self.dyn.signal(self.opPoint), self.opPointModif.signal("positionIN"))
+ plug(
+ self.dyn.signal("J" + self.opPoint), self.opPointModif.signal("jacobianIN")
+ )
self.opPointModif.activ = False
def createFeatures(self):
- self.feature = FeaturePoint6d('feature' + self.name)
- self.featureDes = FeaturePoint6d('feature' + self.name + '_ref')
- self.feature.selec.value = Flags('111111')
- self.feature.frame('current')
+ self.feature = FeaturePoint6d("feature" + self.name)
+ self.featureDes = FeaturePoint6d("feature" + self.name + "_ref")
+ self.feature.selec.value = Flags("111111")
+ self.feature.frame("current")
def createTask(self):
- self.task = Task('task' + self.name)
+ self.task = Task("task" + self.name)
def createGain(self):
- self.gain = GainAdaptive('gain' + self.name)
+ self.gain = GainAdaptive("gain" + self.name)
self.gain.set(0.1, 0.1, 125e3)
def plugEverything(self):
self.feature.setReference(self.featureDes.name)
- plug(self.dyn.signal(self.opPoint), self.feature.signal('position'))
- plug(self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq'))
+ plug(self.dyn.signal(self.opPoint), self.feature.signal("position"))
+ plug(self.dyn.signal("J" + self.opPoint), self.feature.signal("Jq"))
self.task.add(self.feature.name)
plug(self.task.error, self.gain.error)
plug(self.gain.gain, self.task.controlGain)
@@ -65,7 +70,7 @@ class MetaTask6d(object):
self.feature.position.recompute(self.dyn.position.time)
self.feature.keep()
- def __init__(self, name, dyn, opPoint, opPointRef='right-wrist'):
+ def __init__(self, name, dyn, opPoint, opPointRef="right-wrist"):
self.name = name
self.defineDynEntities(dyn)
self.createOpPoint(opPoint, opPointRef)
@@ -93,12 +98,12 @@ class MetaTask6d(object):
@opmodif.setter
def opmodif(self, m):
if isinstance(m, bool) and not m:
- plug(self.dyn.signal(self.opPoint), self.feature.signal('position'))
- plug(self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq'))
+ plug(self.dyn.signal(self.opPoint), self.feature.signal("position"))
+ plug(self.dyn.signal("J" + self.opPoint), self.feature.signal("Jq"))
self.opPointModif.activ = False
else:
if not self.opPointModif.activ:
- plug(self.opPointModif.signal('position'), self.feature.position)
- plug(self.opPointModif.signal('jacobian'), self.feature.Jq)
+ plug(self.opPointModif.signal("position"), self.feature.position)
+ plug(self.opPointModif.signal("jacobian"), self.feature.Jq)
self.opPointModif.setTransformation(m)
self.opPointModif.activ = True
diff --git a/src/dynamic_graph/sot/core/meta_task_posture.py b/src/dynamic_graph/sot/core/meta_task_posture.py
index 04f95c94..e67a9ca4 100644
--- a/src/dynamic_graph/sot/core/meta_task_posture.py
+++ b/src/dynamic_graph/sot/core/meta_task_posture.py
@@ -3,7 +3,9 @@ from dynamic_graph.sot.core import Flags
from dynamic_graph.sot.core.feature_generic import FeatureGeneric
from dynamic_graph.sot.core.gain_adaptive import GainAdaptive
from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple
-from dynamic_graph.sot.core.meta_task_6d import toFlags # noqa kept for backward compatibility
+from dynamic_graph.sot.core.meta_task_6d import (
+ toFlags,
+) # noqa kept for backward compatibility
from dynamic_graph.sot.core.meta_tasks import setGain
from dynamic_graph.sot.core.task import Task
from numpy import identity, matrix, zeros
@@ -37,9 +39,9 @@ class MetaTaskPosture(object):
def __init__(self, dyn, name="posture"):
self.dyn = dyn
self.name = name
- self.feature = FeatureGeneric('feature' + name)
- self.featureDes = FeatureGeneric('featureDes' + name)
- self.gain = GainAdaptive('gain' + name)
+ self.feature = FeatureGeneric("feature" + name)
+ self.featureDes = FeatureGeneric("featureDes" + name)
+ self.gain = GainAdaptive("gain" + name)
plug(dyn.position, self.feature.errorIN)
robotDim = len(dyn.position.value)
self.feature.jacobianIN.value = matrixToTuple(identity(robotDim))
@@ -89,5 +91,5 @@ class MetaTaskPosture(object):
class MetaTaskKinePosture(MetaTaskPosture):
def __init__(self, dyn, name="posture"):
MetaTaskPosture.__init__(self, dyn, name)
- self.task = Task('task' + name)
+ self.task = Task("task" + name)
self.plugTask()
diff --git a/src/dynamic_graph/sot/core/meta_task_visual_point.py b/src/dynamic_graph/sot/core/meta_task_visual_point.py
index ec938df5..a5629b30 100644
--- a/src/dynamic_graph/sot/core/meta_task_visual_point.py
+++ b/src/dynamic_graph/sot/core/meta_task_visual_point.py
@@ -8,8 +8,8 @@ from dynamic_graph.sot.core.visual_point_projecter import VisualPointProjecter
class MetaTaskVisualPoint(object):
- name = ''
- opPoint = ''
+ name = ""
+ opPoint = ""
dyn = 0
task = 0
feature = 0
@@ -17,51 +17,57 @@ class MetaTaskVisualPoint(object):
proj = 0
def opPointExist(self, opPoint):
- sigsP = filter(lambda x: x.getName().split(':')[-1] == opPoint, self.dyn.signals())
- sigsJ = filter(lambda x: x.getName().split(':')[-1] == 'J' + opPoint, self.dyn.signals())
+ sigsP = filter(
+ lambda x: x.getName().split(":")[-1] == opPoint, self.dyn.signals()
+ )
+ sigsJ = filter(
+ lambda x: x.getName().split(":")[-1] == "J" + opPoint, self.dyn.signals()
+ )
return len(sigsP) == 1 & len(sigsJ) == 1
def defineDynEntities(self, dyn):
self.dyn = dyn
- def createOpPoint(self, opPoint, opPointRef='right-wrist'):
+ def createOpPoint(self, opPoint, opPointRef="right-wrist"):
self.opPoint = opPoint
if self.opPointExist(opPoint):
return
self.dyn.createOpPoint(opPoint, opPointRef)
def createOpPointModif(self):
- self.opPointModif = OpPointModifier('opmodif' + self.name)
- plug(self.dyn.signal(self.opPoint), self.opPointModif.signal('positionIN'))
- plug(self.dyn.signal('J' + self.opPoint), self.opPointModif.signal('jacobianIN'))
+ self.opPointModif = OpPointModifier("opmodif" + self.name)
+ plug(self.dyn.signal(self.opPoint), self.opPointModif.signal("positionIN"))
+ plug(
+ self.dyn.signal("J" + self.opPoint), self.opPointModif.signal("jacobianIN")
+ )
self.opPointModif.activ = False
def createFeatures(self):
- self.feature = FeatureVisualPoint('feature' + self.name)
- self.featureDes = FeatureVisualPoint('feature' + self.name + '_ref')
- self.feature.selec.value = '11'
+ self.feature = FeatureVisualPoint("feature" + self.name)
+ self.featureDes = FeatureVisualPoint("feature" + self.name + "_ref")
+ self.feature.selec.value = "11"
def createTask(self):
- self.task = Task('task' + self.name)
+ self.task = Task("task" + self.name)
def createGain(self):
- self.gain = GainAdaptive('gain' + self.name)
+ self.gain = GainAdaptive("gain" + self.name)
self.gain.set(0.1, 0.1, 125e3)
def createProj(self):
- self.proj = VisualPointProjecter('proj' + self.name)
+ self.proj = VisualPointProjecter("proj" + self.name)
def plugEverything(self):
self.feature.setReference(self.featureDes.name)
- plug(self.dyn.signal(self.opPoint), self.proj.signal('transfo'))
- plug(self.proj.signal('point2D'), self.feature.signal('xy'))
- plug(self.proj.signal('depth'), self.feature.signal('Z'))
- plug(self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq'))
+ plug(self.dyn.signal(self.opPoint), self.proj.signal("transfo"))
+ plug(self.proj.signal("point2D"), self.feature.signal("xy"))
+ plug(self.proj.signal("depth"), self.feature.signal("Z"))
+ plug(self.dyn.signal("J" + self.opPoint), self.feature.signal("Jq"))
self.task.add(self.feature.name)
plug(self.task.error, self.gain.error)
plug(self.gain.gain, self.task.controlGain)
- def __init__(self, name, dyn, opPoint, opPointRef='right-wrist'):
+ def __init__(self, name, dyn, opPoint, opPointRef="right-wrist"):
self.name = name
self.defineDynEntities(dyn)
self.createOpPoint(opPoint, opPointRef)
@@ -98,13 +104,13 @@ class MetaTaskVisualPoint(object):
@opmodif.setter
def opmodif(self, m):
if isinstance(m, bool) and not m:
- plug(self.dyn.signal(self.opPoint), self.proj.signal('transfo'))
- plug(self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq'))
+ plug(self.dyn.signal(self.opPoint), self.proj.signal("transfo"))
+ plug(self.dyn.signal("J" + self.opPoint), self.feature.signal("Jq"))
self.opPointModif.activ = False
else:
if not self.opPointModif.activ:
- plug(self.opPointModif.signal('position'), self.proj.signal('transfo'))
- plug(self.opPointModif.signal('jacobian'), self.feature.signal('Jq'))
+ plug(self.opPointModif.signal("position"), self.proj.signal("transfo"))
+ plug(self.opPointModif.signal("jacobian"), self.feature.signal("Jq"))
self.opPointModif.setTransformation(m)
self.opPointModif.activ = True
diff --git a/src/dynamic_graph/sot/core/meta_tasks.py b/src/dynamic_graph/sot/core/meta_tasks.py
index ce634685..45ce19fb 100644
--- a/src/dynamic_graph/sot/core/meta_tasks.py
+++ b/src/dynamic_graph/sot/core/meta_tasks.py
@@ -5,7 +5,9 @@ from dynamic_graph.sot.core import Flags
from dynamic_graph.sot.core.feature_generic import FeatureGeneric
from dynamic_graph.sot.core.gain_adaptive import GainAdaptive
from dynamic_graph.sot.core.matrix_util import rpy2tr
-from dynamic_graph.sot.core.meta_task_6d import toFlags # noqa kept for backward compatibility
+from dynamic_graph.sot.core.meta_task_6d import (
+ toFlags,
+) # noqa kept for backward compatibility
class MetaTaskCom(object):
@@ -14,9 +16,9 @@ class MetaTaskCom(object):
self.name = name
# dyn.setProperty('ComputeCoM','true')
- self.feature = FeatureGeneric('feature' + name)
- self.featureDes = FeatureGeneric('featureDes' + name)
- self.gain = GainAdaptive('gain' + name)
+ self.feature = FeatureGeneric("feature" + name)
+ self.featureDes = FeatureGeneric("featureDes" + name)
+ self.gain = GainAdaptive("gain" + name)
plug(dyn.com, self.feature.errorIN)
plug(dyn.Jcom, self.feature.jacobianIN)
@@ -57,7 +59,9 @@ def generic6dReference(p):
M[0:3, 3] = p
elif isinstance(p, (matrix, ndarray)) and p.shape == (4, 4):
M = p
- elif isinstance(p, (matrix, tuple)) and len(p) == 4 == len(p[0]) == len(p[1]) == len(p[2]) == len(p[3]):
+ elif isinstance(p, (matrix, tuple)) and len(p) == 4 == len(p[0]) == len(
+ p[1]
+ ) == len(p[2]) == len(p[3]):
M = matrix(p)
elif isinstance(p, (matrix, ndarray, tuple)) and len(p) == 6:
M = array(rpy2tr(*p[3:7]))
@@ -72,7 +76,10 @@ def goto6d(task, position, gain=None, resetJacobian=True):
task.featureDes.position.value = array(M)
task.feature.selec.value = Flags("111111")
setGain(task.gain, gain)
- if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian:
+ if (
+ "resetJacobianDerivative" in task.task.__class__.__dict__.keys()
+ and resetJacobian
+ ):
task.task.resetJacobianDerivative()
@@ -84,5 +91,8 @@ def gotoNd(task, position, selec=None, gain=None, resetJacobian=True):
task.feature.selec.value = selec
task.featureDes.position.value = array(M)
setGain(task.gain, gain)
- if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian:
+ if (
+ "resetJacobianDerivative" in task.task.__class__.__dict__.keys()
+ and resetJacobian
+ ):
task.task.resetJacobianDerivative()
diff --git a/src/dynamic_graph/sot/core/meta_tasks_kine.py b/src/dynamic_graph/sot/core/meta_tasks_kine.py
index 66bc4559..147d0724 100644
--- a/src/dynamic_graph/sot/core/meta_tasks_kine.py
+++ b/src/dynamic_graph/sot/core/meta_tasks_kine.py
@@ -1,6 +1,7 @@
from dynamic_graph import plug
from dynamic_graph.sot.core.gain_adaptive import GainAdaptive
from dynamic_graph.sot.core.meta_task_6d import MetaTask6d
+
# TODO: this function is imported from meta_tasks_kine in several places,
# whereas it is defined in meta_tasks
from dynamic_graph.sot.core.meta_tasks import gotoNd # noqa
@@ -10,16 +11,16 @@ from dynamic_graph.sot.core.task import Task
class MetaTaskKine6d(MetaTask6d):
def createTask(self):
- self.task = Task('task' + self.name)
+ self.task = Task("task" + self.name)
def createGain(self):
- self.gain = GainAdaptive('gain' + self.name)
+ self.gain = GainAdaptive("gain" + self.name)
self.gain.set(0.1, 0.1, 125e3)
def plugEverything(self):
self.feature.setReference(self.featureDes.name)
- plug(self.dyn.signal(self.opPoint), self.feature.signal('position'))
- plug(self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq'))
+ plug(self.dyn.signal(self.opPoint), self.feature.signal("position"))
+ plug(self.dyn.signal("J" + self.opPoint), self.feature.signal("Jq"))
self.task.add(self.feature.name)
plug(self.task.error, self.gain.error)
plug(self.gain.gain, self.task.controlGain)
@@ -32,5 +33,5 @@ class MetaTaskKine6d(MetaTask6d):
class MetaTaskKineCom(MetaTaskCom):
def __init__(self, dyn, name="com"):
MetaTaskCom.__init__(self, dyn, name)
- self.task = Task('task' + name)
+ self.task = Task("task" + name)
self.plugTask()
diff --git a/src/dynamic_graph/sot/core/meta_tasks_kine_relative.py b/src/dynamic_graph/sot/core/meta_tasks_kine_relative.py
index e8a43afc..8e2861d5 100644
--- a/src/dynamic_graph/sot/core/meta_tasks_kine_relative.py
+++ b/src/dynamic_graph/sot/core/meta_tasks_kine_relative.py
@@ -2,39 +2,48 @@ from dynamic_graph import plug
from dynamic_graph.sot.core import Flags
from dynamic_graph.sot.core.feature_point6d_relative import FeaturePoint6dRelative
from dynamic_graph.sot.core.matrix_util import matrixToTuple
-from dynamic_graph.sot.core.meta_task_6d import MetaTask6d, toFlags # noqa kept for backward compatibility
+from dynamic_graph.sot.core.meta_task_6d import (
+ MetaTask6d,
+ toFlags,
+) # noqa kept for backward compatibility
from dynamic_graph.sot.core.meta_tasks import generic6dReference, setGain
from dynamic_graph.sot.core.op_point_modifier import OpPointModifier
class MetaTaskKine6dRel(MetaTask6d):
- opPointBase = ''
+ opPointBase = ""
- def createOpPointBase(self, opPointBase, opPointRefBase='left-wrist'):
+ def createOpPointBase(self, opPointBase, opPointRefBase="left-wrist"):
self.opPointBase = opPointBase
if self.opPointExist(opPointBase):
return
self.dyn.createOpPoint(opPointBase, opPointRefBase)
def createOpPointModifBase(self):
- self.opPointModifBase = OpPointModifier('opmodifBase' + self.name)
- plug(self.dyn.signal(self.opPointBase), self.opPointModifBase.signal('positionIN'))
- plug(self.dyn.signal('J' + self.opPointBase), self.opPointModifBase.signal('jacobianIN'))
+ self.opPointModifBase = OpPointModifier("opmodifBase" + self.name)
+ plug(
+ self.dyn.signal(self.opPointBase),
+ self.opPointModifBase.signal("positionIN"),
+ )
+ plug(
+ self.dyn.signal("J" + self.opPointBase),
+ self.opPointModifBase.signal("jacobianIN"),
+ )
self.opPointModifBase.activ = False
def createFeatures(self):
- self.feature = FeaturePoint6dRelative('featureRel' + self.name)
- self.featureDes = FeaturePoint6dRelative('featureRel' + self.name + '_ref')
- self.feature.selec.value = Flags('111111')
- self.feature.frame('current')
+ self.feature = FeaturePoint6dRelative("featureRel" + self.name)
+ self.featureDes = FeaturePoint6dRelative("featureRel" + self.name + "_ref")
+ self.feature.selec.value = Flags("111111")
+ self.feature.frame("current")
def plugEverything(self):
self.feature.setReference(self.featureDes.name)
- plug(self.dyn.signal(self.opPoint), self.feature.signal('position'))
- plug(self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq'))
- plug(self.dyn.signal(self.opPointBase), self.feature.signal('positionRef'))
- plug(self.dyn.signal('J' + self.opPointBase), self.feature.signal('JqRef'))
+ plug(self.dyn.signal(self.opPoint), self.feature.signal("position"))
+ plug(self.dyn.signal("J" + self.opPoint), self.feature.signal("Jq"))
+ plug(self.dyn.signal(self.opPointBase), self.feature.signal("positionRef"))
+ plug(self.dyn.signal("J" + self.opPointBase), self.feature.signal("JqRef"))
self.task.add(self.feature.name)
plug(self.task.error, self.gain.error)
plug(self.gain.gain, self.task.controlGain)
@@ -43,7 +52,15 @@ class MetaTaskKine6dRel(MetaTask6d):
self.feature.position.recompute(self.dyn.position.time)
self.feature.keep()
- def __init__(self, name, dyn, opPoint, opPointBase, opPointRef='right-wrist', opPointRefBase='left-wrist'):
+ def __init__(
+ self,
+ name,
+ dyn,
+ opPoint,
+ opPointBase,
+ opPointRef="right-wrist",
+ opPointRefBase="left-wrist",
+ ):
self.name = name
self.defineDynEntities(dyn)
self.createOpPoint(opPoint, opPointRef)
@@ -65,13 +82,13 @@ class MetaTaskKine6dRel(MetaTask6d):
@opmodifBase.setter
def opmodifBase(self, m):
if isinstance(m, bool) and not m:
- plug(self.dyn.signal(self.opPointBase), self.feature.signal('positionRef'))
- plug(self.dyn.signal('J' + self.opPointBase), self.feature.signal('JqRef'))
+ plug(self.dyn.signal(self.opPointBase), self.feature.signal("positionRef"))
+ plug(self.dyn.signal("J" + self.opPointBase), self.feature.signal("JqRef"))
self.opPointModifBase.activ = False
else:
if not self.opPointModifBase.activ:
- plug(self.opPointModifBase.signal('position'), self.feature.positionRef)
- plug(self.opPointModifBase.signal('jacobian'), self.feature.JqRef)
+ plug(self.opPointModifBase.signal("position"), self.feature.positionRef)
+ plug(self.opPointModifBase.signal("jacobian"), self.feature.JqRef)
self.opPointModifBase.setTransformation(m)
self.opPointModifBase.activ = True
@@ -86,7 +103,10 @@ def goto6dRel(task, position, positionRef, gain=None, resetJacobian=True):
task.featureDes.positionRef.value = matrixToTuple(MRef)
task.feature.selec.value = Flags("111111")
setGain(task.gain, gain)
- if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian:
+ if (
+ "resetJacobianDerivative" in task.task.__class__.__dict__.keys()
+ and resetJacobian
+ ):
task.task.resetJacobianDerivative()
@@ -100,7 +120,10 @@ def gotoNdRel(task, position, positionRef, selec=None, gain=None, resetJacobian=
task.featureDes.position.value = matrixToTuple(M)
task.featureDes.positionRef.value = matrixToTuple(MRef)
setGain(task.gain, gain)
- if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian:
+ if (
+ "resetJacobianDerivative" in task.task.__class__.__dict__.keys()
+ and resetJacobian
+ ):
task.task.resetJacobianDerivative()
diff --git a/src/dynamic_graph/sot/core/utils/attime.py b/src/dynamic_graph/sot/core/utils/attime.py
index dc804774..7b7d84db 100644
--- a/src/dynamic_graph/sot/core/utils/attime.py
+++ b/src/dynamic_graph/sot/core/utils/attime.py
@@ -32,14 +32,14 @@ class Calendar:
# self.periodic=list()
def __repr__(self):
- res = ''
+ res = ""
for iter, funpairs in sorted(self.events.iteritems()):
res += str(iter) + ": \n"
for funpair in funpairs:
- if funpair[1] == '':
- res += funpair[0] + '\n'
+ if funpair[1] == "":
+ res += funpair[0] + "\n"
else:
- res += str(funpair[1]) + '\n'
+ res += str(funpair[1]) + "\n"
return res
def stop(self, *args):
@@ -57,10 +57,10 @@ class Calendar:
self.events[iter].append(pairfundoc)
def registerEvents(self, iter, *funs):
- '''
+ """
3 entry types are possible: 1. only the functor. 2. a pair
(functor,doc). 3. a list of pairs (functor,doc).
- '''
+ """
if len(funs) == 2 and callable(funs[0]) and isinstance(funs[1], str):
self.registerEvent(iter, (funs[0], funs[1]))
else:
@@ -68,7 +68,7 @@ class Calendar:
if isinstance(fun, tuple):
self.registerEvent(iter, fun)
else: # assert iscallable(fun)
- if 'functor' in fun.__dict__:
+ if "functor" in fun.__dict__:
self.registerEvent(iter, (fun.functor, fun.functor.__doc__))
else:
self.registerEvent(iter, (fun, fun.__doc__))
@@ -108,6 +108,7 @@ class Calendar:
This next calling pattern is a little bit strange. Use it to decorate
a function definition: @attime(30) def run30(): ...
"""
+
class calendarDeco:
iterRef = iterarg
calendarRef = self
@@ -118,9 +119,13 @@ class Calendar:
functer.__doc__ = "No doc fun"
if len(functer.__doc__) > 0:
selfdeco.__doc__ = functer.__doc__
- selfdeco.__doc__ += " (will be run at time " + str(selfdeco.iterRef) + ")"
+ selfdeco.__doc__ += (
+ " (will be run at time " + str(selfdeco.iterRef) + ")"
+ )
selfdeco.fun = functer
- selfdeco.calendarRef.registerEvents(selfdeco.iterRef, functer, functer.__doc__)
+ selfdeco.calendarRef.registerEvents(
+ selfdeco.iterRef, functer, functer.__doc__
+ )
def __call__(selfdeco, *args):
selfdeco.fun(*args)
@@ -134,5 +139,5 @@ class Calendar:
attime = Calendar()
-sigset = (lambda s, v: s.__class__.value.__set__(s, v))
-refset = (lambda mt, v: mt.__class__.ref.__set__(mt, v))
+sigset = lambda s, v: s.__class__.value.__set__(s, v)
+refset = lambda mt, v: mt.__class__.ref.__set__(mt, v)
diff --git a/src/dynamic_graph/sot/core/utils/history.py b/src/dynamic_graph/sot/core/utils/history.py
index c098b50e..2e6dcd81 100644
--- a/src/dynamic_graph/sot/core/utils/history.py
+++ b/src/dynamic_graph/sot/core/utils/history.py
@@ -13,7 +13,9 @@ class History:
self.freq = freq
self.zmpSig = zmpSig
self.dynEnt = dynEnt
- self.withZmp = (self.zmpSig is not None) and ("waist" in map(lambda x: x.name, self.dynEnt.signals()))
+ self.withZmp = (self.zmpSig is not None) and (
+ "waist" in map(lambda x: x.name, self.dynEnt.signals())
+ )
def record(self):
i = self.dynEnt.position.time
@@ -22,7 +24,7 @@ class History:
self.qdot.append(self.dynEnt.velocity.value)
if self.withZmp:
waMwo = matrix(self.dynEnt.waist.value).I
- wo_z = matrix(self.zmpSig.value + (1, )).T
+ wo_z = matrix(self.zmpSig.value + (1,)).T
self.zmp.append(list(vectorToTuple(waMwo * wo_z)))
def restore(self, t):
@@ -35,27 +37,58 @@ class History:
print("attime.fastForward(T0)")
def dumpToOpenHRP(self, baseName="dyninv", sample=1):
- filePos = open(baseName + '.pos', 'w')
- fileRPY = open(baseName + '.hip', 'w')
- fileWaist = open(baseName + '.waist', 'w')
+ filePos = open(baseName + ".pos", "w")
+ fileRPY = open(baseName + ".hip", "w")
+ fileWaist = open(baseName + ".waist", "w")
sampleT = 0.005
for nT, q in enumerate(self.q):
- fileRPY.write(str(sampleT * nT) + ' ' + str(q[3]) + ' ' + str(q[4]) + ' ' + str(q[5]) + '\n')
+ fileRPY.write(
+ str(sampleT * nT)
+ + " "
+ + str(q[3])
+ + " "
+ + str(q[4])
+ + " "
+ + str(q[5])
+ + "\n"
+ )
fileWaist.write(
- str(sampleT * nT) + ' ' + str(q[0]) + ' ' + str(q[1]) + ' ' + str(q[2]) + ' ' + str(q[3]) + ' ' +
- str(q[4]) + ' ' + str(q[5]) + '\n')
- filePos.write(str(sampleT * nT) + ' ')
+ str(sampleT * nT)
+ + " "
+ + str(q[0])
+ + " "
+ + str(q[1])
+ + " "
+ + str(q[2])
+ + " "
+ + str(q[3])
+ + " "
+ + str(q[4])
+ + " "
+ + str(q[5])
+ + "\n"
+ )
+ filePos.write(str(sampleT * nT) + " ")
for j in range(6, 36):
- filePos.write(str(q[j]) + ' ')
- filePos.write(10 * ' 0' + '\n')
+ filePos.write(str(q[j]) + " ")
+ filePos.write(10 * " 0" + "\n")
if self.withZmp:
- fileZMP = open(baseName + '.zmp', 'w')
+ fileZMP = open(baseName + ".zmp", "w")
for nT, z in enumerate(self.zmp):
- fileZMP.write(str(sampleT * nT) + ' ' + str(z[0]) + ' ' + str(z[1]) + ' ' + str(z[2]) + '\n')
+ fileZMP.write(
+ str(sampleT * nT)
+ + " "
+ + str(z[0])
+ + " "
+ + str(z[1])
+ + " "
+ + str(z[2])
+ + "\n"
+ )
- filePos0 = open(baseName + '_pos0.py', 'w')
+ filePos0 = open(baseName + "_pos0.py", "w")
filePos0.write("dyninv_posinit = '")
q0 = self.q[0]
for x in q0[6:36]:
- filePos0.write(str(x * 180.0 / pi) + ' ')
+ filePos0.write(str(x * 180.0 / pi) + " ")
filePos0.write(" 0 0 0 0 0 0 0 0 0 0 '")
diff --git a/src/dynamic_graph/sot/core/utils/thread_interruptible_loop.py b/src/dynamic_graph/sot/core/utils/thread_interruptible_loop.py
index 9ffeea5f..7b937c06 100644
--- a/src/dynamic_graph/sot/core/utils/thread_interruptible_loop.py
+++ b/src/dynamic_graph/sot/core/utils/thread_interruptible_loop.py
@@ -60,7 +60,7 @@ class ThreadInterruptibleLoop(threading.Thread):
self.pause()
time.sleep(self.sleepTime)
self.isRunning = False
- print('Thread loop will now end.')
+ print("Thread loop will now end.")
def start(self):
self.setPlay(True)
@@ -88,7 +88,8 @@ def loopInThread(funLoop):
class ThreadViewer(ThreadInterruptibleLoop):
def __init__(self):
ThreadInterruptibleLoop.__init__(self)
-# self.start()
+
+ # self.start()
def loop(self):
funLoop()
diff --git a/src/dynamic_graph/sot/core/utils/viewer_helper.py b/src/dynamic_graph/sot/core/utils/viewer_helper.py
index 2215cb05..ca78b11f 100644
--- a/src/dynamic_graph/sot/core/utils/viewer_helper.py
+++ b/src/dynamic_graph/sot/core/utils/viewer_helper.py
@@ -11,10 +11,10 @@ def stateFullSize(robot, additionalData=()):
def refreshView(robot):
- if robot.name == 'robot':
- name = 'hrp'
- if robot.name == 'robot_device':
- name = 'hrp'
+ if robot.name == "robot":
+ name = "hrp"
+ if robot.name == "robot_device":
+ name = "hrp"
else:
name = robot.name
robot.viewer.updateElementConfig(name, robot.stateFullSize())
@@ -24,39 +24,39 @@ def refreshView(robot):
def incrementView(robot, dt):
- '''Increment then refresh.'''
+ """Increment then refresh."""
robot.incrementNoView(dt)
robot.refresh()
def setView(robot, *args):
- '''Set robot config then refresh.'''
+ """Set robot config then refresh."""
robot.setNoView(*args)
# print('view')
robot.refresh()
def addRobotViewer(robot, **args):
- '''
+ """
Arguments are:
* small=False
* server='XML-RPC' == { 'XML-RPC' | 'CORBA' }
* verbose=True
- '''
- verbose = args.get('verbose', True)
- small = args.get('small', False)
- small_extra = args.get('small_extra', 10)
- server = args.get('server', 'XML-RPC')
+ """
+ verbose = args.get("verbose", True)
+ small = args.get("small", False)
+ small_extra = args.get("small_extra", 10)
+ server = args.get("server", "XML-RPC")
try:
import robotviewer
+
RobotClass = robot.__class__
if small:
if verbose:
- print('using a small robot')
- RobotClass.stateFullSize = lambda x: \
- stateFullSize(x, small_extra * (0.0, ))
+ print("using a small robot")
+ RobotClass.stateFullSize = lambda x: stateFullSize(x, small_extra * (0.0,))
else:
RobotClass.stateFullSize = stateFullSize
@@ -71,7 +71,7 @@ def addRobotViewer(robot, **args):
robot.displayList = []
# Check the connection
- if args.get('dorefresh', True):
+ if args.get("dorefresh", True):
robot.refresh()
except Exception:
@@ -91,13 +91,15 @@ class VisualPinger:
self.refresh()
def refresh(self):
- self.viewer.updateElementConfig('pong', [0, 0.9, self.pos * 0.1, 0, 0, 0])
+ self.viewer.updateElementConfig("pong", [0, 0.9, self.pos * 0.1, 0, 0, 0])
def __call__(self):
self.pos += 1
self.refresh()
-def updateComDisplay(robot, comsig, objname='com'):
+def updateComDisplay(robot, comsig, objname="com"):
if comsig.time > 0:
- robot.viewer.updateElementConfig(objname, [comsig.value[0], comsig.value[1], 0, 0, 0, 0])
+ robot.viewer.updateElementConfig(
+ objname, [comsig.value[0], comsig.value[1], 0, 0, 0, 0]
+ )
diff --git a/src/dynamic_graph/sot/core/utils/viewer_loger.py b/src/dynamic_graph/sot/core/utils/viewer_loger.py
index 39ef50f4..d2759c5c 100644
--- a/src/dynamic_graph/sot/core/utils/viewer_loger.py
+++ b/src/dynamic_graph/sot/core/utils/viewer_loger.py
@@ -3,7 +3,7 @@ import os
class ViewerLoger:
- '''
+ """
This class replace the robotviewer client and log the data sent to the
viewer for future replay.
@@ -11,19 +11,29 @@ class ViewerLoger:
from viewer_loger import ViewerLoger
robot.viewer = ViewerLoger(robot)
- '''
+ """
+
def __init__(self, robot):
self.robot = robot
self.viewer = robot.viewer
self.fileMap = {}
- for f in glob.glob('/tmp/view*.dat'):
+ for f in glob.glob("/tmp/view*.dat"):
os.remove(f)
def updateElementConfig(self, name, state):
t = self.robot.state.time
if name not in self.fileMap:
- self.fileMap[name] = open('/tmp/view_' + name + '.dat', 'w')
- self.fileMap[name].write("\t".join([str(f) for f in [
- t,
- ] + list(state)]) + '\n')
+ self.fileMap[name] = open("/tmp/view_" + name + ".dat", "w")
+ self.fileMap[name].write(
+ "\t".join(
+ [
+ str(f)
+ for f in [
+ t,
+ ]
+ + list(state)
+ ]
+ )
+ + "\n"
+ )
self.viewer.updateElementConfig(name, state)
diff --git a/src/exception/exception-abstract.cpp b/src/exception/exception-abstract.cpp
index 97d7ddd1..17742eab 100644
--- a/src/exception/exception-abstract.cpp
+++ b/src/exception/exception-abstract.cpp
@@ -20,7 +20,8 @@ using namespace dynamicgraph::sot;
const std::string ExceptionAbstract::EXCEPTION_NAME = "Abstract";
ExceptionAbstract::ExceptionAbstract(const int &_code, const string &_msg)
- : code(_code), message(_msg)
+ : code(_code),
+ message(_msg)
{
return;
@@ -65,7 +66,7 @@ ExceptionAbstract::Param::Param(const int &_line, const char *_function,
: functionPTR(_function), line(_line), filePTR(_file), pointersSet(true) {
sotDEBUGINOUT(25);
}
-#endif //#ifdef SOT_EXCEPTION_PASSING_PARAM
+#endif //#ifdef SOT_EXCEPTION_PASSING_PARAM
/* ------------------------------------------------------------------------- */
/* --- OP << --------------------------------------------------------------- */
@@ -81,11 +82,11 @@ ostream &operator<<(ostream &os, const ExceptionAbstract &error) {
if (error.p.set)
os << "Thrown from " << error.p.file << ": " << error.p.function << " (#"
<< error.p.line << ")" << endl;
-#endif //#ifdef SOT_EXCEPTION_PASSING_PARAM
+#endif //#ifdef SOT_EXCEPTION_PASSING_PARAM
return os;
}
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
/** \file $Source$
*/
diff --git a/src/exception/exception-dynamic.cpp b/src/exception/exception-dynamic.cpp
index e154d138..d2ee691d 100644
--- a/src/exception/exception-dynamic.cpp
+++ b/src/exception/exception-dynamic.cpp
@@ -7,9 +7,10 @@
*
*/
+#include <stdarg.h>
+
#include <cstdio>
#include <sot/core/exception-dynamic.hh>
-#include <stdarg.h>
using namespace dynamicgraph::sot;
diff --git a/src/exception/exception-factory.cpp b/src/exception/exception-factory.cpp
index bda77b1e..310580b7 100644
--- a/src/exception/exception-factory.cpp
+++ b/src/exception/exception-factory.cpp
@@ -7,10 +7,11 @@
*
*/
+#include <stdarg.h>
+
#include <cstdio>
#include <sot/core/debug.hh>
#include <sot/core/exception-factory.hh>
-#include <stdarg.h>
using namespace dynamicgraph::sot;
diff --git a/src/exception/exception-feature.cpp b/src/exception/exception-feature.cpp
index 34efa1ed..4c8c5563 100644
--- a/src/exception/exception-feature.cpp
+++ b/src/exception/exception-feature.cpp
@@ -7,9 +7,10 @@
*
*/
+#include <stdarg.h>
+
#include <cstdio>
#include <sot/core/exception-feature.hh>
-#include <stdarg.h>
using namespace dynamicgraph::sot;
diff --git a/src/exception/exception-signal.cpp b/src/exception/exception-signal.cpp
index 1cb6ce0b..1cc9f825 100644
--- a/src/exception/exception-signal.cpp
+++ b/src/exception/exception-signal.cpp
@@ -7,9 +7,10 @@
*
*/
+#include <stdarg.h>
+
#include <cstdio>
#include <sot/core/exception-signal.hh>
-#include <stdarg.h>
using namespace dynamicgraph::sot;
diff --git a/src/exception/exception-task.cpp b/src/exception/exception-task.cpp
index 1d301291..0d916fdf 100644
--- a/src/exception/exception-task.cpp
+++ b/src/exception/exception-task.cpp
@@ -7,9 +7,10 @@
*
*/
+#include <stdarg.h>
+
#include <cstdio>
#include <sot/core/exception-task.hh>
-#include <stdarg.h>
using namespace dynamicgraph::sot;
diff --git a/src/exception/exception-tools.cpp b/src/exception/exception-tools.cpp
index 6ef436bd..152c0b86 100644
--- a/src/exception/exception-tools.cpp
+++ b/src/exception/exception-tools.cpp
@@ -7,9 +7,10 @@
*
*/
+#include <stdarg.h>
+
#include <cstdio>
#include <sot/core/exception-tools.hh>
-#include <stdarg.h>
using namespace dynamicgraph::sot;
diff --git a/src/factory/additional-functions.cpp b/src/factory/additional-functions.cpp
index a8a28e39..89dab4d5 100644
--- a/src/factory/additional-functions.cpp
+++ b/src/factory/additional-functions.cpp
@@ -8,6 +8,7 @@
*/
#include <dynamic-graph/signal.h>
+
#include <sot/core/additional-functions.hh>
#include <sot/core/debug.hh>
#include <sot/core/factory.hh>
diff --git a/src/factory/pool.cpp b/src/factory/pool.cpp
index 5a820bc9..c77cd8c3 100644
--- a/src/factory/pool.cpp
+++ b/src/factory/pool.cpp
@@ -17,6 +17,7 @@
/* --- SOT --- */
#include <dynamic-graph/entity.h>
+
#include <sot/core/debug.hh>
#include <sot/core/feature-abstract.hh>
#include <sot/core/pool.hh>
@@ -38,7 +39,7 @@ PoolStorage::~PoolStorage(void) {
/* --------------------------------------------------------------------- */
void PoolStorage::registerTask(const std::string &entname, TaskAbstract *ent) {
Tasks::iterator entkey = task.find(entname);
- if (entkey != task.end()) // key does exist
+ if (entkey != task.end()) // key does exist
{
throw ExceptionFactory(ExceptionFactory::OBJECT_CONFLICT,
"Another task already defined with the "
@@ -65,7 +66,7 @@ TaskAbstract &PoolStorage::getTask(const std::string &name) {
void PoolStorage::registerFeature(const std::string &entname,
FeatureAbstract *ent) {
Features::iterator entkey = feature.find(entname);
- if (entkey != feature.end()) // key does exist
+ if (entkey != feature.end()) // key does exist
{
throw ExceptionFactory(ExceptionFactory::OBJECT_CONFLICT,
"Another feature already defined with the"
@@ -157,5 +158,5 @@ void PoolStorage::destroy() {
PoolStorage::PoolStorage() {}
PoolStorage *PoolStorage::instance_ = 0;
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
diff --git a/src/feature/feature-1d.cpp b/src/feature/feature-1d.cpp
index fde0ec72..96357841 100644
--- a/src/feature/feature-1d.cpp
+++ b/src/feature/feature-1d.cpp
@@ -77,8 +77,7 @@ dynamicgraph::Matrix &Feature1D::computeJacobian(dynamicgraph::Matrix &res,
res.resize(1, Jac.cols());
res.fill(0);
for (int j = 0; j < Jac.cols(); ++j)
- for (int i = 0; i < Jac.rows(); ++i)
- res(0, j) += err(i) * Jac(i, j);
+ for (int i = 0; i < Jac.rows(); ++i) res(0, j) += err(i) * Jac(i, j);
sotDEBUGOUT(15);
return res;
diff --git a/src/feature/feature-abstract.cpp b/src/feature/feature-abstract.cpp
index 2eeb603c..42c0791f 100644
--- a/src/feature/feature-abstract.cpp
+++ b/src/feature/feature-abstract.cpp
@@ -7,13 +7,14 @@
*
*/
+#include <dynamic-graph/all-commands.h>
+
#include <iostream>
+#include <sot/core/feature-abstract.hh>
+#include <sot/core/pool.hh>
#include "sot/core/debug.hh"
#include "sot/core/exception-feature.hh"
-#include <dynamic-graph/all-commands.h>
-#include <sot/core/feature-abstract.hh>
-#include <sot/core/pool.hh>
using namespace dynamicgraph::sot;
using dynamicgraph::sot::ExceptionFeature;
@@ -21,10 +22,11 @@ using dynamicgraph::sot::ExceptionFeature;
const std::string FeatureAbstract::CLASS_NAME = "FeatureAbstract";
FeatureAbstract::FeatureAbstract(const std::string &name)
- : Entity(name), selectionSIN(NULL, "sotFeatureAbstract(" + name +
- ")::input(flag)::selec"),
- errordotSIN(NULL, "sotFeatureAbstract(" + name +
- ")::input(vector)::errordotIN"),
+ : Entity(name),
+ selectionSIN(NULL,
+ "sotFeatureAbstract(" + name + ")::input(flag)::selec"),
+ errordotSIN(
+ NULL, "sotFeatureAbstract(" + name + ")::input(vector)::errordotIN"),
errorSOUT(boost::bind(&FeatureAbstract::computeError, this, _1, _2),
selectionSIN,
"sotFeatureAbstract(" + name + ")::output(vector)::error"),
@@ -33,10 +35,10 @@ FeatureAbstract::FeatureAbstract(const std::string &name)
"sotFeatureAbstract(" + name + ")::output(vector)::errordot")
,
- jacobianSOUT(boost::bind(&FeatureAbstract::computeJacobian, this, _1, _2),
- selectionSIN,
- "sotFeatureAbstract(" + name +
- ")::output(matrix)::jacobian"),
+ jacobianSOUT(
+ boost::bind(&FeatureAbstract::computeJacobian, this, _1, _2),
+ selectionSIN,
+ "sotFeatureAbstract(" + name + ")::output(matrix)::jacobian"),
dimensionSOUT(boost::bind(&FeatureAbstract::getDimension, this, _1, _2),
selectionSIN,
"sotFeatureAbstract(" + name + ")::output(uint)::dim") {
@@ -90,8 +92,8 @@ std::string FeatureAbstract::getReferenceByName() const {
return "none";
}
-dynamicgraph::Vector &
-FeatureAbstract::computeErrorDot(dynamicgraph::Vector &res, int time) {
+dynamicgraph::Vector &FeatureAbstract::computeErrorDot(
+ dynamicgraph::Vector &res, int time) {
const Flags &fl = selectionSIN.access(time);
const int &dim = dimensionSOUT(time);
@@ -113,8 +115,7 @@ FeatureAbstract::computeErrorDot(dynamicgraph::Vector &res, int time) {
}
for (int i = 0; i < errdotDes.size(); ++i)
- if (fl(i))
- res(curr++) = -errdotDes(i);
+ if (fl(i)) res(curr++) = -errdotDes(i);
} else
res.setZero();
diff --git a/src/feature/feature-generic.cpp b/src/feature/feature-generic.cpp
index 94cb5fe2..a330dc77 100644
--- a/src/feature/feature-generic.cpp
+++ b/src/feature/feature-generic.cpp
@@ -27,7 +27,8 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureGeneric, "FeatureGeneric");
/* --------------------------------------------------------------------- */
FeatureGeneric::FeatureGeneric(const string &pointName)
- : FeatureAbstract(pointName), dimensionDefault(0),
+ : FeatureAbstract(pointName),
+ dimensionDefault(0),
errorSIN(NULL, "sotFeatureGeneric(" + name + ")::input(vector)::errorIN"),
jacobianSIN(NULL,
"sotFeatureGeneric(" + name + ")::input(matrix)::jacobianIN")
@@ -64,13 +65,11 @@ unsigned int &FeatureGeneric::getDimension(unsigned int &dim, int time) {
const Flags &fl = selectionSIN.access(time);
- if (dimensionDefault == 0)
- dimensionDefault = errorSIN.access(time).size();
+ if (dimensionDefault == 0) dimensionDefault = errorSIN.access(time).size();
dim = 0;
for (unsigned int i = 0; i < dimensionDefault; ++i)
- if (fl(i))
- dim++;
+ if (fl(i)) dim++;
sotDEBUG(25) << "# Out }" << endl;
return dim;
@@ -107,12 +106,10 @@ Vector &FeatureGeneric::computeError(Vector &res, int time) {
for (int i = 0; i < err.size(); ++i)
if (fl(i))
- if (fl(i))
- res(curr++) = err(i) - errDes(i);
+ if (fl(i)) res(curr++) = err(i) - errDes(i);
} else {
for (int i = 0; i < err.size(); ++i)
- if (fl(i))
- res(curr++) = err(i);
+ if (fl(i)) res(curr++) = err(i);
}
return res;
@@ -130,8 +127,7 @@ Matrix &FeatureGeneric::computeJacobian(Matrix &res, int time) {
for (unsigned int i = 0; curr < dim; ++i)
if (fl(i)) {
- for (int j = 0; j < Jac.cols(); ++j)
- res(curr, j) = Jac(i, j);
+ for (int j = 0; j < Jac.cols(); ++j) res(curr, j) = Jac(i, j);
curr++;
}
diff --git a/src/feature/feature-joint-limits-command.h b/src/feature/feature-joint-limits-command.h
index f998e4db..08fface5 100644
--- a/src/feature/feature-joint-limits-command.h
+++ b/src/feature/feature-joint-limits-command.h
@@ -9,12 +9,12 @@
#ifndef FEATURE_JOINT_LIMITS_COMMAND_H
#define FEATURE_JOINT_LIMITS_COMMAND_H
-#include <boost/assign/list_of.hpp>
-
#include <dynamic-graph/command-getter.h>
#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/command.h>
+#include <boost/assign/list_of.hpp>
+
namespace dynamicgraph {
namespace sot {
namespace command {
@@ -24,7 +24,7 @@ using ::dynamicgraph::command::Value;
// Command Actuate
class Actuate : public Command {
-public:
+ public:
virtual ~Actuate() {}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
@@ -33,15 +33,15 @@ public:
: Command(entity, std::vector<Value::Type>(), docstring) {}
virtual Value doExecute() {
FeatureJointLimits &fjl = static_cast<FeatureJointLimits &>(owner());
- Flags fl(63); // 0x0000003f = 00000000000000000000000000111111
+ Flags fl(63); // 0x0000003f = 00000000000000000000000000111111
fjl.selectionSIN = (!fl);
// return void
return Value();
}
-}; // class Actuate
-} // namespace featureJointLimits
-} // namespace command
+}; // class Actuate
+} // namespace featureJointLimits
+} // namespace command
} /* namespace sot */
} /* namespace dynamicgraph */
-#endif // FEATURE_JOINT_LIMITS_COMMAND_H
+#endif // FEATURE_JOINT_LIMITS_COMMAND_H
diff --git a/src/feature/feature-joint-limits.cpp b/src/feature/feature-joint-limits.cpp
index be88135b..c36ef573 100644
--- a/src/feature/feature-joint-limits.cpp
+++ b/src/feature/feature-joint-limits.cpp
@@ -18,6 +18,7 @@
using namespace std;
#include <../src/feature/feature-joint-limits-command.h>
+
#include <sot/core/factory.hh>
/* --------------------------------------------------------------------- */
@@ -31,7 +32,8 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureJointLimits, "FeatureJointLimits");
const double FeatureJointLimits::THRESHOLD_DEFAULT = .9;
FeatureJointLimits::FeatureJointLimits(const string &fName)
- : FeatureAbstract(fName), threshold(THRESHOLD_DEFAULT)
+ : FeatureAbstract(fName),
+ threshold(THRESHOLD_DEFAULT)
,
jointSIN(NULL,
@@ -54,9 +56,10 @@ FeatureJointLimits::FeatureJointLimits(const string &fName)
//
std::string docstring;
// Actuate
- docstring = " \n"
- " Actuate\n"
- " \n";
+ docstring =
+ " \n"
+ " Actuate\n"
+ " \n";
addCommand("actuate",
new command::featureJointLimits::Actuate(*this, docstring));
}
@@ -78,8 +81,7 @@ unsigned int &FeatureJointLimits::getDimension(unsigned int &dim, int time) {
dim = 0;
for (Matrix::Index i = 0; i < NBJL; ++i)
- if (fl(static_cast<int>(i)))
- dim++;
+ if (fl(static_cast<int>(i))) dim++;
sotDEBUG(25) << "# Out }" << endl;
return dim;
diff --git a/src/feature/feature-line-distance.cpp b/src/feature/feature-line-distance.cpp
index 36622bd6..48d2b24d 100644
--- a/src/feature/feature-line-distance.cpp
+++ b/src/feature/feature-line-distance.cpp
@@ -15,7 +15,6 @@
#include <sot/core/debug.hh>
#include <sot/core/exception-feature.hh>
#include <sot/core/feature-line-distance.hh>
-
#include <sot/core/matrix-geometry.hh>
using namespace std;
@@ -34,8 +33,8 @@ FeatureLineDistance::FeatureLineDistance(const string &pointName)
: FeatureAbstract(pointName),
positionSIN(NULL, "sotFeatureLineDistance(" + name +
")::input(matrixHomo)::position"),
- articularJacobianSIN(NULL, "sotFeatureLineDistance(" + name +
- ")::input(matrix)::Jq"),
+ articularJacobianSIN(
+ NULL, "sotFeatureLineDistance(" + name + ")::input(matrix)::Jq"),
positionRefSIN(NULL, "sotFeatureLineDistance(" + name +
")::input(vector)::positionRef"),
vectorSIN(NULL,
@@ -104,7 +103,7 @@ Matrix &FeatureLineDistance::computeJacobian(Matrix &J, int time) {
const Vector &vect = vectorSIN(time);
const MatrixHomogeneous &M = positionSIN(time);
MatrixRotation R;
- R = M.linear(); // wRh
+ R = M.linear(); // wRh
Matrix Skew(3, 3);
Skew(0, 0) = 0;
diff --git a/src/feature/feature-point6d-relative.cpp b/src/feature/feature-point6d-relative.cpp
index a9a0484a..471d5e14 100644
--- a/src/feature/feature-point6d-relative.cpp
+++ b/src/feature/feature-point6d-relative.cpp
@@ -12,12 +12,12 @@
/* --------------------------------------------------------------------- */
/* --- SOT --- */
+#include <dynamic-graph/all-commands.h>
+#include <dynamic-graph/pool.h>
+
#include <sot/core/debug.hh>
#include <sot/core/exception-feature.hh>
#include <sot/core/feature-point6d-relative.hh>
-
-#include <dynamic-graph/all-commands.h>
-#include <dynamic-graph/pool.h>
#include <sot/core/macros.hh>
#include <sot/core/matrix-geometry.hh>
@@ -101,8 +101,7 @@ Matrix &FeaturePoint6dRelative::computeJacobian(Matrix &Jres, int time) {
unsigned int rJ = 0;
for (unsigned int r = 0; r < 6; ++r)
if (fl(r)) {
- for (unsigned int c = 0; c < cJ; ++c)
- Jres(rJ, c) = J(r, c);
+ for (unsigned int c = 0; c < cJ; ++c) Jres(rJ, c) = J(r, c);
rJ++;
}
@@ -166,12 +165,10 @@ Vector &FeaturePoint6dRelative::computeError(Vector &error, int time) {
error.resize(dimensionSOUT(time));
unsigned int cursor = 0;
for (unsigned int i = 0; i < 3; ++i) {
- if (fl(i))
- error(cursor++) = Merr(i, 3);
+ if (fl(i)) error(cursor++) = Merr(i, 3);
}
for (unsigned int i = 0; i < 3; ++i) {
- if (fl(i + 3))
- error(cursor++) = rerr.angle() * rerr.axis()(i);
+ if (fl(i + 3)) error(cursor++) = rerr.angle() * rerr.axis()(i);
}
sotDEBUGOUT(15);
@@ -246,12 +243,10 @@ Vector &FeaturePoint6dRelative::computeErrorDot(Vector &errordot, int time) {
errordot.resize(dimensionSOUT(time));
unsigned int cursor = 0;
for (unsigned int i = 0; i < 3; ++i) {
- if (fl(i))
- errordot(cursor++) = dtrerr(i);
+ if (fl(i)) errordot(cursor++) = dtrerr(i);
}
for (unsigned int i = 0; i < 3; ++i) {
- if (fl(i + 3))
- errordot(cursor++) = rerr.angle() * rerr.axis()(i);
+ if (fl(i + 3)) errordot(cursor++) = rerr.angle() * rerr.axis()(i);
}
sotDEBUGOUT(15);
diff --git a/src/feature/feature-point6d.cpp b/src/feature/feature-point6d.cpp
index 536fa2cf..6e631167 100644
--- a/src/feature/feature-point6d.cpp
+++ b/src/feature/feature-point6d.cpp
@@ -22,7 +22,6 @@
#include <dynamic-graph/command.h>
#include <Eigen/LU>
-
#include <sot/core/debug.hh>
#include <sot/core/exception-feature.hh>
#include <sot/core/feature-point6d.hh>
@@ -46,14 +45,21 @@ const FeaturePoint6d::ComputationFrameType
FeaturePoint6d::COMPUTATION_FRAME_DEFAULT = FRAME_DESIRED;
FeaturePoint6d::FeaturePoint6d(const string &pointName)
- : FeatureAbstract(pointName), computationFrame_(COMPUTATION_FRAME_DEFAULT),
- positionSIN(NULL, "sotFeaturePoint6d(" + name +
- ")::input(matrixHomo)::position"),
+ : FeatureAbstract(pointName),
+ computationFrame_(COMPUTATION_FRAME_DEFAULT),
+ positionSIN(
+ NULL, "sotFeaturePoint6d(" + name + ")::input(matrixHomo)::position"),
velocitySIN(NULL,
"sotFeaturePoint6d(" + name + ")::input(vector)::velocity"),
- articularJacobianSIN(NULL, "sotFeaturePoint6d(" + name +
- ")::input(matrix)::Jq"),
- error_th_(), R_(), Rref_(), Rt_(), Rreft_(), P_(3, 3), Pinv_(3, 3),
+ articularJacobianSIN(
+ NULL, "sotFeaturePoint6d(" + name + ")::input(matrix)::Jq"),
+ error_th_(),
+ R_(),
+ Rref_(),
+ Rt_(),
+ Rreft_(),
+ P_(3, 3),
+ Pinv_(3, 3),
accuracy_(1e-8) {
jacobianSOUT.addDependency(positionSIN);
jacobianSOUT.addDependency(articularJacobianSIN);
@@ -74,24 +80,26 @@ FeaturePoint6d::FeaturePoint6d(const string &pointName)
using namespace dynamicgraph::command;
std::string docstring;
// Set computation frame
- docstring = "Set computation frame\n"
- "\n"
- " Input:\n"
- " a string: 'current' or 'desired'.\n"
- " If 'current', the error is defined as the rotation "
- "vector (VectorUTheta)\n"
- " corresponding to the position of the reference in the "
- "current frame:\n"
- " -1 *\n"
- " error = utheta (M M )\n"
- " If 'desired', *-1\n"
- " error = utheta (M M)\n";
+ docstring =
+ "Set computation frame\n"
+ "\n"
+ " Input:\n"
+ " a string: 'current' or 'desired'.\n"
+ " If 'current', the error is defined as the rotation "
+ "vector (VectorUTheta)\n"
+ " corresponding to the position of the reference in the "
+ "current frame:\n"
+ " -1 *\n"
+ " error = utheta (M M )\n"
+ " If 'desired', *-1\n"
+ " error = utheta (M M)\n";
addCommand("frame",
new dynamicgraph::command::Setter<FeaturePoint6d, std::string>(
*this, &FeaturePoint6d::computationFrame, docstring));
- docstring = "Get frame of computation of the error\n"
- "\n"
- " See command 'frame' for definition.\n";
+ docstring =
+ "Get frame of computation of the error\n"
+ "\n"
+ " See command 'frame' for definition.\n";
addCommand("getFrame",
new dynamicgraph::command::Getter<FeaturePoint6d, std::string>(
*this, &FeaturePoint6d::computationFrame, docstring));
@@ -135,10 +143,10 @@ void FeaturePoint6d::computationFrame(const std::string &inFrame) {
/// \brief Get computation frame
std::string FeaturePoint6d::computationFrame() const {
switch (computationFrame_) {
- case FRAME_CURRENT:
- return "current";
- case FRAME_DESIRED:
- return "desired";
+ case FRAME_CURRENT:
+ return "current";
+ case FRAME_DESIRED:
+ return "desired";
}
assert(false && "Case not handled");
return "Case not handled";
@@ -154,8 +162,7 @@ unsigned int &FeaturePoint6d::getDimension(unsigned int &dim, int time) {
dim = 0;
for (int i = 0; i < 6; ++i)
- if (fl(i))
- dim++;
+ if (fl(i)) dim++;
sotDEBUG(25) << "# Out }" << endl;
return dim;
@@ -197,12 +204,10 @@ Matrix &FeaturePoint6d::computeJacobian(Matrix &J, int time) {
if (isReferenceSet()) {
const MatrixHomogeneous &wMhd = getReference()->positionSIN(time);
wRhd = wMhd.linear();
- for (unsigned int i = 0; i < 3; ++i)
- hdth(i) = wMhd(i, 3) - wMh(i, 3);
+ for (unsigned int i = 0; i < 3; ++i) hdth(i) = wMhd(i, 3) - wMh(i, 3);
} else {
wRhd.setIdentity();
- for (unsigned int i = 0; i < 3; ++i)
- hdth(i) = -wMh(i, 3);
+ for (unsigned int i = 0; i < 3; ++i) hdth(i) = -wMh(i, 3);
}
Rhdth = (wRh.inverse()) * hdth;
MatrixRotation hdRh;
@@ -264,8 +269,7 @@ Matrix &FeaturePoint6d::computeJacobian(Matrix &J, int time) {
unsigned int rJ = 0;
for (unsigned int r = 0; r < 6; ++r)
if (fl(r)) {
- for (unsigned int c = 0; c < cJ; ++c)
- J(rJ, c) = LJq(r, c);
+ for (unsigned int c = 0; c < cJ; ++c) J(rJ, c) = LJq(r, c);
rJ++;
}
@@ -273,13 +277,13 @@ Matrix &FeaturePoint6d::computeJacobian(Matrix &J, int time) {
return J;
}
-#define SOT_COMPUTE_H1MH2(wMh, wMhd, hMhd) \
- { \
- MatrixHomogeneous hMw; \
- hMw = wMh.inverse(Eigen::Affine); \
- sotDEBUG(15) << "hMw = " << hMw << endl; \
- hMhd = hMw * wMhd; \
- sotDEBUG(15) << "hMhd = " << hMhd << endl; \
+#define SOT_COMPUTE_H1MH2(wMh, wMhd, hMhd) \
+ { \
+ MatrixHomogeneous hMw; \
+ hMw = wMh.inverse(Eigen::Affine); \
+ sotDEBUG(15) << "hMw = " << hMw << endl; \
+ hMhd = hMw * wMhd; \
+ sotDEBUG(15) << "hMhd = " << hMhd << endl; \
}
/** Compute the error between two visual features from a subset
@@ -303,21 +307,21 @@ Vector &FeaturePoint6d::computeError(Vector &error, int time) {
const MatrixHomogeneous &wMhd = getReference()->positionSIN(time);
sotDEBUG(15) << "wMhd = " << wMhd << endl;
switch (computationFrame_) {
- case FRAME_CURRENT:
- SOT_COMPUTE_H1MH2(wMh, wMhd, hMhd);
- break;
- case FRAME_DESIRED:
- SOT_COMPUTE_H1MH2(wMhd, wMh, hMhd);
- break; // Compute hdMh indeed.
+ case FRAME_CURRENT:
+ SOT_COMPUTE_H1MH2(wMh, wMhd, hMhd);
+ break;
+ case FRAME_DESIRED:
+ SOT_COMPUTE_H1MH2(wMhd, wMh, hMhd);
+ break; // Compute hdMh indeed.
};
} else {
switch (computationFrame_) {
- case FRAME_CURRENT:
- hMhd = wMh.inverse();
- break;
- case FRAME_DESIRED:
- hMhd = wMh;
- break; // Compute hdMh indeed.
+ case FRAME_CURRENT:
+ hMhd = wMh.inverse();
+ break;
+ case FRAME_DESIRED:
+ hMhd = wMh;
+ break; // Compute hdMh indeed.
};
}
@@ -331,8 +335,7 @@ Vector &FeaturePoint6d::computeError(Vector &error, int time) {
error.resize(dimensionSOUT(time));
unsigned int cursor = 0;
for (unsigned int i = 0; i < 3; ++i) {
- if (fl(i))
- error(cursor++) = hMhd(i, 3);
+ if (fl(i)) error(cursor++) = hMhd(i, 3);
}
if (fl(3) || fl(4) || fl(5)) {
@@ -340,8 +343,7 @@ Vector &FeaturePoint6d::computeError(Vector &error, int time) {
hRhd = hMhd.linear();
error_th_.fromRotationMatrix(hRhd);
for (unsigned int i = 0; i < 3; ++i) {
- if (fl(i + 3))
- error(cursor++) = error_th_.angle() * error_th_.axis()(i);
+ if (fl(i + 3)) error(cursor++) = error_th_.angle() * error_th_.axis()(i);
}
}
@@ -436,17 +438,17 @@ Vector &FeaturePoint6d::computeErrordot(Vector &errordot, int time) {
errorSOUT.recompute(time);
inverseJacobianRodrigues();
switch (computationFrame_) {
- case FRAME_CURRENT:
- // \dot{e}_{t} = R^{T} v
- errordot_t_ = Rt_ * v_;
- // \dot{e}_{\theta} = P^{-1}(e_{theta})R^{*T}\omega
- Rreftomega_ = Rreft_ * omega_;
- errordot_th_ = Pinv_ * Rreftomega_;
- break;
- case FRAME_DESIRED:
- errordot_t_ = Rreft_ * (omega_.cross(tref_ - t_) - v_);
- errordot_th_ = -Pinv_ * (Rt_ * omega_);
- break;
+ case FRAME_CURRENT:
+ // \dot{e}_{t} = R^{T} v
+ errordot_t_ = Rt_ * v_;
+ // \dot{e}_{\theta} = P^{-1}(e_{theta})R^{*T}\omega
+ Rreftomega_ = Rreft_ * omega_;
+ errordot_th_ = Pinv_ * Rreftomega_;
+ break;
+ case FRAME_DESIRED:
+ errordot_t_ = Rreft_ * (omega_.cross(tref_ - t_) - v_);
+ errordot_th_ = -Pinv_ * (Rt_ * omega_);
+ break;
}
} else {
errordot_t_.setZero();
diff --git a/src/feature/feature-pose.cpp b/src/feature/feature-pose.cpp
index c3b9cc4f..211aa93d 100644
--- a/src/feature/feature-pose.cpp
+++ b/src/feature/feature-pose.cpp
@@ -13,18 +13,15 @@
/* --- SOT --- */
//#define VP_DEBUG
//#define VP_DEBUG_MODE 45
-#include <boost/mpl/if.hpp>
-#include <boost/type_traits/is_same.hpp>
-
#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/command-getter.h>
#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/command.h>
-#include <pinocchio/multibody/liegroup/liegroup.hpp>
-
#include <Eigen/LU>
-
+#include <boost/mpl/if.hpp>
+#include <boost/type_traits/is_same.hpp>
+#include <pinocchio/multibody/liegroup/liegroup.hpp>
#include <sot/core/factory.hh>
#include <sot/core/feature-pose.hh>
#include <sot/core/feature-pose.hxx>
@@ -36,7 +33,8 @@ SOT_CORE_DISABLE_WARNING_PUSH
SOT_CORE_DISABLE_WARNING_DEPRECATED
typedef FeaturePose<R3xSO3Representation> FeaturePose_t;
typedef FeaturePose<SE3Representation> FeaturePoseSE3_t;
-template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePose_t, "FeaturePose");
+template <>
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePose_t, "FeaturePose");
template <>
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePoseSE3_t, "FeaturePoseSE3");
SOT_CORE_DISABLE_WARNING_POP
@@ -45,5 +43,5 @@ namespace dynamicgraph {
namespace sot {
template class FeaturePose<R3xSO3Representation>;
template class FeaturePose<SE3Representation>;
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
diff --git a/src/feature/feature-posture.cpp b/src/feature/feature-posture.cpp
index ad636cd6..ca60c5c8 100644
--- a/src/feature/feature-posture.cpp
+++ b/src/feature/feature-posture.cpp
@@ -1,14 +1,14 @@
// Copyright 2010, François Bleibel, Thomas Moulard, Olivier Stasse,
// JRL, CNRS/AIST.
-#include <boost/assign/list_of.hpp>
#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/pool.h>
-#include <string>
+#include <boost/assign/list_of.hpp>
#include <boost/numeric/conversion/cast.hpp>
#include <sot/core/feature-posture.hh>
+#include <string>
namespace dg = ::dynamicgraph;
using dynamicgraph::sot::FeatureAbstract;
@@ -19,7 +19,7 @@ using command::Command;
using command::Value;
class FeaturePosture::SelectDof : public Command {
-public:
+ public:
virtual ~SelectDof() {}
SelectDof(FeaturePosture &entity, const std::string &docstring)
: Command(entity, boost::assign::list_of(Value::UNSIGNED)(Value::BOOL),
@@ -32,32 +32,34 @@ public:
feature.selectDof(dofId, control);
return Value();
}
-}; // class SelectDof
+}; // class SelectDof
FeaturePosture::FeaturePosture(const std::string &name)
: FeatureAbstract(name),
state_(NULL, "FeaturePosture(" + name + ")::input(Vector)::state"),
posture_(0, "FeaturePosture(" + name + ")::input(Vector)::posture"),
postureDot_(0, "FeaturePosture(" + name + ")::input(Vector)::postureDot"),
- activeDofs_(), nbActiveDofs_(0) {
+ activeDofs_(),
+ nbActiveDofs_(0) {
signalRegistration(state_ << posture_ << postureDot_);
errorSOUT.addDependency(state_);
jacobianSOUT.setConstant(Matrix());
std::string docstring;
- docstring = " \n"
- " Select degree of freedom to control\n"
- " \n"
- " input:\n"
- " - positive integer: rank of degree of freedom,\n"
- " - boolean: whether to control the selected degree of "
- "freedom.\n"
- " \n"
- " Note: rank should be more than 5 since posture is "
- "independent\n"
- " from freeflyer position.\n"
- " \n";
+ docstring =
+ " \n"
+ " Select degree of freedom to control\n"
+ " \n"
+ " input:\n"
+ " - positive integer: rank of degree of freedom,\n"
+ " - boolean: whether to control the selected degree of "
+ "freedom.\n"
+ " \n"
+ " Note: rank should be more than 5 since posture is "
+ "independent\n"
+ " from freeflyer position.\n"
+ " \n";
addCommand("selectDof", new SelectDof(*this, docstring));
}
@@ -84,8 +86,9 @@ dg::Vector &FeaturePosture::computeError(dg::Vector &res, int t) {
}
dg::Matrix &FeaturePosture::computeJacobian(dg::Matrix &, int) {
- throw std::runtime_error("jacobian signal should be constant."
- " This function should never be called");
+ throw std::runtime_error(
+ "jacobian signal should be constant."
+ " This function should never be called");
}
dg::Vector &FeaturePosture::computeErrorDot(dg::Vector &res, int t) {
@@ -94,8 +97,7 @@ dg::Vector &FeaturePosture::computeErrorDot(dg::Vector &res, int t) {
res.resize(nbActiveDofs_);
std::size_t index = 0;
for (std::size_t i = 0; i < activeDofs_.size(); ++i) {
- if (activeDofs_[i])
- res(index++) = -postureDot(i);
+ if (activeDofs_[i]) res(index++) = -postureDot(i);
}
return res;
}
@@ -127,7 +129,7 @@ void FeaturePosture::selectDof(unsigned dofId, bool control) {
activeDofs_[dofId] = true;
nbActiveDofs_++;
}
- } else { // control = false
+ } else { // control = false
if (activeDofs_[dofId]) {
activeDofs_[dofId] = false;
nbActiveDofs_--;
@@ -148,5 +150,5 @@ void FeaturePosture::selectDof(unsigned dofId, bool control) {
}
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePosture, "FeaturePosture");
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
diff --git a/src/feature/feature-task.cpp b/src/feature/feature-task.cpp
index bb6b7290..2177dab2 100644
--- a/src/feature/feature-task.cpp
+++ b/src/feature/feature-task.cpp
@@ -13,6 +13,7 @@
/* --- SOT --- */
#include <dynamic-graph/pool.h>
+
#include <sot/core/debug.hh>
#include <sot/core/exception-feature.hh>
#include <sot/core/feature-task.hh>
@@ -40,7 +41,6 @@ FeatureTask::FeatureTask(const string &pointName) : FeatureGeneric(pointName) {}
void FeatureTask::display(std::ostream &os) const {
os << "Feature from task <" << getName();
- if (taskPtr)
- os << ": from task " << taskPtr->getName();
+ if (taskPtr) os << ": from task " << taskPtr->getName();
os << std::endl;
}
diff --git a/src/feature/feature-vector3.cpp b/src/feature/feature-vector3.cpp
index 9bfefa4c..9b7899d3 100644
--- a/src/feature/feature-vector3.cpp
+++ b/src/feature/feature-vector3.cpp
@@ -16,9 +16,8 @@
//#define VP_DEBUG_MODE 45
#include <sot/core/debug.hh>
#include <sot/core/exception-feature.hh>
-#include <sot/core/feature-vector3.hh>
-
#include <sot/core/factory.hh>
+#include <sot/core/feature-vector3.hh>
#include <sot/core/matrix-geometry.hh>
using namespace dynamicgraph::sot;
@@ -35,12 +34,12 @@ FeatureVector3::FeatureVector3(const string &pointName)
: FeatureAbstract(pointName),
vectorSIN(NULL,
"sotFeatureVector3(" + name + ")::input(vector3)::vector"),
- positionSIN(NULL, "sotFeaturePoint6d(" + name +
- ")::input(matrixHomo)::position"),
- articularJacobianSIN(NULL, "sotFeatureVector3(" + name +
- ")::input(matrix)::Jq"),
- positionRefSIN(NULL, "sotFeatureVector3(" + name +
- ")::input(vector)::positionRef") {
+ positionSIN(
+ NULL, "sotFeaturePoint6d(" + name + ")::input(matrixHomo)::position"),
+ articularJacobianSIN(
+ NULL, "sotFeatureVector3(" + name + ")::input(matrix)::Jq"),
+ positionRefSIN(
+ NULL, "sotFeatureVector3(" + name + ")::input(vector)::positionRef") {
jacobianSOUT.addDependency(positionSIN);
jacobianSOUT.addDependency(articularJacobianSIN);
diff --git a/src/feature/feature-visual-point.cpp b/src/feature/feature-visual-point.cpp
index c768f6c4..b2167272 100644
--- a/src/feature/feature-visual-point.cpp
+++ b/src/feature/feature-visual-point.cpp
@@ -27,11 +27,12 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureVisualPoint, "FeatureVisualPoint");
/* --------------------------------------------------------------------- */
FeatureVisualPoint::FeatureVisualPoint(const string &pointName)
- : FeatureAbstract(pointName), L(),
+ : FeatureAbstract(pointName),
+ L(),
xySIN(NULL, "sotFeatureVisualPoint(" + name + ")::input(vector)::xy"),
ZSIN(NULL, "sotFeatureVisualPoint(" + name + ")::input(double)::Z"),
- articularJacobianSIN(NULL, "sotFeatureVisualPoint(" + name +
- ")::input(matrix)::Jq") {
+ articularJacobianSIN(
+ NULL, "sotFeatureVisualPoint(" + name + ")::input(matrix)::Jq") {
ZSIN = 1.;
jacobianSOUT.addDependency(xySIN);
@@ -64,10 +65,8 @@ unsigned int &FeatureVisualPoint::getDimension(unsigned int &dim, int time) {
const Flags &fl = selectionSIN.access(time);
dim = 0;
- if (fl(0))
- dim++;
- if (fl(1))
- dim++;
+ if (fl(0)) dim++;
+ if (fl(1)) dim++;
sotDEBUG(25) << "# Out }" << endl;
return dim;
@@ -166,10 +165,8 @@ void FeatureVisualPoint::display(std::ostream &os) const {
try {
const Vector &xy = xySIN.accessCopy();
const Flags &fl = selectionSIN.accessCopy();
- if (fl(0))
- os << " x=" << xy(0);
- if (fl(1))
- os << " y=" << xy(1);
+ if (fl(0)) os << " x=" << xy(0);
+ if (fl(1)) os << " y=" << xy(1);
} catch (ExceptionAbstract e) {
os << " XY or select not set.";
}
diff --git a/src/feature/visual-point-projecter.cpp b/src/feature/visual-point-projecter.cpp
index 4a8e19c6..6f1c500f 100644
--- a/src/feature/visual-point-projecter.cpp
+++ b/src/feature/visual-point-projecter.cpp
@@ -4,6 +4,7 @@
*/
#include <dynamic-graph/factory.h>
+
#include <sot/core/debug.hh>
#include <sot/core/visual-point-projecter.hh>
@@ -44,9 +45,8 @@ VisualPointProjecter::VisualPointProjecter(const std::string &name)
/* --- SIGNALS ---------------------------------------------------------- */
/* --- SIGNALS ---------------------------------------------------------- */
-dynamicgraph::Vector &
-VisualPointProjecter::point3DgazeSOUT_function(dynamicgraph::Vector &p3g,
- int iter) {
+dynamicgraph::Vector &VisualPointProjecter::point3DgazeSOUT_function(
+ dynamicgraph::Vector &p3g, int iter) {
const dynamicgraph::Vector &p3 = m_point3DSIN(iter);
const MatrixHomogeneous &M = m_transfoSIN(iter);
MatrixHomogeneous Mi;
@@ -55,8 +55,8 @@ VisualPointProjecter::point3DgazeSOUT_function(dynamicgraph::Vector &p3g,
return p3g;
}
-dynamicgraph::Vector &
-VisualPointProjecter::point2DSOUT_function(dynamicgraph::Vector &p2, int iter) {
+dynamicgraph::Vector &VisualPointProjecter::point2DSOUT_function(
+ dynamicgraph::Vector &p2, int iter) {
sotDEBUGIN(15);
const dynamicgraph::Vector &p3 = m_point3DgazeSOUT(iter);
@@ -86,5 +86,5 @@ void VisualPointProjecter::display(std::ostream &os) const {
os << "VisualPointProjecter " << getName();
}
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
diff --git a/src/filters/causal-filter.cpp b/src/filters/causal-filter.cpp
index 4d0d2c4d..0ace0ad2 100644
--- a/src/filters/causal-filter.cpp
+++ b/src/filters/causal-filter.cpp
@@ -15,7 +15,6 @@
*/
#include <iostream>
-
#include <sot/core/causal-filter.hh>
using namespace dynamicgraph::sot;
@@ -38,12 +37,15 @@ CausalFilter::CausalFilter(const double ×tep, const int &xSize,
const Eigen::VectorXd &filter_numerator,
const Eigen::VectorXd &filter_denominator)
- : m_dt(timestep), m_x_size(xSize),
+ : m_dt(timestep),
+ m_x_size(xSize),
m_filter_order_m(filter_numerator.size()),
m_filter_order_n(filter_denominator.size()),
m_filter_numerator(filter_numerator),
- m_filter_denominator(filter_denominator), m_first_sample(true),
- m_pt_numerator(0), m_pt_denominator(0),
+ m_filter_denominator(filter_denominator),
+ m_first_sample(true),
+ m_pt_numerator(0),
+ m_pt_denominator(0),
m_input_buffer(Eigen::MatrixXd::Zero(xSize, filter_numerator.size())),
m_output_buffer(
Eigen::MatrixXd::Zero(xSize, filter_denominator.size() - 1)) {
@@ -56,8 +58,7 @@ void CausalFilter::get_x_dx_ddx(const Eigen::VectorXd &base_x,
Eigen::VectorXd &x_output_dx_ddx) {
// const dynamicgraph::Vector &base_x = m_xSIN(iter);
if (m_first_sample) {
- for (int i = 0; i < m_filter_order_m; i++)
- m_input_buffer.col(i) = base_x;
+ for (int i = 0; i < m_filter_order_m; i++) m_input_buffer.col(i) = base_x;
for (int i = 0; i < m_filter_order_n - 1; i++)
m_output_buffer.col(i) =
base_x * m_filter_numerator.sum() / m_filter_denominator.sum();
@@ -112,8 +113,7 @@ void CausalFilter::switch_filter(const Eigen::VectorXd &filter_numerator,
m_input_buffer.resize(Eigen::NoChange, filter_order_m);
m_output_buffer.resize(Eigen::NoChange, filter_order_n - 1);
- for (int i = 0; i < filter_order_m; i++)
- m_input_buffer.col(i) = current_x;
+ for (int i = 0; i < filter_order_m; i++) m_input_buffer.col(i) = current_x;
for (int i = 0; i < filter_order_n - 1; i++)
m_output_buffer.col(i) =
diff --git a/src/filters/filter-differentiator.cpp b/src/filters/filter-differentiator.cpp
index f30a7e3f..4769606b 100644
--- a/src/filters/filter-differentiator.cpp
+++ b/src/filters/filter-differentiator.cpp
@@ -16,16 +16,17 @@
#define LOGFILE "/tmp/fd_log.dat"
-#define LOG(x) \
- { \
- std::ofstream LogFile; \
- LogFile.open(LOGFILE, std::ofstream::app); \
- LogFile << x << std::endl; \
- LogFile.close(); \
+#define LOG(x) \
+ { \
+ std::ofstream LogFile; \
+ LogFile.open(LOGFILE, std::ofstream::app); \
+ LogFile << x << std::endl; \
+ LogFile.close(); \
}
#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>
+
#include <sot/core/debug.hh>
#include <sot/core/filter-differentiator.hh>
//#include <sot/torque_control/motor-model.hh>
@@ -55,7 +56,8 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FilterDifferentiator,
/* --- CONSTRUCTION ------------------------------------------------- */
/* --- CONSTRUCTION ------------------------------------------------- */
FilterDifferentiator::FilterDifferentiator(const std::string &name)
- : Entity(name), CONSTRUCT_SIGNAL_IN(x, dynamicgraph::Vector),
+ : Entity(name),
+ CONSTRUCT_SIGNAL_IN(x, dynamicgraph::Vector),
CONSTRUCT_SIGNAL_OUT(x_filtered, dynamicgraph::Vector, m_x_dx_ddxSINNER),
CONSTRUCT_SIGNAL_OUT(dx, dynamicgraph::Vector, m_x_dx_ddxSINNER),
CONSTRUCT_SIGNAL_OUT(ddx, dynamicgraph::Vector, m_x_dx_ddxSINNER),
@@ -78,10 +80,10 @@ FilterDifferentiator::FilterDifferentiator(const std::string &name)
"Numerator of the filter",
"Denominator of the filter")));
addCommand("switch_filter",
- makeCommandVoid2(*this, &FilterDifferentiator::switch_filter,
- docCommandVoid2("Switch Filter.",
- "Numerator of the filter",
- "Denominator of the filter")));
+ makeCommandVoid2(
+ *this, &FilterDifferentiator::switch_filter,
+ docCommandVoid2("Switch Filter.", "Numerator of the filter",
+ "Denominator of the filter")));
}
/* --- COMMANDS ------------------------------------------------------ */
@@ -117,8 +119,7 @@ void FilterDifferentiator::switch_filter(
DEFINE_SIGNAL_INNER_FUNCTION(x_dx_ddx, dynamicgraph::Vector) {
sotDEBUG(15) << "Compute x_dx inner signal " << iter << std::endl;
- if (s.size() != 3 * m_x_size)
- s.resize(3 * m_x_size);
+ if (s.size() != 3 * m_x_size) s.resize(3 * m_x_size);
// read encoders
const dynamicgraph::Vector &base_x = m_xSIN(iter);
assert(base_x.size() == m_x_size);
@@ -136,8 +137,7 @@ DEFINE_SIGNAL_OUT_FUNCTION(x_filtered, dynamicgraph::Vector) {
sotDEBUG(15) << "Compute x_filtered output signal " << iter << std::endl;
const dynamicgraph::Vector &x_dx_ddx = m_x_dx_ddxSINNER(iter);
- if (s.size() != m_x_size)
- s.resize(m_x_size);
+ if (s.size() != m_x_size) s.resize(m_x_size);
s = x_dx_ddx.head(m_x_size);
return s;
}
@@ -146,8 +146,7 @@ DEFINE_SIGNAL_OUT_FUNCTION(dx, dynamicgraph::Vector) {
sotDEBUG(15) << "Compute dx output signal " << iter << std::endl;
const dynamicgraph::Vector &x_dx_ddx = m_x_dx_ddxSINNER(iter);
- if (s.size() != m_x_size)
- s.resize(m_x_size);
+ if (s.size() != m_x_size) s.resize(m_x_size);
s = x_dx_ddx.segment(m_x_size, m_x_size);
return s;
}
@@ -156,8 +155,7 @@ DEFINE_SIGNAL_OUT_FUNCTION(ddx, dynamicgraph::Vector) {
sotDEBUG(15) << "Compute ddx output signal " << iter << std::endl;
const dynamicgraph::Vector &x_dx_ddx = m_x_dx_ddxSINNER(iter);
- if (s.size() != m_x_size)
- s.resize(m_x_size);
+ if (s.size() != m_x_size) s.resize(m_x_size);
s = x_dx_ddx.tail(m_x_size);
return s;
}
@@ -170,5 +168,5 @@ void FilterDifferentiator::display(std::ostream &os) const {
}
}
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
diff --git a/src/filters/madgwickahrs.cpp b/src/filters/madgwickahrs.cpp
index e21be756..baabe9cf 100644
--- a/src/filters/madgwickahrs.cpp
+++ b/src/filters/madgwickahrs.cpp
@@ -10,11 +10,11 @@
//
//=========================================================================
+#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>
+
#include <sot/core/debug.hh>
#include <sot/core/madgwickahrs.hh>
-
-#include <dynamic-graph/all-commands.h>
#include <sot/core/stop-watch.hh>
namespace dynamicgraph {
@@ -42,12 +42,18 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(MadgwickAHRS, "MadgwickAHRS");
/* --- CONSTRUCTION -------------------------------------------------- */
/* ------------------------------------------------------------------- */
MadgwickAHRS::MadgwickAHRS(const std::string &name)
- : Entity(name), CONSTRUCT_SIGNAL_IN(accelerometer, dynamicgraph::Vector),
+ : Entity(name),
+ CONSTRUCT_SIGNAL_IN(accelerometer, dynamicgraph::Vector),
CONSTRUCT_SIGNAL_IN(gyroscope, dynamicgraph::Vector),
CONSTRUCT_SIGNAL_OUT(imu_quat, dynamicgraph::Vector,
m_gyroscopeSIN << m_accelerometerSIN),
- m_initSucceeded(false), m_beta(betaDef), m_q0(1.0), m_q1(0.0), m_q2(0.0),
- m_q3(0.0), m_sampleFreq(512.0) {
+ m_initSucceeded(false),
+ m_beta(betaDef),
+ m_q0(1.0),
+ m_q1(0.0),
+ m_q2(0.0),
+ m_q3(0.0),
+ m_sampleFreq(512.0) {
Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS);
/* Commands. */
@@ -69,8 +75,7 @@ MadgwickAHRS::MadgwickAHRS(const std::string &name)
}
void MadgwickAHRS::init(const double &dt) {
- if (dt <= 0.0)
- return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
+ if (dt <= 0.0) return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
m_sampleFreq = 1.0 / dt;
m_initSucceeded = true;
}
@@ -107,8 +112,7 @@ DEFINE_SIGNAL_OUT_FUNCTION(imu_quat, dynamicgraph::Vector) {
// Update state with new measurment
madgwickAHRSupdateIMU(gyroscope(0), gyroscope(1), gyroscope(2),
accelerometer(0), accelerometer(1), accelerometer(2));
- if (s.size() != 4)
- s.resize(4);
+ if (s.size() != 4) s.resize(4);
s(0) = m_q0;
s(1) = m_q1;
s(2) = m_q2;
@@ -136,7 +140,7 @@ double MadgwickAHRS::invSqrt(double x) {
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
return y;*/
- return (1.0 / sqrt(x)); // we're not in the 70's
+ return (1.0 / sqrt(x)); // we're not in the 70's
}
// IMU algorithm update
@@ -226,5 +230,5 @@ void MadgwickAHRS::display(std::ostream &os) const {
} catch (ExceptionSignal e) {
}
}
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
diff --git a/src/math/op-point-modifier.cpp b/src/math/op-point-modifier.cpp
index b2e89140..f77af946 100644
--- a/src/math/op-point-modifier.cpp
+++ b/src/math/op-point-modifier.cpp
@@ -25,10 +25,11 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(OpPointModifier, "OpPointModifier");
/* --------------------------------------------------------------------- */
OpPointModifier::OpPointModifier(const std::string &name)
- : Entity(name), jacobianSIN(NULL, "OpPointModifior(" + name +
- ")::input(matrix)::jacobianIN"),
- positionSIN(NULL, "OpPointModifior(" + name +
- ")::input(matrixhomo)::positionIN"),
+ : Entity(name),
+ jacobianSIN(NULL,
+ "OpPointModifior(" + name + ")::input(matrix)::jacobianIN"),
+ positionSIN(
+ NULL, "OpPointModifior(" + name + ")::input(matrixhomo)::positionIN"),
jacobianSOUT(
boost::bind(&OpPointModifier::jacobianSOUT_function, this, _1, _2),
jacobianSIN,
@@ -37,13 +38,13 @@ OpPointModifier::OpPointModifier(const std::string &name)
boost::bind(&OpPointModifier::positionSOUT_function, this, _1, _2),
positionSIN,
"OpPointModifior(" + name + ")::output(matrixhomo)::position"),
- transformation(), isEndEffector(true) {
+ transformation(),
+ isEndEffector(true) {
sotDEBUGIN(15);
signalRegistration(jacobianSIN << positionSIN << jacobianSOUT
<< positionSOUT);
{
-
using namespace dynamicgraph::command;
addCommand(
"getTransformation",
@@ -64,16 +65,15 @@ OpPointModifier::OpPointModifier(const std::string &name)
sotDEBUGOUT(15);
}
-dynamicgraph::Matrix &
-OpPointModifier::jacobianSOUT_function(dynamicgraph::Matrix &res,
- const int &iter) {
+dynamicgraph::Matrix &OpPointModifier::jacobianSOUT_function(
+ dynamicgraph::Matrix &res, const int &iter) {
if (isEndEffector) {
const dynamicgraph::Matrix &aJa = jacobianSIN(iter);
const MatrixHomogeneous &aMb = transformation;
MatrixTwist bVa;
buildFrom(aMb.inverse(), bVa);
- res = bVa * aJa; // res := bJb
+ res = bVa * aJa; // res := bJb
return res;
} else {
/* Consider that the jacobian of point A in frame A is given: J = aJa
@@ -106,13 +106,12 @@ OpPointModifier::jacobianSOUT_function(dynamicgraph::Matrix &res,
res(i + 3, j) = oJa(i + 3, j);
}
}
- return res; // res := 0Jb
+ return res; // res := 0Jb
}
}
-MatrixHomogeneous &
-OpPointModifier::positionSOUT_function(MatrixHomogeneous &res,
- const int &iter) {
+MatrixHomogeneous &OpPointModifier::positionSOUT_function(
+ MatrixHomogeneous &res, const int &iter) {
sotDEBUGIN(15);
sotDEBUGIN(15) << iter << " " << positionSIN.getTime()
<< positionSOUT.getTime() << endl;
diff --git a/src/math/vector-quaternion.cpp b/src/math/vector-quaternion.cpp
index ead76d20..eb505fe1 100644
--- a/src/math/vector-quaternion.cpp
+++ b/src/math/vector-quaternion.cpp
@@ -96,7 +96,7 @@ MatrixRotation &VectorQuaternion::toMatrix(MatrixRotation &rot) const {
double z2 = _z * _z;
double r2 = _r * _r;
- rotmat(0, 0) = r2 + x2 - y2 - z2; // fill diagonal terms
+ rotmat(0, 0) = r2 + x2 - y2 - z2; // fill diagonal terms
rotmat(1, 1) = r2 - x2 + y2 - z2;
rotmat(2, 2) = r2 - x2 - y2 + z2;
@@ -107,7 +107,7 @@ MatrixRotation &VectorQuaternion::toMatrix(MatrixRotation &rot) const {
double ry = _r * _y;
double rz = _r * _z;
- rotmat(0, 1) = 2 * (xy - rz); // fill off diagonal terms
+ rotmat(0, 1) = 2 * (xy - rz); // fill off diagonal terms
rotmat(0, 2) = 2 * (zx + ry);
rotmat(1, 0) = 2 * (xy + rz);
rotmat(1, 2) = 2 * (yz - rx);
diff --git a/src/matrix/derivator.cpp b/src/matrix/derivator.cpp
index 3032b719..e592b738 100644
--- a/src/matrix/derivator.cpp
+++ b/src/matrix/derivator.cpp
@@ -33,22 +33,24 @@ using namespace dynamicgraph;
// ®FunctiondoubleDerivator );
// }
-#define SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(sotClassType, sotType, className) \
- template <> const double Derivator<sotType>::TIMESTEP_DEFAULT = 1.; \
- template <> std::string sotClassType<sotType>::getTypeName(void) { \
- return #sotType; \
- } \
- template <> \
- const std::string sotClassType<sotType>::CLASS_NAME = \
- std::string(className) + "_of_" + #sotType; \
- extern "C" { \
- Entity * \
- regFunction##_##sotType##_##sotClassType(const std::string &objname) { \
- return new sotClassType<sotType>(objname); \
- } \
- EntityRegisterer regObj##_##sotType##_##sotClassType( \
- std::string(className) + "_of_" + #sotType, \
- ®Function##_##sotType##_##sotClassType); \
+#define SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(sotClassType, sotType, className) \
+ template <> \
+ const double Derivator<sotType>::TIMESTEP_DEFAULT = 1.; \
+ template <> \
+ std::string sotClassType<sotType>::getTypeName(void) { \
+ return #sotType; \
+ } \
+ template <> \
+ const std::string sotClassType<sotType>::CLASS_NAME = \
+ std::string(className) + "_of_" + #sotType; \
+ extern "C" { \
+ Entity *regFunction##_##sotType##_##sotClassType( \
+ const std::string &objname) { \
+ return new sotClassType<sotType>(objname); \
+ } \
+ EntityRegisterer regObj##_##sotType##_##sotClassType( \
+ std::string(className) + "_of_" + #sotType, \
+ ®Function##_##sotType##_##sotClassType); \
}
namespace dynamicgraph {
@@ -57,15 +59,15 @@ SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(Derivator, double, "Derivator")
SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(Derivator, Vector, "Derivator")
SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(Derivator, Matrix, "Derivator")
SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(Derivator, VectorQuaternion, "Derivator")
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
#include <sot/core/derivator-impl.hh>
#ifdef WIN32
-#define SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_WIN32(sotClassType, sotType, \
- className) \
- sotClassType##sotType## ::sotClassType##sotType##(const std::string &name) \
+#define SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_WIN32(sotClassType, sotType, \
+ className) \
+ sotClassType##sotType## ::sotClassType##sotType##(const std::string &name) \
: sotClassType<sotType>(name){};
typedef double Double;
diff --git a/src/matrix/double-constant.cpp b/src/matrix/double-constant.cpp
index 48ad16ec..5ac7e345 100644
--- a/src/matrix/double-constant.cpp
+++ b/src/matrix/double-constant.cpp
@@ -6,9 +6,9 @@
*
*/
-#include <sot/core/double-constant.hh>
-
#include <dynamic-graph/command-setter.h>
+
+#include <sot/core/double-constant.hh>
#include <sot/core/factory.hh>
namespace dynamicgraph {
@@ -24,12 +24,13 @@ DoubleConstant::DoubleConstant(const std::string &name)
// Commands
// set
- std::string docstring = " \n"
- " Set value of output signal\n"
- " \n"
- " input:\n"
- " - a double\n"
- " \n";
+ std::string docstring =
+ " \n"
+ " Set value of output signal\n"
+ " \n"
+ " input:\n"
+ " - a double\n"
+ " \n";
addCommand("set", new ::dynamicgraph::command::Setter<DoubleConstant, double>(
*this, &DoubleConstant::setValue, docstring));
}
@@ -38,5 +39,5 @@ void DoubleConstant::setValue(const double &inValue) {
SOUT.setConstant(inValue);
}
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
diff --git a/src/matrix/fir-filter.cpp b/src/matrix/fir-filter.cpp
index adb2087c..358cf450 100644
--- a/src/matrix/fir-filter.cpp
+++ b/src/matrix/fir-filter.cpp
@@ -65,15 +65,15 @@ void FIRFilter<Vector, Matrix>::reset_signal(Vector &res,
res.fill(0);
}
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
#include <sot/core/fir-filter-impl.hh>
#ifdef WIN32
-#define DEFINE_SPECIFICATION(sotClassType, sotSigType, sotCoefType) \
- sotClassType##sotSigType##sotCoefType:: \
- sotClassType##sotSigType##sotCoefType(const std::string &name) \
+#define DEFINE_SPECIFICATION(sotClassType, sotSigType, sotCoefType) \
+ sotClassType##sotSigType##sotCoefType:: \
+ sotClassType##sotSigType##sotCoefType(const std::string &name) \
: sotClassType<sotSigType, sotCoefType>(name){};
namespace dynamicgraph {
@@ -83,6 +83,6 @@ typedef Value dynamicgraph::command::Value;
DEFINE_SPECIFICATION(FIRFilter, Double, Double)
DEFINE_SPECIFICATION(FIRFilter, Vector, Double)
DEFINE_SPECIFICATION(FIRFilter, Vector, Matrix)
-} // namespace sot
-} // namespace dynamicgraph
-#endif // WIN32
+} // namespace sot
+} // namespace dynamicgraph
+#endif // WIN32
diff --git a/src/matrix/integrator-euler-python-module-py.cc b/src/matrix/integrator-euler-python-module-py.cc
index 022483ea..2b44c62a 100644
--- a/src/matrix/integrator-euler-python-module-py.cc
+++ b/src/matrix/integrator-euler-python-module-py.cc
@@ -1,8 +1,6 @@
-#include <dynamic-graph/python/module.hh>
-
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include <dynamic-graph/python/dynamic-graph-py.hh>
-
+#include <dynamic-graph/python/module.hh>
#include <sot/core/integrator-euler.hh>
namespace dg = dynamicgraph;
@@ -11,28 +9,31 @@ namespace dgs = dynamicgraph::sot;
using dg::Matrix;
using dg::Vector;
-template <typename S, typename C> void exposeIntegratorEuler() {
+template <typename S, typename C>
+void exposeIntegratorEuler() {
typedef dgs::IntegratorEuler<S, C> IE_t;
const std::string cName = dgc::Value::typeName(dgc::ValueHelper<C>::TypeID);
dg::python::exposeEntity<IE_t>()
- .add_property("numerators",
- +[](const IE_t &e) {
- return dg::python::to_py_list(e.numCoeffs().begin(),
- e.numCoeffs().end());
- },
- +[](IE_t &e, bp::object iterable) {
- e.numCoeffs(dg::python::to_std_vector<C>(iterable));
- })
- .add_property("denominators",
- +[](const IE_t &e) {
- return dg::python::to_py_list(e.denomCoeffs().begin(),
- e.denomCoeffs().end());
- },
- +[](IE_t &e, bp::object iterable) {
- e.denomCoeffs(dg::python::to_std_vector<C>(iterable));
- });
+ .add_property(
+ "numerators",
+ +[](const IE_t &e) {
+ return dg::python::to_py_list(e.numCoeffs().begin(),
+ e.numCoeffs().end());
+ },
+ +[](IE_t &e, bp::object iterable) {
+ e.numCoeffs(dg::python::to_std_vector<C>(iterable));
+ })
+ .add_property(
+ "denominators",
+ +[](const IE_t &e) {
+ return dg::python::to_py_list(e.denomCoeffs().begin(),
+ e.denomCoeffs().end());
+ },
+ +[](IE_t &e, bp::object iterable) {
+ e.denomCoeffs(dg::python::to_std_vector<C>(iterable));
+ });
}
BOOST_PYTHON_MODULE(wrap) {
diff --git a/src/matrix/integrator-euler.cpp b/src/matrix/integrator-euler.cpp
index 03416ec0..e8693b94 100644
--- a/src/matrix/integrator-euler.cpp
+++ b/src/matrix/integrator-euler.cpp
@@ -7,12 +7,11 @@
*
*/
+#include <sot/core/integrator-euler-impl.hh>
#include <sot/core/integrator-euler.hh>
#include "integrator-euler.t.cpp"
-#include <sot/core/integrator-euler-impl.hh>
-
#ifdef WIN32
IntegratorEulerVectorMatrix::IntegratorEulerVectorMatrix(
const std::string &name)
@@ -27,4 +26,4 @@ IntegratorEulerVectorDouble::IntegratorEulerVectorDouble(
std::string IntegratorEulerVectorDouble::getTypeName(void) {
return "IntegratorEulerVectorDouble";
}
-#endif // WIN32
+#endif // WIN32
diff --git a/src/matrix/integrator-euler.t.cpp b/src/matrix/integrator-euler.t.cpp
index b8307fb2..acd53186 100644
--- a/src/matrix/integrator-euler.t.cpp
+++ b/src/matrix/integrator-euler.t.cpp
@@ -28,8 +28,8 @@ using namespace dynamicgraph;
return CLASS_NAME; \
} \
extern "C" { \
- Entity * \
- regFunction##_##sotSigType##_##sotCoefType(const std::string &objname) { \
+ Entity *regFunction##_##sotSigType##_##sotCoefType( \
+ const std::string &objname) { \
return new sotClassType<sotSigType, sotCoefType>(objname); \
} \
EntityRegisterer regObj##_##sotSigType##_##sotCoefType( \
@@ -50,5 +50,5 @@ SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_EULER(IntegratorEuler, Vector, double,
template class IntegratorEuler<double, double>;
template class IntegratorEuler<Vector, double>;
template class IntegratorEuler<Vector, Matrix>;
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
diff --git a/src/matrix/matrix-constant-command.h b/src/matrix/matrix-constant-command.h
index b6b64e34..f6cfc97d 100644
--- a/src/matrix/matrix-constant-command.h
+++ b/src/matrix/matrix-constant-command.h
@@ -9,12 +9,12 @@
#ifndef MATRIX_CONSTANT_COMMAND_H
#define MATRIX_CONSTANT_COMMAND_H
-#include <boost/assign/list_of.hpp>
-
#include <dynamic-graph/command-getter.h>
#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/command.h>
+#include <boost/assign/list_of.hpp>
+
namespace dynamicgraph {
namespace sot {
namespace command {
@@ -24,7 +24,7 @@ using ::dynamicgraph::command::Value;
// Command Resize
class Resize : public Command {
-public:
+ public:
virtual ~Resize() {}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
@@ -44,10 +44,10 @@ public:
// return void
return Value();
}
-}; // class Resize
-} // namespace matrixConstant
-} // namespace command
-} // namespace sot
-} // namespace dynamicgraph
+}; // class Resize
+} // namespace matrixConstant
+} // namespace command
+} // namespace sot
+} // namespace dynamicgraph
-#endif // MATRIX_CONSTANT_COMMAND_H
+#endif // MATRIX_CONSTANT_COMMAND_H
diff --git a/src/matrix/matrix-constant.cpp b/src/matrix/matrix-constant.cpp
index 0d9fb4d8..eb22fd73 100644
--- a/src/matrix/matrix-constant.cpp
+++ b/src/matrix/matrix-constant.cpp
@@ -23,7 +23,9 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(MatrixConstant, "MatrixConstant");
/* --------------------------------------------------------------------- */
MatrixConstant::MatrixConstant(const std::string &name)
- : Entity(name), rows(0), cols(0),
+ : Entity(name),
+ rows(0),
+ cols(0),
SOUT("sotMatrixConstant(" + name + ")::output(matrix)::sout") {
SOUT.setDependencyType(TimeDependency<int>::BOOL_DEPENDENT);
signalRegistration(SOUT);
@@ -32,20 +34,22 @@ MatrixConstant::MatrixConstant(const std::string &name)
// Resize
std::string docstring;
- docstring = " \n"
- " Resize the matrix and set it to zero.\n"
- " Input\n"
- " - unsigned int: number of lines.\n"
- " - unsigned int: number of columns.\n"
- "\n";
+ docstring =
+ " \n"
+ " Resize the matrix and set it to zero.\n"
+ " Input\n"
+ " - unsigned int: number of lines.\n"
+ " - unsigned int: number of columns.\n"
+ "\n";
addCommand("resize", new command::matrixConstant::Resize(*this, docstring));
// set
- docstring = " \n"
- " Set value of output signal\n"
- " \n"
- " input:\n"
- " - a matrix\n"
- " \n";
+ docstring =
+ " \n"
+ " Set value of output signal\n"
+ " \n"
+ " input:\n"
+ " - a matrix\n"
+ " \n";
addCommand(
"set",
new ::dynamicgraph::command::Setter<MatrixConstant, dynamicgraph::Matrix>(
diff --git a/src/matrix/matrix-svd.cpp b/src/matrix/matrix-svd.cpp
index 103bb099..95963aa0 100644
--- a/src/matrix/matrix-svd.cpp
+++ b/src/matrix/matrix-svd.cpp
@@ -55,4 +55,4 @@ void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix,
dampedInverse(svd, _inverseMatrix, threshold);
}
-} // namespace dynamicgraph
+} // namespace dynamicgraph
diff --git a/src/matrix/operator-python-module-py.cc b/src/matrix/operator-python-module-py.cc
index d9e3a753..75bb1a2b 100644
--- a/src/matrix/operator-python-module-py.cc
+++ b/src/matrix/operator-python-module-py.cc
@@ -1,5 +1,4 @@
#include "dynamic-graph/python/module.hh"
-
#include "operator.hh"
namespace dg = dynamicgraph;
@@ -9,7 +8,8 @@ namespace bp = boost::python;
typedef bp::return_value_policy<bp::reference_existing_object>
reference_existing_object;
-template <typename Operator> void exposeUnaryOp() {
+template <typename Operator>
+void exposeUnaryOp() {
typedef dgs::UnaryOp<Operator> O_t;
dg::python::exposeEntity<O_t, bp::bases<dg::Entity>,
dg::python::AddCommands>()
@@ -17,7 +17,8 @@ template <typename Operator> void exposeUnaryOp() {
.def_readonly("sout", &O_t::SOUT);
}
-template <typename Operator> void exposeBinaryOp() {
+template <typename Operator>
+void exposeBinaryOp() {
typedef dgs::BinaryOp<Operator> O_t;
dg::python::exposeEntity<O_t, bp::bases<dg::Entity>,
dg::python::AddCommands>()
@@ -26,7 +27,8 @@ template <typename Operator> void exposeBinaryOp() {
.def_readonly("sout", &O_t::SOUT);
}
-template <typename Operator> auto exposeVariadicOpBase() {
+template <typename Operator>
+auto exposeVariadicOpBase() {
typedef dgs::VariadicOp<Operator> O_t;
typedef typename O_t::Base B_t;
return dg::python::exposeEntity<O_t, bp::bases<dg::Entity>,
@@ -42,11 +44,13 @@ template <typename Operator> auto exposeVariadicOpBase() {
"get the number of input signal.", bp::arg("size"));
}
-template <typename Operator> struct exposeVariadicOpImpl {
+template <typename Operator>
+struct exposeVariadicOpImpl {
static void run() { exposeVariadicOpBase<Operator>(); }
};
-template <typename T> struct exposeVariadicOpImpl<dgs::AdderVariadic<T> > {
+template <typename T>
+struct exposeVariadicOpImpl<dgs::AdderVariadic<T> > {
static void run() {
typedef dgs::VariadicOp<dgs::AdderVariadic<T> > E_t;
exposeVariadicOpBase<dgs::AdderVariadic<T> >().add_property(
@@ -56,7 +60,8 @@ template <typename T> struct exposeVariadicOpImpl<dgs::AdderVariadic<T> > {
}
};
-template <typename Operator> void exposeVariadicOp() {
+template <typename Operator>
+void exposeVariadicOp() {
exposeVariadicOpImpl<Operator>::run();
}
diff --git a/src/matrix/operator.cpp b/src/matrix/operator.cpp
index 58f0bd52..f199f7a4 100644
--- a/src/matrix/operator.cpp
+++ b/src/matrix/operator.cpp
@@ -17,12 +17,12 @@ namespace dg = ::dynamicgraph;
/* ------- GENERIC HELPERS -------------------------------------------------- */
/* ---------------------------------------------------------------------------*/
-#define REGISTER_UNARY_OP(OpType, name) \
- template <> \
- const std::string UnaryOp<OpType>::CLASS_NAME = std::string(#name); \
- Entity *regFunction_##name(const std::string &objname) { \
- return new UnaryOp<OpType>(objname); \
- } \
+#define REGISTER_UNARY_OP(OpType, name) \
+ template <> \
+ const std::string UnaryOp<OpType>::CLASS_NAME = std::string(#name); \
+ Entity *regFunction_##name(const std::string &objname) { \
+ return new UnaryOp<OpType>(objname); \
+ } \
EntityRegisterer regObj_##name(std::string(#name), ®Function_##name)
/* ---------------------------------------------------------------------------*/
@@ -85,12 +85,12 @@ REGISTER_UNARY_OP(UThetaToQuaternion, UThetaToQuaternion);
/* ---------------------------------------------------------------------------*/
/* ---------------------------------------------------------------------------*/
-#define REGISTER_BINARY_OP(OpType, name) \
- template <> \
- const std::string BinaryOp<OpType>::CLASS_NAME = std::string(#name); \
- Entity *regFunction_##name(const std::string &objname) { \
- return new BinaryOp<OpType>(objname); \
- } \
+#define REGISTER_BINARY_OP(OpType, name) \
+ template <> \
+ const std::string BinaryOp<OpType>::CLASS_NAME = std::string(#name); \
+ Entity *regFunction_##name(const std::string &objname) { \
+ return new BinaryOp<OpType>(objname); \
+ } \
EntityRegisterer regObj_##name(std::string(#name), ®Function_##name)
/* --- MULTIPLICATION --------------------------------------------------- */
@@ -123,12 +123,12 @@ REGISTER_BINARY_OP(WeightedAdder<dynamicgraph::Matrix>, WeightAdd_of_matrix);
REGISTER_BINARY_OP(WeightedAdder<dynamicgraph::Vector>, WeightAdd_of_vector);
REGISTER_BINARY_OP(WeightedAdder<double>, WeightAdd_of_double);
-#define REGISTER_VARIADIC_OP(OpType, name) \
- template <> \
- const std::string VariadicOp<OpType>::CLASS_NAME = std::string(#name); \
- Entity *regFunction_##name(const std::string &objname) { \
- return new VariadicOp<OpType>(objname); \
- } \
+#define REGISTER_VARIADIC_OP(OpType, name) \
+ template <> \
+ const std::string VariadicOp<OpType>::CLASS_NAME = std::string(#name); \
+ Entity *regFunction_##name(const std::string &objname) { \
+ return new VariadicOp<OpType>(objname); \
+ } \
EntityRegisterer regObj_##name(std::string(#name), ®Function_##name)
/* --- VectorMix ------------------------------------------------------------ */
@@ -152,8 +152,8 @@ REGISTER_VARIADIC_OP(Multiplier<double>, Multiply_of_double);
REGISTER_VARIADIC_OP(BoolOp<0>, And);
REGISTER_VARIADIC_OP(BoolOp<1>, Or);
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
/* --- TODO ------------------------------------------------------------------*/
// The following commented lines are sot-v1 entities that are still waiting
diff --git a/src/matrix/operator.hh b/src/matrix/operator.hh
index 1b62bff6..bd11c8be 100644
--- a/src/matrix/operator.hh
+++ b/src/matrix/operator.hh
@@ -9,22 +9,19 @@
*
*/
-#include <boost/function.hpp>
-
-#include <sot/core/binary-op.hh>
-#include <sot/core/unary-op.hh>
-#include <sot/core/variadic-op.hh>
-
-#include <sot/core/matrix-geometry.hh>
-
#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>
+#include <dynamic-graph/linear-algebra.h>
+#include <boost/function.hpp>
#include <boost/numeric/conversion/cast.hpp>
#include <deque>
-#include <dynamic-graph/linear-algebra.h>
+#include <sot/core/binary-op.hh>
#include <sot/core/debug.hh>
#include <sot/core/factory.hh>
+#include <sot/core/matrix-geometry.hh>
+#include <sot/core/unary-op.hh>
+#include <sot/core/variadic-op.hh>
#include "../tools/type-name-helper.hh"
@@ -38,7 +35,8 @@ namespace dg = ::dynamicgraph;
namespace dynamicgraph {
namespace sot {
-template <typename TypeIn, typename TypeOut> struct UnaryOpHeader {
+template <typename TypeIn, typename TypeOut>
+struct UnaryOpHeader {
typedef TypeIn Tin;
typedef TypeOut Tout;
static inline std::string nameTypeIn(void) {
@@ -49,11 +47,13 @@ template <typename TypeIn, typename TypeOut> struct UnaryOpHeader {
}
inline void addSpecificCommands(Entity &, Entity::CommandMap_t &) {}
inline std::string getDocString() const {
- return std::string("Undocumented unary operator\n"
- " - input ") +
+ return std::string(
+ "Undocumented unary operator\n"
+ " - input ") +
nameTypeIn() +
- std::string("\n"
- " - output ") +
+ std::string(
+ "\n"
+ " - output ") +
nameTypeOut() + std::string("\n");
}
};
@@ -129,9 +129,10 @@ struct VectorComponent : public UnaryOpHeader<dg::Vector, double> {
ADD_COMMAND("setIndex", command::makeCommandVoid1(ent, callback, doc));
}
inline std::string getDocString() const {
- std::string docString("Select a component of a vector\n"
- " - input vector\n"
- " - output double");
+ std::string docString(
+ "Select a component of a vector\n"
+ " - input vector\n"
+ " - output double");
return docString;
}
};
@@ -143,11 +144,10 @@ struct MatrixSelector : public UnaryOpHeader<dg::Matrix, dg::Matrix> {
assert((jmin <= jmax) && (jmax <= m.cols()));
res.resize(imax - imin, jmax - jmin);
for (int i = imin; i < imax; ++i)
- for (int j = jmin; j < jmax; ++j)
- res(i - imin, j - jmin) = m(i, j);
+ for (int j = jmin; j < jmax; ++j) res(i - imin, j - jmin) = m(i, j);
}
-public:
+ public:
int imin, imax;
int jmin, jmax;
@@ -181,14 +181,13 @@ public:
/* ---------------------------------------------------------------------- */
struct MatrixColumnSelector : public UnaryOpHeader<dg::Matrix, dg::Vector> {
-public:
+ public:
inline void operator()(const Tin &m, Tout &res) const {
assert((imin <= imax) && (imax <= m.rows()));
assert(jcol < m.cols());
res.resize(imax - imin);
- for (int i = imin; i < imax; ++i)
- res(i - imin) = m(i, jcol);
+ for (int i = imin; i < imax; ++i) res(i - imin) = m(i, jcol);
}
int imin, imax;
@@ -228,7 +227,7 @@ struct Diagonalizer : public UnaryOpHeader<Vector, Matrix> {
res = r.asDiagonal();
}
-public:
+ public:
Diagonalizer(void) : nbr(0), nbc(0) {}
unsigned int nbr, nbc;
inline void resize(const int &r, const int &c) {
@@ -265,9 +264,10 @@ struct Normalize : public UnaryOpHeader<dg::Vector, double> {
}
inline std::string getDocString() const {
- std::string docString("Computes the norm of a vector\n"
- " - input vector\n"
- " - output double");
+ std::string docString(
+ "Computes the norm of a vector\n"
+ " - input vector\n"
+ " - output double");
return docString;
}
};
@@ -363,28 +363,23 @@ struct MatrixHomoToPoseRollPitchYaw
dg::Vector t(3);
t = M.translation();
res.resize(6);
- for (unsigned int i = 0; i < 3; ++i)
- res(i) = t(i);
- for (unsigned int i = 0; i < 3; ++i)
- res(i + 3) = r(i);
+ for (unsigned int i = 0; i < 3; ++i) res(i) = t(i);
+ for (unsigned int i = 0; i < 3; ++i) res(i + 3) = r(i);
}
};
struct PoseRollPitchYawToMatrixHomo
: public UnaryOpHeader<Vector, MatrixHomogeneous> {
inline void operator()(const dg::Vector &vect, MatrixHomogeneous &Mres) {
-
VectorRollPitchYaw r;
- for (unsigned int i = 0; i < 3; ++i)
- r(i) = vect(i + 3);
+ for (unsigned int i = 0; i < 3; ++i) r(i) = vect(i + 3);
MatrixRotation R = (Eigen::AngleAxisd(r(2), Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(r(1), Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(r(0), Eigen::Vector3d::UnitX()))
.toRotationMatrix();
dg::Vector t(3);
- for (unsigned int i = 0; i < 3; ++i)
- t(i) = vect(i);
+ for (unsigned int i = 0; i < 3; ++i) t(i) = vect(i);
// buildFrom(R,t);
Mres = Eigen::Translation3d(t) * R;
@@ -394,8 +389,7 @@ struct PoseRollPitchYawToMatrixHomo
struct PoseRollPitchYawToPoseUTheta : public UnaryOpHeader<Vector, Vector> {
inline void operator()(const dg::Vector &vect, dg::Vector &vectres) {
VectorRollPitchYaw r;
- for (unsigned int i = 0; i < 3; ++i)
- r(i) = vect(i + 3);
+ for (unsigned int i = 0; i < 3; ++i) r(i) = vect(i + 3);
MatrixRotation R = (Eigen::AngleAxisd(r(2), Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(r(1), Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(r(0), Eigen::Vector3d::UnitX()))
@@ -529,14 +523,17 @@ struct BinaryOpHeader {
}
inline void addSpecificCommands(Entity &, Entity::CommandMap_t &) {}
inline std::string getDocString() const {
- return std::string("Undocumented binary operator\n"
- " - input ") +
+ return std::string(
+ "Undocumented binary operator\n"
+ " - input ") +
nameTypeIn1() +
- std::string("\n"
- " - ") +
+ std::string(
+ "\n"
+ " - ") +
nameTypeIn2() +
- std::string("\n"
- " - output ") +
+ std::string(
+ "\n"
+ " - output ") +
nameTypeOut() + std::string("\n");
}
};
@@ -567,9 +564,9 @@ operator()(const dynamicgraph::sot::MatrixHomogeneous &f,
}
template <>
-inline void Multiplier_FxE__E<double, dynamicgraph::Vector>::
-operator()(const double &x, const dynamicgraph::Vector &v,
- dynamicgraph::Vector &res) const {
+inline void Multiplier_FxE__E<double, dynamicgraph::Vector>::operator()(
+ const double &x, const dynamicgraph::Vector &v,
+ dynamicgraph::Vector &res) const {
res = v;
res *= x;
}
@@ -584,7 +581,8 @@ typedef Multiplier_FxE__E<MatrixTwist, dynamicgraph::Vector>
Multiplier_matrixTwist_vector;
/* --- SUBSTRACTION ----------------------------------------------------- */
-template <typename T> struct Substraction : public BinaryOpHeader<T, T, T> {
+template <typename T>
+struct Substraction : public BinaryOpHeader<T, T, T> {
inline void operator()(const T &v1, const T &v2, T &r) const {
r = v1;
r -= v2;
@@ -595,7 +593,7 @@ template <typename T> struct Substraction : public BinaryOpHeader<T, T, T> {
struct VectorStack
: public BinaryOpHeader<dynamicgraph::Vector, dynamicgraph::Vector,
dynamicgraph::Vector> {
-public:
+ public:
int v1min, v1max;
int v2min, v2max;
inline void operator()(const dynamicgraph::Vector &v1,
@@ -670,8 +668,7 @@ struct ConvolutionTemporal
dynamicgraph::Vector &res) {
const Vector::Index nconv = (Vector::Index)f1.size(), nsig = f2.rows();
sotDEBUG(15) << "Size: " << nconv << "x" << nsig << std::endl;
- if (nconv > f2.cols())
- return; // TODO: error, this should not happen
+ if (nconv > f2.cols()) return; // TODO: error, this should not happen
res.resize(nsig);
res.fill(0);
@@ -680,8 +677,7 @@ struct ConvolutionTemporal
iter++) {
const dynamicgraph::Vector &s_tau = *iter;
sotDEBUG(45) << "Sig" << j << ": " << s_tau;
- if (s_tau.size() != nsig)
- return; // TODO: error throw;
+ if (s_tau.size() != nsig) return; // TODO: error throw;
for (int i = 0; i < nsig; ++i) {
res(i) += f2(i, j) * s_tau(i);
}
@@ -692,31 +688,35 @@ struct ConvolutionTemporal
const dynamicgraph::Matrix &m2,
dynamicgraph::Vector &res) {
memory.push_front(v1);
- while ((Vector::Index)memory.size() > m2.cols())
- memory.pop_back();
+ while ((Vector::Index)memory.size() > m2.cols()) memory.pop_back();
convolution(memory, m2, res);
}
};
/* --- BOOLEAN REDUCTION ------------------------------------------------ */
-template <typename T> struct Comparison : public BinaryOpHeader<T, T, bool> {
+template <typename T>
+struct Comparison : public BinaryOpHeader<T, T, bool> {
inline void operator()(const T &a, const T &b, bool &res) const {
res = (a < b);
}
inline std::string getDocString() const {
typedef BinaryOpHeader<T, T, bool> Base;
- return std::string("Comparison of inputs:\n"
- " - input ") +
+ return std::string(
+ "Comparison of inputs:\n"
+ " - input ") +
Base::nameTypeIn1() +
- std::string("\n"
- " - ") +
+ std::string(
+ "\n"
+ " - ") +
Base::nameTypeIn2() +
- std::string("\n"
- " - output ") +
+ std::string(
+ "\n"
+ " - output ") +
Base::nameTypeOut() +
- std::string("\n"
- " sout = ( sin1 < sin2 )\n");
+ std::string(
+ "\n"
+ " sout = ( sin1 < sin2 )\n");
}
};
@@ -735,20 +735,25 @@ struct MatrixComparison : public BinaryOpHeader<T1, T2, bool> {
}
inline std::string getDocString() const {
typedef BinaryOpHeader<T1, T2, bool> Base;
- return std::string("Comparison of inputs:\n"
- " - input ") +
+ return std::string(
+ "Comparison of inputs:\n"
+ " - input ") +
Base::nameTypeIn1() +
- std::string("\n"
- " - ") +
+ std::string(
+ "\n"
+ " - ") +
Base::nameTypeIn2() +
- std::string("\n"
- " - output ") +
+ std::string(
+ "\n"
+ " - output ") +
Base::nameTypeOut() +
- std::string("\n"
- " sout = ( sin1 < sin2 ).op()\n") +
- std::string("\n"
- " where op is either any (default) or all. The "
- "comparison can be made <=.\n");
+ std::string(
+ "\n"
+ " sout = ( sin1 < sin2 ).op()\n") +
+ std::string(
+ "\n"
+ " where op is either any (default) or all. The "
+ "comparison can be made <=.\n");
}
MatrixComparison() : any(true), equal(false) {}
inline void addSpecificCommands(Entity &ent,
@@ -774,8 +779,9 @@ struct MatrixComparison : public BinaryOpHeader<T1, T2, bool> {
namespace dynamicgraph {
namespace sot {
-template <typename T> struct WeightedAdder : public BinaryOpHeader<T, T, T> {
-public:
+template <typename T>
+struct WeightedAdder : public BinaryOpHeader<T, T, T> {
+ public:
double gain1, gain2;
inline void operator()(const T &v1, const T &v2, T &res) const {
res = v1;
@@ -807,8 +813,8 @@ public:
}
};
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
namespace dynamicgraph {
namespace sot {
@@ -821,7 +827,8 @@ std::string VariadicAbstract<Tin, Tout, Time>::getTypeOutName(void) {
return TypeNameHelper<Tout>::typeName();
}
-template <typename TypeIn, typename TypeOut> struct VariadicOpHeader {
+template <typename TypeIn, typename TypeOut>
+struct VariadicOpHeader {
typedef TypeIn Tin;
typedef TypeOut Tout;
inline static std::string nameTypeIn(void) {
@@ -834,18 +841,19 @@ template <typename TypeIn, typename TypeOut> struct VariadicOpHeader {
inline void initialize(VariadicOp<Op> *, Entity::CommandMap_t &) {}
inline void updateSignalNumber(const int &) {}
inline std::string getDocString() const {
- return std::string("Undocumented variadic operator\n"
- " - input " +
- nameTypeIn() +
- "\n"
- " - output " +
- nameTypeOut() + "\n");
+ return std::string(
+ "Undocumented variadic operator\n"
+ " - input " +
+ nameTypeIn() +
+ "\n"
+ " - output " +
+ nameTypeOut() + "\n");
}
};
/* --- VectorMix ------------------------------------------------------------ */
struct VectorMix : public VariadicOpHeader<Vector, Vector> {
-public:
+ public:
typedef VariadicOp<VectorMix> Base;
struct segment_t {
Vector::Index index, size, input;
@@ -890,7 +898,8 @@ public:
};
/* --- ADDITION --------------------------------------------------------- */
-template <typename T> struct AdderVariadic : public VariadicOpHeader<T, T> {
+template <typename T>
+struct AdderVariadic : public VariadicOpHeader<T, T> {
typedef VariadicOp<AdderVariadic> Base;
Base *entity;
@@ -899,11 +908,9 @@ template <typename T> struct AdderVariadic : public VariadicOpHeader<T, T> {
AdderVariadic() : coeffs() {}
inline void operator()(const std::vector<const T *> &vs, T &res) const {
assert(vs.size() == (std::size_t)coeffs.size());
- if (vs.size() == 0)
- return;
+ if (vs.size() == 0) return;
res = coeffs[0] * (*vs[0]);
- for (std::size_t i = 1; i < vs.size(); ++i)
- res += coeffs[i] * (*vs[i]);
+ for (std::size_t i = 1; i < vs.size(); ++i) res += coeffs[i] * (*vs[i]);
}
inline void setCoeffs(const Vector &c) {
@@ -932,7 +939,8 @@ template <typename T> struct AdderVariadic : public VariadicOpHeader<T, T> {
};
/* --- MULTIPLICATION --------------------------------------------------- */
-template <typename T> struct Multiplier : public VariadicOpHeader<T, T> {
+template <typename T>
+struct Multiplier : public VariadicOpHeader<T, T> {
typedef VariadicOp<Multiplier> Base;
inline void operator()(const std::vector<const T *> &vs, T &res) const {
@@ -940,8 +948,7 @@ template <typename T> struct Multiplier : public VariadicOpHeader<T, T> {
setIdentity(res);
else {
res = *vs[0];
- for (std::size_t i = 1; i < vs.size(); ++i)
- res *= *vs[i];
+ for (std::size_t i = 1; i < vs.size(); ++i) res *= *vs[i];
}
}
@@ -951,62 +958,58 @@ template <typename T> struct Multiplier : public VariadicOpHeader<T, T> {
ent->setSignalNumber(2);
}
};
-template <> inline void Multiplier<double>::setIdentity(double &res) const {
+template <>
+inline void Multiplier<double>::setIdentity(double &res) const {
res = 1;
}
template <>
-inline void Multiplier<MatrixHomogeneous>::
-operator()(const std::vector<const MatrixHomogeneous *> &vs,
- MatrixHomogeneous &res) const {
+inline void Multiplier<MatrixHomogeneous>::operator()(
+ const std::vector<const MatrixHomogeneous *> &vs,
+ MatrixHomogeneous &res) const {
if (vs.size() == 0)
setIdentity(res);
else {
res = *vs[0];
- for (std::size_t i = 1; i < vs.size(); ++i)
- res = res * *vs[i];
+ for (std::size_t i = 1; i < vs.size(); ++i) res = res * *vs[i];
}
}
template <>
-inline void Multiplier<Vector>::
-operator()(const std::vector<const Vector *> &vs, Vector &res) const {
+inline void Multiplier<Vector>::operator()(
+ const std::vector<const Vector *> &vs, Vector &res) const {
if (vs.size() == 0)
res.resize(0);
else {
res = *vs[0];
- for (std::size_t i = 1; i < vs.size(); ++i)
- res.array() *= vs[i]->array();
+ for (std::size_t i = 1; i < vs.size(); ++i) res.array() *= vs[i]->array();
}
}
/* --- BOOLEAN --------------------------------------------------------- */
-template <int operation> struct BoolOp : public VariadicOpHeader<bool, bool> {
+template <int operation>
+struct BoolOp : public VariadicOpHeader<bool, bool> {
typedef VariadicOp<BoolOp> Base;
inline void operator()(const std::vector<const bool *> &vs, bool &res) const {
// TODO computation could be optimized with lazy evaluation of the
// signals. When the output result is know, the remaining signals are
// not computed.
- if (vs.size() == 0)
- return;
+ if (vs.size() == 0) return;
res = *vs[0];
- for (std::size_t i = 1; i < vs.size(); ++i)
- switch (operation) {
- case 0:
- if (!res)
- return;
- res = *vs[i];
- break;
- case 1:
- if (res)
- return;
- res = *vs[i];
- break;
+ for (std::size_t i = 1; i < vs.size(); ++i) switch (operation) {
+ case 0:
+ if (!res) return;
+ res = *vs[i];
+ break;
+ case 1:
+ if (res) return;
+ res = *vs[i];
+ break;
}
}
};
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
/* --- TODO ------------------------------------------------------------------*/
// The following commented lines are sot-v1 entities that are still waiting
diff --git a/src/matrix/vector-constant-command.h b/src/matrix/vector-constant-command.h
index cc24ed68..80a1564b 100644
--- a/src/matrix/vector-constant-command.h
+++ b/src/matrix/vector-constant-command.h
@@ -9,12 +9,12 @@
#ifndef VECTOR_CONSTANT_COMMAND_H
#define VECTOR_CONSTANT_COMMAND_H
-#include <boost/assign/list_of.hpp>
-
#include <dynamic-graph/command-getter.h>
#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/command.h>
+#include <boost/assign/list_of.hpp>
+
namespace dynamicgraph {
namespace sot {
namespace command {
@@ -24,7 +24,7 @@ using ::dynamicgraph::command::Value;
// Command Resize
class Resize : public Command {
-public:
+ public:
virtual ~Resize() {}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
@@ -41,10 +41,10 @@ public:
// return void
return Value();
}
-}; // class Resize
-} // namespace vectorConstant
-} // namespace command
+}; // class Resize
+} // namespace vectorConstant
+} // namespace command
} /* namespace sot */
} /* namespace dynamicgraph */
-#endif // VECTOR_CONSTANT_COMMAND_H
+#endif // VECTOR_CONSTANT_COMMAND_H
diff --git a/src/matrix/vector-constant.cpp b/src/matrix/vector-constant.cpp
index 8b9eb231..4b19bd22 100644
--- a/src/matrix/vector-constant.cpp
+++ b/src/matrix/vector-constant.cpp
@@ -22,7 +22,8 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(VectorConstant, "VectorConstant");
/* --- VECTOR ---------------------------------------------------------- */
/* --------------------------------------------------------------------- */
VectorConstant::VectorConstant(const std::string &name)
- : Entity(name), rows(0),
+ : Entity(name),
+ rows(0),
SOUT("sotVectorConstant(" + name + ")::output(vector)::sout") {
SOUT.setDependencyType(TimeDependency<int>::BOOL_DEPENDENT);
signalRegistration(SOUT);
@@ -32,19 +33,21 @@ VectorConstant::VectorConstant(const std::string &name)
//
// Resize
std::string docstring;
- docstring = " \n"
- " Resize the vector and set it to zero.\n"
- " Input\n"
- " unsigned size.\n"
- "\n";
+ docstring =
+ " \n"
+ " Resize the vector and set it to zero.\n"
+ " Input\n"
+ " unsigned size.\n"
+ "\n";
addCommand("resize", new command::vectorConstant::Resize(*this, docstring));
// set
- docstring = " \n"
- " Set value of output signal\n"
- " \n"
- " input:\n"
- " - a vector\n"
- " \n";
+ docstring =
+ " \n"
+ " Set value of output signal\n"
+ " \n"
+ " input:\n"
+ " - a vector\n"
+ " \n";
addCommand(
"set",
new ::dynamicgraph::command::Setter<VectorConstant, dynamicgraph::Vector>(
diff --git a/src/matrix/vector-to-rotation.cpp b/src/matrix/vector-to-rotation.cpp
index 1d954075..50eb30cf 100644
--- a/src/matrix/vector-to-rotation.cpp
+++ b/src/matrix/vector-to-rotation.cpp
@@ -7,12 +7,11 @@
*
*/
-#include <sot/core/vector-to-rotation.hh>
-
#include <sot/core/debug.hh>
#include <sot/core/factory.hh>
#include <sot/core/macros-signal.hh>
#include <sot/core/macros.hh>
+#include <sot/core/vector-to-rotation.hh>
using namespace std;
using namespace dynamicgraph::sot;
@@ -28,21 +27,21 @@ SOT_CORE_DISABLE_WARNING_POP
/* --------------------------------------------------------------------- */
VectorToRotation::VectorToRotation(const std::string &name)
- : Entity(name), size(0), axes(0),
+ : Entity(name),
+ size(0),
+ axes(0),
SIN(NULL, "sotVectorToRotation(" + name + ")::output(vector)::sin"),
SOUT(SOT_MEMBER_SIGNAL_1(VectorToRotation::computeRotation, SIN,
dynamicgraph::Vector),
"sotVectorToRotation(" + name + ")::output(matrixRotation)::sout") {
-
signalRegistration(SIN << SOUT);
}
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-MatrixRotation &
-VectorToRotation::computeRotation(const dynamicgraph::Vector &angles,
- MatrixRotation &res) {
+MatrixRotation &VectorToRotation::computeRotation(
+ const dynamicgraph::Vector &angles, MatrixRotation &res) {
res.setIdentity();
MatrixRotation Ra, Rtmp;
for (unsigned int i = 0; i < size; ++i) {
@@ -51,27 +50,27 @@ VectorToRotation::computeRotation(const dynamicgraph::Vector &angles,
const double sa = sin(angles(i));
const unsigned int i_X = 0, i_Y = 1, i_Z = 2;
switch (axes[i]) {
- case AXIS_X: {
- Ra(i_Y, i_Y) = ca;
- Ra(i_Y, i_Z) = -sa;
- Ra(i_Z, i_Y) = sa;
- Ra(i_Z, i_Z) = ca;
- break;
- }
- case AXIS_Y: {
- Ra(i_Z, i_Z) = ca;
- Ra(i_Z, i_X) = -sa;
- Ra(i_X, i_Z) = sa;
- Ra(i_X, i_X) = ca;
- break;
- }
- case AXIS_Z: {
- Ra(i_X, i_X) = ca;
- Ra(i_X, i_Y) = -sa;
- Ra(i_Y, i_X) = sa;
- Ra(i_Y, i_Y) = ca;
- break;
- }
+ case AXIS_X: {
+ Ra(i_Y, i_Y) = ca;
+ Ra(i_Y, i_Z) = -sa;
+ Ra(i_Z, i_Y) = sa;
+ Ra(i_Z, i_Z) = ca;
+ break;
+ }
+ case AXIS_Y: {
+ Ra(i_Z, i_Z) = ca;
+ Ra(i_Z, i_X) = -sa;
+ Ra(i_X, i_Z) = sa;
+ Ra(i_X, i_X) = ca;
+ break;
+ }
+ case AXIS_Z: {
+ Ra(i_X, i_X) = ca;
+ Ra(i_X, i_Y) = -sa;
+ Ra(i_Y, i_X) = sa;
+ Ra(i_Y, i_Y) = ca;
+ break;
+ }
}
sotDEBUG(15) << "R" << i << " = " << Ra;
diff --git a/src/python-module.cc b/src/python-module.cc
index 099ab666..16fb1f17 100644
--- a/src/python-module.cc
+++ b/src/python-module.cc
@@ -1,9 +1,9 @@
-#include "dynamic-graph/python/module.hh"
-#include "dynamic-graph/python/signal.hh"
-
#include <sot/core/device.hh>
#include <sot/core/flags.hh>
+#include "dynamic-graph/python/module.hh"
+#include "dynamic-graph/python/signal.hh"
+
namespace dg = dynamicgraph;
namespace dgs = dynamicgraph::sot;
@@ -46,11 +46,12 @@ BOOST_PYTHON_MODULE(wrap) {
"Remove the signal to the refresh list", bp::arg("name"))
.def("clear", &PeriodicCall::clear,
"Clear all signals and commands from the refresh list.")
- .def("__str__", +[](const PeriodicCall &e) {
- std::ostringstream os;
- e.display(os);
- return os.str();
- });
+ .def(
+ "__str__", +[](const PeriodicCall &e) {
+ std::ostringstream os;
+ e.display(os);
+ return os.str();
+ });
dynamicgraph::python::exposeEntity<dgs::Device>()
.add_property("after", bp::make_function(&dgs::Device::periodicCallAfter,
@@ -87,16 +88,18 @@ BOOST_PYTHON_MODULE(wrap) {
.def("__bool__", &Flags::operator bool)
.def("reversed", &Flags::operator!)
- .def("set",
- +[](Flags &f, const std::string &s) {
- std::istringstream is(s);
- is >> f;
- })
- .def("__str__", +[](const Flags &f) {
- std::ostringstream os;
- os << f;
- return os.str();
- });
+ .def(
+ "set",
+ +[](Flags &f, const std::string &s) {
+ std::istringstream is(s);
+ is >> f;
+ })
+ .def(
+ "__str__", +[](const Flags &f) {
+ std::ostringstream os;
+ os << f;
+ return os.str();
+ });
dg::python::exposeSignalsOfType<Flags, int>("Flags");
}
diff --git a/src/signal/signal-cast.cpp b/src/signal/signal-cast.cpp
index c20bf06e..f9407b22 100644
--- a/src/signal/signal-cast.cpp
+++ b/src/signal/signal-cast.cpp
@@ -7,18 +7,17 @@
*
*/
-#include <Eigen/Core>
#include <dynamic-graph/eigen-io.h>
-#include <dynamic-graph/signal-caster.h>
-#include <iomanip>
-#include <sot/core/matrix-geometry.hh>
-#include <sot/core/pool.hh>
-
#include <dynamic-graph/signal-cast-helper.h>
#include <dynamic-graph/signal-caster.h>
+
+#include <Eigen/Core>
+#include <iomanip>
#include <sot/core/feature-abstract.hh>
#include <sot/core/flags.hh>
+#include <sot/core/matrix-geometry.hh>
#include <sot/core/multi-bound.hh>
+#include <sot/core/pool.hh>
#include <sot/core/trajectory.hh>
#ifdef WIN32
@@ -176,4 +175,4 @@ void SignalCast<VectorMultiBound>::trace(const VectorMultiBound &t,
DG_ADD_CASTER(sot::VectorMultiBound, sotVMB);
*/
-} // namespace dynamicgraph
+} // namespace dynamicgraph
diff --git a/src/sot/flags.cpp b/src/sot/flags.cpp
index 29a5642d..8744bb05 100644
--- a/src/sot/flags.cpp
+++ b/src/sot/flags.cpp
@@ -12,9 +12,10 @@
/* --------------------------------------------------------------------- */
/*! System framework */
-#include <list>
#include <stdlib.h>
+#include <list>
+
/*! Local Framework */
#include <sot/core/debug.hh>
#include <sot/core/flags.hh>
@@ -32,17 +33,17 @@ Flags::Flags(const char *_flags)
: flags(strlen(_flags)), outOfRangeFlag(false) {
for (unsigned int i = 0; i < flags.size(); ++i) {
switch (_flags[i]) {
- case '0':
- flags[i] = false;
- break;
- case '1':
- flags[i] = true;
- break;
- case ' ':
- break;
- default:
- throw std::invalid_argument("Could not parse input string " +
- std::string(_flags) + ". Expected 0 or 1.");
+ case '0':
+ flags[i] = false;
+ break;
+ case '1':
+ flags[i] = true;
+ break;
+ case ' ':
+ break;
+ default:
+ throw std::invalid_argument("Could not parse input string " +
+ std::string(_flags) + ". Expected 0 or 1.");
}
}
}
@@ -51,19 +52,16 @@ Flags::Flags(const std::vector<bool> &_flags)
: flags(_flags), outOfRangeFlag(false) {}
Flags::operator bool(void) const {
- if (outOfRangeFlag)
- return true;
+ if (outOfRangeFlag) return true;
for (unsigned int i = 0; i < flags.size(); ++i)
- if (flags[i])
- return true;
+ if (flags[i]) return true;
return false;
}
/* --------------------------------------------------------------------- */
bool Flags::operator()(const int &i) const {
- if (i < (int)flags.size())
- return flags[i];
+ if (i < (int)flags.size()) return flags[i];
return outOfRangeFlag;
}
@@ -72,13 +70,11 @@ void Flags::add(const bool &b) { flags.push_back(b); }
/* --------------------------------------------------------------------- */
void Flags::set(const unsigned int &idx) {
- if (idx < flags.size())
- flags[idx] = true;
+ if (idx < flags.size()) flags[idx] = true;
}
void Flags::unset(const unsigned int &idx) {
- if (idx < flags.size())
- flags[idx] = false;
+ if (idx < flags.size()) flags[idx] = false;
}
namespace dynamicgraph {
@@ -128,8 +124,7 @@ Flags &Flags::operator|=(const Flags &f2) {
/* --------------------------------------------------------------------- */
std::ostream &operator<<(std::ostream &os, const Flags &fl) {
- for (auto f : fl.flags)
- os << (f ? '1' : '0');
+ for (auto f : fl.flags) os << (f ? '1' : '0');
return os;
}
@@ -138,17 +133,17 @@ std::istream &operator>>(std::istream &is, Flags &fl) {
fl.flags.clear();
while (is.get(c).good()) {
switch (c) {
- case '0':
- fl.flags.push_back(false);
- break;
- case '1':
- fl.flags.push_back(true);
- break;
- case ' ':
- break;
- default:
- throw std::invalid_argument("Could not parse input character " +
- std::string(1, c) + ". Expected 0 or 1.");
+ case '0':
+ fl.flags.push_back(false);
+ break;
+ case '1':
+ fl.flags.push_back(true);
+ break;
+ case ' ':
+ break;
+ default:
+ throw std::invalid_argument("Could not parse input character " +
+ std::string(1, c) + ". Expected 0 or 1.");
}
}
return is;
diff --git a/src/sot/memory-task-sot.cpp b/src/sot/memory-task-sot.cpp
index 07110b44..135276a1 100644
--- a/src/sot/memory-task-sot.cpp
+++ b/src/sot/memory-task-sot.cpp
@@ -29,11 +29,10 @@ void MemoryTaskSOT::initMemory(const Matrix::Index nJ, const Matrix::Index mJ) {
svd = SVD_t(nJ, mJ, Eigen::ComputeThinU | Eigen::ComputeFullV);
// If the constraint is well conditioned, the kernel can be pre-allocated.
- if (mJ > nJ)
- kernelMem.resize(mJ - nJ, mJ);
+ if (mJ > nJ) kernelMem.resize(mJ - nJ, mJ);
JK.setZero();
Jt.setZero();
}
-void MemoryTaskSOT::display(std::ostream & /*os*/) const {} // TODO
+void MemoryTaskSOT::display(std::ostream& /*os*/) const {} // TODO
diff --git a/src/sot/sot-command.h b/src/sot/sot-command.h
index 2b390a1c..c07608d8 100644
--- a/src/sot/sot-command.h
+++ b/src/sot/sot-command.h
@@ -9,12 +9,12 @@
#ifndef SOT_COMMAND_H
#define SOT_COMMAND_H
-#include <boost/assign/list_of.hpp>
-
#include <dynamic-graph/command-getter.h>
#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/command.h>
+#include <boost/assign/list_of.hpp>
+
namespace dynamicgraph {
namespace sot {
namespace command {
@@ -24,7 +24,7 @@ using ::dynamicgraph::command::Value;
// Command Push
class Push : public Command {
-public:
+ public:
virtual ~Push() {}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
@@ -41,11 +41,11 @@ public:
// return void
return Value();
}
-}; // class Push
+}; // class Push
// Command Remove
class Remove : public Command {
-public:
+ public:
virtual ~Remove() {}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
@@ -62,11 +62,11 @@ public:
// return void
return Value();
}
-}; // class Remove
+}; // class Remove
// Command Up
class Up : public Command {
-public:
+ public:
virtual ~Up() {}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
@@ -83,11 +83,11 @@ public:
// return void
return Value();
}
-}; // class Remove
+}; // class Remove
// Command Down
class Down : public Command {
-public:
+ public:
virtual ~Down() {}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
@@ -104,11 +104,11 @@ public:
// return void
return Value();
}
-}; // class Down
+}; // class Down
// Command Display
class Display : public Command {
-public:
+ public:
virtual ~Display() {}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
@@ -123,11 +123,11 @@ public:
// return the stack
return Value(returnString.str());
}
-}; // class Display
+}; // class Display
// Command List
class List : public Command {
-public:
+ public:
virtual ~List() {}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
@@ -150,11 +150,11 @@ public:
// return the stack
return Value(returnString.str());
}
-}; // class List
+}; // class List
// Command Clear
class Clear : public Command {
-public:
+ public:
virtual ~Clear() {}
/// Clear the stack
/// \param docstring documentation of the command
@@ -167,11 +167,11 @@ public:
// return the stack
return Value();
}
-}; // class Clear
+}; // class Clear
-} // namespace classSot
-} // namespace command
+} // namespace classSot
+} // namespace command
} /* namespace sot */
} /* namespace dynamicgraph */
-#endif // SOT_COMMAND_H
+#endif // SOT_COMMAND_H
diff --git a/src/sot/sot.cpp b/src/sot/sot.cpp
index 01a5c8a4..88ee365d 100644
--- a/src/sot/sot.cpp
+++ b/src/sot/sot.cpp
@@ -18,22 +18,22 @@
/* SOT */
#ifdef VP_DEBUG
class sotSOT__INIT {
-public:
+ public:
sotSOT__INIT(void) { dynamicgraph::sot::DebugTrace::openFile(); }
};
sotSOT__INIT sotSOT_initiator;
-#endif //#ifdef VP_DEBUG
-
-#include <sot/core/sot.hh>
+#endif //#ifdef VP_DEBUG
#include <dynamic-graph/command-direct-getter.h>
#include <dynamic-graph/command-direct-setter.h>
+
#include <sot/core/factory.hh>
#include <sot/core/feature-posture.hh>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/matrix-svd.hh>
#include <sot/core/memory-task-sot.hh>
#include <sot/core/pool.hh>
+#include <sot/core/sot.hh>
#include <sot/core/task.hh>
using namespace std;
@@ -50,17 +50,20 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Sot, "SOT");
const double Sot::INVERSION_THRESHOLD_DEFAULT = 1e-4;
const Eigen::IOFormat python(Eigen::FullPrecision, 0,
- ", ", // coeff sep
- ",\n", // row sep
- "[", "]", // row prefix and suffix
- "[", "]" // mat prefix and suffix
+ ", ", // coeff sep
+ ",\n", // row sep
+ "[", "]", // row prefix and suffix
+ "[", "]" // mat prefix and suffix
);
/* --------------------------------------------------------------------- */
/* --- CONSTRUCTION ---------------------------------------------------- */
/* --------------------------------------------------------------------- */
Sot::Sot(const std::string &name)
- : Entity(name), stack(), nbJoints(0), enablePostureTaskAcceleration(false),
+ : Entity(name),
+ stack(),
+ nbJoints(0),
+ enablePostureTaskAcceleration(false),
maxControlIncrementSquaredNorm(std::numeric_limits<double>::max()),
q0SIN(NULL, "sotSOT(" + name + ")::input(double)::q0"),
proj0SIN(NULL, "sotSOT(" + name + ")::input(double)::proj0"),
@@ -77,23 +80,25 @@ Sot::Sot(const std::string &name)
//
std::string docstring;
- docstring = " \n"
- " setNumberDofs.\n"
- " \n"
- " Input:\n"
- " - a positive integer : number of degrees of freedom of "
- "the robot.\n"
- " \n";
+ docstring =
+ " \n"
+ " setNumberDofs.\n"
+ " \n"
+ " Input:\n"
+ " - a positive integer : number of degrees of freedom of "
+ "the robot.\n"
+ " \n";
addCommand("setSize", new dynamicgraph::command::Setter<Sot, unsigned int>(
*this, &Sot::defineNbDof, docstring));
- docstring = " \n"
- " getNumberDofs.\n"
- " \n"
- " Output:\n"
- " - a positive integer : number of degrees of freedom of "
- "the robot.\n"
- " \n";
+ docstring =
+ " \n"
+ " getNumberDofs.\n"
+ " \n"
+ " Output:\n"
+ " - a positive integer : number of degrees of freedom of "
+ "the robot.\n"
+ " \n";
addCommand("getSize",
new dynamicgraph::command::Getter<Sot, const unsigned int &>(
*this, &Sot::getNbDof, docstring));
@@ -116,15 +121,16 @@ Sot::Sot(const std::string &name)
"level",
"boolean")));
- docstring = " \n"
- " Maximum allowed squared norm of control increment.\n"
- " A task whose control increment is above this value is\n"
- " discarded. It defaults to the maximum double value.\n"
- " \n"
- " WARNING: This is a security feature and is **not** a good\n"
- " way of adding a proper constraint on the control\n"
- " generated by SoT.\n"
- " \n";
+ docstring =
+ " \n"
+ " Maximum allowed squared norm of control increment.\n"
+ " A task whose control increment is above this value is\n"
+ " discarded. It defaults to the maximum double value.\n"
+ " \n"
+ " WARNING: This is a security feature and is **not** a good\n"
+ " way of adding a proper constraint on the control\n"
+ " generated by SoT.\n"
+ " \n";
addCommand("setMaxControlIncrementSquaredNorm",
dynamicgraph::command::makeDirectSetter(
@@ -140,54 +146,61 @@ Sot::Sot(const std::string &name)
" - a double\n"
" \n"));
- docstring = " \n"
- " push a task into the stack.\n"
- " \n"
- " Input:\n"
- " - a string : Name of the task.\n"
- " \n";
+ docstring =
+ " \n"
+ " push a task into the stack.\n"
+ " \n"
+ " Input:\n"
+ " - a string : Name of the task.\n"
+ " \n";
addCommand("push", new command::classSot::Push(*this, docstring));
- docstring = " \n"
- " remove a task into the stack.\n"
- " \n"
- " Input:\n"
- " - a string : Name of the task.\n"
- " \n";
+ docstring =
+ " \n"
+ " remove a task into the stack.\n"
+ " \n"
+ " Input:\n"
+ " - a string : Name of the task.\n"
+ " \n";
addCommand("remove", new command::classSot::Remove(*this, docstring));
- docstring = " \n"
- " up a task into the stack.\n"
- " \n"
- " Input:\n"
- " - a string : Name of the task.\n"
- " \n";
+ docstring =
+ " \n"
+ " up a task into the stack.\n"
+ " \n"
+ " Input:\n"
+ " - a string : Name of the task.\n"
+ " \n";
addCommand("up", new command::classSot::Up(*this, docstring));
- docstring = " \n"
- " down a task into the stack.\n"
- " \n"
- " Input:\n"
- " - a string : Name of the task.\n"
- " \n";
+ docstring =
+ " \n"
+ " down a task into the stack.\n"
+ " \n"
+ " Input:\n"
+ " - a string : Name of the task.\n"
+ " \n";
addCommand("down", new command::classSot::Down(*this, docstring));
// Display
- docstring = " \n"
- " display the list of tasks pushed inside the stack.\n"
- " \n";
+ docstring =
+ " \n"
+ " display the list of tasks pushed inside the stack.\n"
+ " \n";
addCommand("display", new command::classSot::Display(*this, docstring));
// Clear
- docstring = " \n"
- " clear the list of tasks pushed inside the stack.\n"
- " \n";
+ docstring =
+ " \n"
+ " clear the list of tasks pushed inside the stack.\n"
+ " \n";
addCommand("clear", new command::classSot::Clear(*this, docstring));
// List
- docstring = " \n"
- " returns the list of tasks pushed inside the stack.\n"
- " \n";
+ docstring =
+ " \n"
+ " returns the list of tasks pushed inside the stack.\n"
+ " \n";
addCommand("list", new command::classSot::List(*this, docstring));
}
@@ -321,8 +334,7 @@ const Matrix &computeJacobianActivated(TaskAbstract *Ta, Task *T, Matrix &Jmem,
if (!controlSelec) {
Jmem = Ta->jacobianSOUT.accessCopy();
for (int i = 0; i < Jmem.cols(); ++i)
- if (!controlSelec(i))
- Jmem.col(i).setZero();
+ if (!controlSelec(i)) Jmem.col(i).setZero();
return Jmem;
} else
return Ta->jacobianSOUT.accessCopy();
@@ -367,8 +379,7 @@ bool updateControl(MemoryTaskSOT *mem, const Matrix::Index rankJ,
tmpControl.noalias() = kernel * tmpVar.head(kernel.cols());
} else
tmpControl.noalias() = svd.matrixV().leftCols(rankJ) * tmpTask.head(rankJ);
- if (tmpControl.squaredNorm() > threshold)
- return false;
+ if (tmpControl.squaredNorm() > threshold) return false;
control += tmpControl;
return true;
}
@@ -389,8 +400,7 @@ MemoryTaskSOT *getMemory(TaskAbstract &t, const Matrix::Index &tDim,
const Matrix::Index &nDof) {
MemoryTaskSOT *mem = dynamic_cast<MemoryTaskSOT *>(t.memoryInternal);
if (NULL == mem) {
- if (NULL != t.memoryInternal)
- delete t.memoryInternal;
+ if (NULL != t.memoryInternal) delete t.memoryInternal;
mem = new MemoryTaskSOT(tDim, nDof);
t.memoryInternal = mem;
}
@@ -413,28 +423,28 @@ MemoryTaskSOT *getMemory(TaskAbstract &t, const Matrix::Index &tDim,
#ifdef WITH_CHRONO
#define TIME_STREAM DYNAMIC_GRAPH_ENTITY_DEBUG(*this)
-#define sotINIT_CHRONO1 \
- struct timespec t0, t1; \
+#define sotINIT_CHRONO1 \
+ struct timespec t0, t1; \
double dt
#define sotSTART_CHRONO1 clock_gettime(CLOCK_THREAD_CPUTIME_ID, &t0)
-#define sotCHRONO1 \
- clock_gettime(CLOCK_THREAD_CPUTIME_ID, &t1); \
- dt = ((double)(t1.tv_sec - t0.tv_sec) * 1e6 + \
- (double)(t1.tv_nsec - t0.tv_nsec) / 1e3); \
+#define sotCHRONO1 \
+ clock_gettime(CLOCK_THREAD_CPUTIME_ID, &t1); \
+ dt = ((double)(t1.tv_sec - t0.tv_sec) * 1e6 + \
+ (double)(t1.tv_nsec - t0.tv_nsec) / 1e3); \
TIME_STREAM << "dT " << (long int)dt << std::endl
#define sotINITPARTCOUNTERS struct timespec tpart0
#define sotSTARTPARTCOUNTERS clock_gettime(CLOCK_THREAD_CPUTIME_ID, &tpart0)
-#define sotCOUNTER(nbc1, nbc2) \
- clock_gettime(CLOCK_THREAD_CPUTIME_ID, &tpart##nbc2); \
- dt##nbc2 = ((double)(tpart##nbc2.tv_sec - tpart##nbc1.tv_sec) * 1e6 + \
+#define sotCOUNTER(nbc1, nbc2) \
+ clock_gettime(CLOCK_THREAD_CPUTIME_ID, &tpart##nbc2); \
+ dt##nbc2 = ((double)(tpart##nbc2.tv_sec - tpart##nbc1.tv_sec) * 1e6 + \
(double)(tpart##nbc2.tv_nsec - tpart##nbc1.tv_nsec) / 1e3)
-#define sotINITCOUNTER(nbc1) \
- struct timespec tpart##nbc1; \
+#define sotINITCOUNTER(nbc1) \
+ struct timespec tpart##nbc1; \
double dt##nbc1 = 0;
-#define sotPRINTCOUNTER(nbc1) \
- TIME_STREAM << "dt" << iterTask << '_' << nbc1 << ' ' << (long int)dt##nbc1 \
+#define sotPRINTCOUNTER(nbc1) \
+ TIME_STREAM << "dt" << iterTask << '_' << nbc1 << ' ' << (long int)dt##nbc1 \
<< ' '
-#else // #ifdef WITH_CHRONO
+#else // #ifdef WITH_CHRONO
#define sotINIT_CHRONO1
#define sotSTART_CHRONO1
#define sotCHRONO1
@@ -443,7 +453,7 @@ MemoryTaskSOT *getMemory(TaskAbstract &t, const Matrix::Index &tDim,
#define sotCOUNTER(nbc1, nbc2)
#define sotINITCOUNTER(nbc1)
#define sotPRINTCOUNTER(nbc1)
-#endif // #ifdef WITH_CHRONO
+#endif // #ifdef WITH_CHRONO
void Sot::taskVectorToMlVector(const VectorMultiBound &taskVector,
Vector &res) {
@@ -524,30 +534,28 @@ dynamicgraph::Vector &Sot::computeControlLaw(dynamicgraph::Vector &control,
sotDEBUG(15) << "Task: e_" << taskA.getName() << std::endl;
/// Computing first the jacobian may be a little faster overall.
- if (!fullPostureTask)
- taskA.jacobianSOUT.recompute(iterTime);
+ if (!fullPostureTask) taskA.jacobianSOUT.recompute(iterTime);
taskA.taskSOUT.recompute(iterTime);
const Matrix::Index dim = taskA.taskSOUT.accessCopy().size();
- sotCOUNTER(0, 1); // Direct Dynamic
+ sotCOUNTER(0, 1); // Direct Dynamic
/* Init memory. */
MemoryTaskSOT *mem = getMemory(taskA, dim, nbJoints);
- /***/ sotCOUNTER(1, 2); // first allocs
+ /***/ sotCOUNTER(1, 2); // first allocs
Matrix::Index rankJ = -1;
taskVectorToMlVector(taskA.taskSOUT(iterTime), mem->err);
if (fullPostureTask) {
- /***/ sotCOUNTER(2, 3); // compute JK*S
- /***/ sotCOUNTER(3, 4); // compute Jt
+ /***/ sotCOUNTER(2, 3); // compute JK*S
+ /***/ sotCOUNTER(3, 4); // compute Jt
// Jp = kernel.transpose()
rankJ = kernel.cols();
- /***/ sotCOUNTER(4, 5); // SVD and rank
+ /***/ sotCOUNTER(4, 5); // SVD and rank
/* --- COMPUTE QDOT AND P --- */
- if (!controlIsZero)
- mem->err.noalias() -= control.tail(nbJoints - 6);
+ if (!controlIsZero) mem->err.noalias() -= control.tail(nbJoints - 6);
mem->tmpVar.head(rankJ).noalias() =
kernel.transpose().rightCols(nbJoints - 6) * mem->err;
control.noalias() += kernel * mem->tmpVar.head(rankJ);
@@ -558,7 +566,7 @@ dynamicgraph::Vector &Sot::computeControlLaw(dynamicgraph::Vector &control,
/* --- COMPUTE S * JK --- */
const Matrix &JK =
computeJacobianActivated(&taskA, task, mem->JK, iterTime);
- /***/ sotCOUNTER(2, 3); // compute JK*S
+ /***/ sotCOUNTER(2, 3); // compute JK*S
/* --- COMPUTE Jt --- */
const Matrix *Jt = &mem->Jt;
@@ -566,7 +574,7 @@ dynamicgraph::Vector &Sot::computeControlLaw(dynamicgraph::Vector &control,
mem->Jt.noalias() = JK * kernel;
else
Jt = &JK;
- /***/ sotCOUNTER(3, 4); // compute Jt
+ /***/ sotCOUNTER(3, 4); // compute Jt
/* --- SVD and RANK--- */
SVD_t &svd = mem->svd;
@@ -578,11 +586,10 @@ dynamicgraph::Vector &Sot::computeControlLaw(dynamicgraph::Vector &control,
while (rankJ < svd.singularValues().size() &&
th < svd.singularValues()[rankJ])
++rankJ;
- /***/ sotCOUNTER(4, 5); // SVD and rank
+ /***/ sotCOUNTER(4, 5); // SVD and rank
/* --- COMPUTE QDOT AND P --- */
- if (!controlIsZero)
- mem->err.noalias() -= JK * control;
+ if (!controlIsZero) mem->err.noalias() -= JK * control;
bool success = updateControl(mem, rankJ, has_kernel, kernel, control,
maxControlIncrementSquaredNorm);
@@ -613,7 +620,7 @@ dynamicgraph::Vector &Sot::computeControlLaw(dynamicgraph::Vector &control,
<< mem->tmpControl.transpose().format(python) << '\n';
}
}
- /***/ sotCOUNTER(5, 6); // QDOT + Projector
+ /***/ sotCOUNTER(5, 6); // QDOT + Projector
sotDEBUG(2) << "Proj non optimal (rankJ= " << rankJ
<< ", iterTask =" << iterTask << ")";
@@ -626,8 +633,7 @@ dynamicgraph::Vector &Sot::computeControlLaw(dynamicgraph::Vector &control,
sotPRINTCOUNTER(4);
sotPRINTCOUNTER(5);
sotPRINTCOUNTER(6);
- if (last || kernel.cols() == 0)
- break;
+ if (last || kernel.cols() == 0) break;
}
sotCHRONO1;
@@ -640,7 +646,6 @@ dynamicgraph::Vector &Sot::computeControlLaw(dynamicgraph::Vector &control,
/* --- DISPLAY --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
void Sot::display(std::ostream &os) const {
-
os << "+-----------------" << std::endl
<< "+ SOT " << std::endl
<< "+-----------------" << std::endl;
diff --git a/src/task/gain-adaptive.cpp b/src/task/gain-adaptive.cpp
index 0fab95c7..1acfce74 100644
--- a/src/task/gain-adaptive.cpp
+++ b/src/task/gain-adaptive.cpp
@@ -13,14 +13,12 @@
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
+#include <dynamic-graph/command-bind.h>
+
#include <sot/core/debug.hh>
#include <sot/core/exception-signal.hh>
#include <sot/core/factory.hh>
-#include <sot/core/debug.hh>
-
-#include <dynamic-graph/command-bind.h>
-
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
@@ -34,11 +32,11 @@ const double GainAdaptive::TAN_DEFAULT = 1;
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#define __SOT_GAIN_ADAPTATIVE_INIT \
- Entity(name), coeff_a(0), coeff_b(0), coeff_c(0), \
- errorSIN(NULL, "sotGainAdaptive(" + name + ")::input(vector)::error"), \
- gainSOUT(boost::bind(&GainAdaptive::computeGain, this, _1, _2), \
- errorSIN, \
+#define __SOT_GAIN_ADAPTATIVE_INIT \
+ Entity(name), coeff_a(0), coeff_b(0), coeff_c(0), \
+ errorSIN(NULL, "sotGainAdaptive(" + name + ")::input(vector)::error"), \
+ gainSOUT(boost::bind(&GainAdaptive::computeGain, this, _1, _2), \
+ errorSIN, \
"sotGainAdaptive(" + name + ")::output(double)::gain")
void GainAdaptive::addCommands() {
@@ -56,22 +54,24 @@ void GainAdaptive::addCommands() {
makeCommandVoid1(*this, &GainAdaptive::init, docstring));
// Command Set
- docstring = " \n"
- " set\n"
- " Input:\n"
- " floating point value: value at 0,\n"
- " floating point value: value at infinity,\n"
- " floating point value: value at slope,\n"
- " \n";
+ docstring =
+ " \n"
+ " set\n"
+ " Input:\n"
+ " floating point value: value at 0,\n"
+ " floating point value: value at infinity,\n"
+ " floating point value: value at slope,\n"
+ " \n";
addCommand("set", makeCommandVoid3(*this, &GainAdaptive::init, docstring));
- docstring = " \n"
- " set from value at 0 and infinity, with a passing point\n"
- " Input:\n"
- " floating point value: value at 0,\n"
- " floating point value: value at infinity,\n"
- " floating point value: reference point,\n"
- " floating point value: percentage at ref point.\n"
- " \n";
+ docstring =
+ " \n"
+ " set from value at 0 and infinity, with a passing point\n"
+ " Input:\n"
+ " floating point value: value at 0,\n"
+ " floating point value: value at infinity,\n"
+ " floating point value: reference point,\n"
+ " floating point value: percentage at ref point.\n"
+ " \n";
addCommand(
"setByPoint",
makeCommandVoid4(*this, &GainAdaptive::initFromPassingPoint, docstring));
@@ -133,7 +133,7 @@ void GainAdaptive::init(const double &valueAt0, const double &valueAtInfty,
void GainAdaptive::initFromPassingPoint(const double &valueAt0,
const double &valueAtInfty,
const double &xref,
- const double &p) // gref )
+ const double &p) // gref )
{
coeff_c = valueAtInfty;
coeff_a = valueAt0 - valueAtInfty;
diff --git a/src/task/multi-bound.cpp b/src/task/multi-bound.cpp
index 2590b4e8..650d1324 100644
--- a/src/task/multi-bound.cpp
+++ b/src/task/multi-bound.cpp
@@ -14,22 +14,36 @@
using namespace dynamicgraph::sot;
MultiBound::MultiBound(const double x)
- : mode(MODE_SINGLE), boundSingle(x), boundSup(0), boundInf(0),
- boundSupSetup(false), boundInfSetup(false) {}
+ : mode(MODE_SINGLE),
+ boundSingle(x),
+ boundSup(0),
+ boundInf(0),
+ boundSupSetup(false),
+ boundInfSetup(false) {}
MultiBound::MultiBound(const double xi, const double xs)
- : mode(MODE_DOUBLE), boundSingle(0), boundSup(xs), boundInf(xi),
- boundSupSetup(true), boundInfSetup(true) {}
+ : mode(MODE_DOUBLE),
+ boundSingle(0),
+ boundSup(xs),
+ boundInf(xi),
+ boundSupSetup(true),
+ boundInfSetup(true) {}
MultiBound::MultiBound(const double x, const MultiBound::SupInfType bound)
- : mode(MODE_DOUBLE), boundSingle(0), boundSup((bound == BOUND_SUP) ? x : 0),
- boundInf((bound == BOUND_INF) ? x : 0), boundSupSetup(bound == BOUND_SUP),
+ : mode(MODE_DOUBLE),
+ boundSingle(0),
+ boundSup((bound == BOUND_SUP) ? x : 0),
+ boundInf((bound == BOUND_INF) ? x : 0),
+ boundSupSetup(bound == BOUND_SUP),
boundInfSetup(bound == BOUND_INF) {}
MultiBound::MultiBound(const MultiBound &clone)
- : mode(clone.mode), boundSingle(clone.boundSingle),
- boundSup(clone.boundSup), boundInf(clone.boundInf),
- boundSupSetup(clone.boundSupSetup), boundInfSetup(clone.boundInfSetup) {}
+ : mode(clone.mode),
+ boundSingle(clone.boundSingle),
+ boundSup(clone.boundSup),
+ boundInf(clone.boundInf),
+ boundSupSetup(clone.boundSupSetup),
+ boundInfSetup(clone.boundInfSetup) {}
MultiBound::MultiBoundModeType MultiBound::getMode(void) const { return mode; }
double MultiBound::getSingleBound(void) const {
@@ -45,20 +59,20 @@ double MultiBound::getDoubleBound(const MultiBound::SupInfType bound) const {
"Accessing double bound of a non-double type.");
}
switch (bound) {
- case BOUND_SUP: {
- if (!boundSupSetup) {
- SOT_THROW ExceptionTask(ExceptionTask::BOUND_TYPE,
- "Accessing un-setup sup bound.");
+ case BOUND_SUP: {
+ if (!boundSupSetup) {
+ SOT_THROW ExceptionTask(ExceptionTask::BOUND_TYPE,
+ "Accessing un-setup sup bound.");
+ }
+ return boundSup;
}
- return boundSup;
- }
- case BOUND_INF: {
- if (!boundInfSetup) {
- SOT_THROW ExceptionTask(ExceptionTask::BOUND_TYPE,
- "Accessing un-setup inf bound");
+ case BOUND_INF: {
+ if (!boundInfSetup) {
+ SOT_THROW ExceptionTask(ExceptionTask::BOUND_TYPE,
+ "Accessing un-setup inf bound");
+ }
+ return boundInf;
}
- return boundInf;
- }
}
return 0;
}
@@ -68,10 +82,10 @@ bool MultiBound::getDoubleBoundSetup(const MultiBound::SupInfType bound) const {
"Accessing double bound of a non-double type.");
}
switch (bound) {
- case BOUND_SUP:
- return boundSupSetup;
- case BOUND_INF:
- return boundInfSetup;
+ case BOUND_SUP:
+ return boundSupSetup;
+ case BOUND_INF:
+ return boundInfSetup;
}
return false;
}
@@ -82,14 +96,14 @@ void MultiBound::setDoubleBound(SupInfType boundType, double boundValue) {
boundInfSetup = false;
}
switch (boundType) {
- case BOUND_INF:
- boundInfSetup = true;
- boundInf = boundValue;
- break;
- case BOUND_SUP:
- boundSupSetup = true;
- boundSup = boundValue;
- break;
+ case BOUND_INF:
+ boundInfSetup = true;
+ boundInf = boundValue;
+ break;
+ case BOUND_SUP:
+ boundSupSetup = true;
+ boundSup = boundValue;
+ break;
}
}
void MultiBound::unsetDoubleBound(SupInfType boundType) {
@@ -99,12 +113,12 @@ void MultiBound::unsetDoubleBound(SupInfType boundType) {
boundInfSetup = false;
} else {
switch (boundType) {
- case BOUND_INF:
- boundInfSetup = false;
- break;
- case BOUND_SUP:
- boundSupSetup = false;
- break;
+ case BOUND_INF:
+ boundInfSetup = false;
+ break;
+ case BOUND_SUP:
+ boundSupSetup = false;
+ break;
}
}
}
@@ -131,24 +145,24 @@ namespace sot {
std::ostream &operator<<(std::ostream &os, const MultiBound &m) {
switch (m.mode) {
- case MultiBound::MODE_SINGLE: {
- os << m.boundSingle;
- break;
- }
- case MultiBound::MODE_DOUBLE: {
- os << "(";
- if (m.boundInfSetup)
- os << m.boundInf;
- else
- os << "--";
- os << ",";
- if (m.boundSupSetup)
- os << m.boundSup;
- else
- os << "--";
- os << ")";
- break;
- }
+ case MultiBound::MODE_SINGLE: {
+ os << m.boundSingle;
+ break;
+ }
+ case MultiBound::MODE_DOUBLE: {
+ os << "(";
+ if (m.boundInfSetup)
+ os << m.boundInf;
+ else
+ os << "--";
+ os << ",";
+ if (m.boundSupSetup)
+ os << m.boundSup;
+ else
+ os << "--";
+ os << ")";
+ break;
+ }
}
return os;
}
@@ -215,8 +229,7 @@ std::ostream &operator<<(std::ostream &os, const VectorMultiBound &v) {
os << "[" << v.size() << "](";
for (VectorMultiBound::const_iterator iter = v.begin(); iter != v.end();
++iter) {
- if (iter != v.begin())
- os << ",";
+ if (iter != v.begin()) os << ",";
os << (*iter);
}
return os << ")";
diff --git a/src/task/task-abstract.cpp b/src/task/task-abstract.cpp
index 2c9fafd1..7070b939 100644
--- a/src/task/task-abstract.cpp
+++ b/src/task/task-abstract.cpp
@@ -23,7 +23,8 @@ using namespace dynamicgraph;
/* --------------------------------------------------------------------- */
TaskAbstract::TaskAbstract(const std::string &n)
- : Entity(n), memoryInternal(NULL),
+ : Entity(n),
+ memoryInternal(NULL),
taskSOUT("sotTaskAbstract(" + n + ")::output(vector)::task"),
jacobianSOUT("sotTaskAbstract(" + n + ")::output(matrix)::jacobian") {
taskRegistration();
diff --git a/src/task/task-command.h b/src/task/task-command.h
index 04704c07..be34bbbb 100644
--- a/src/task/task-command.h
+++ b/src/task/task-command.h
@@ -9,12 +9,12 @@
#ifndef TASK_COMMAND_H
#define TASK_COMMAND_H
-#include <boost/assign/list_of.hpp>
-
#include <dynamic-graph/command-getter.h>
#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/command.h>
+#include <boost/assign/list_of.hpp>
+
namespace dynamicgraph {
namespace sot {
namespace command {
@@ -24,7 +24,7 @@ using ::dynamicgraph::command::Value;
// Command ListFeatures
class ListFeatures : public Command {
-public:
+ public:
virtual ~ListFeatures() {}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
@@ -42,10 +42,10 @@ public:
result += "]";
return Value(result);
}
-}; // class ListFeatures
-} // namespace task
-} // namespace command
+}; // class ListFeatures
+} // namespace task
+} // namespace command
} /* namespace sot */
} /* namespace dynamicgraph */
-#endif // TASK_COMMAND_H
+#endif // TASK_COMMAND_H
diff --git a/src/task/task-conti.cpp b/src/task/task-conti.cpp
index 986e2275..4129f95e 100644
--- a/src/task/task-conti.cpp
+++ b/src/task/task-conti.cpp
@@ -13,6 +13,7 @@
/* SOT */
#include <dynamic-graph/linear-algebra.h>
+
#include <sot/core/debug.hh>
#include <sot/core/factory.hh>
#include <sot/core/task-conti.hh>
@@ -28,16 +29,17 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TaskConti, "TaskConti");
/* --------------------------------------------------------------------- */
TaskConti::TaskConti(const std::string &n)
- : Task(n), timeRef(TIME_REF_UNSIGNIFICANT), mu(0),
+ : Task(n),
+ timeRef(TIME_REF_UNSIGNIFICANT),
+ mu(0),
controlPrevSIN(NULL, "sotTaskConti(" + n + ")::input(double)::q0") {
taskSOUT.setFunction(
boost::bind(&TaskConti::computeContiDesiredVelocity, this, _1, _2));
signalRegistration(controlPrevSIN);
}
-VectorMultiBound &
-TaskConti::computeContiDesiredVelocity(VectorMultiBound &desvel2b,
- const int &timecurr) {
+VectorMultiBound &TaskConti::computeContiDesiredVelocity(
+ VectorMultiBound &desvel2b, const int &timecurr) {
sotDEBUG(15) << "# In {" << endl;
dynamicgraph::Vector desvel = errorSOUT(timecurr);
@@ -49,12 +51,10 @@ TaskConti::computeContiDesiredVelocity(VectorMultiBound &desvel2b,
dynamicgraph::Vector deref(J.rows());
sotDEBUG(15) << "q0 = " << q0 << std::endl;
sotDEBUG(25) << "J = " << J << std::endl;
- if (q0.size() != (J.cols() - 6))
- throw; // TODO
+ if (q0.size() != (J.cols() - 6)) throw; // TODO
for (int i = 0; i < J.rows(); ++i) {
deref(i) = 0;
- for (int j = 6; j < J.cols(); ++j)
- deref(i) += J(i, j) * q0(j - 6);
+ for (int j = 6; j < J.cols(); ++j) deref(i) += J(i, j) * q0(j - 6);
}
if (timeRef == TIME_REF_TO_BE_SET) {
@@ -82,8 +82,7 @@ TaskConti::computeContiDesiredVelocity(VectorMultiBound &desvel2b,
sotDEBUG(25) << "task: " << desvel << std::endl;
desvel2b.resize(desvel.size());
- for (int i = 0; i < desvel.size(); ++i)
- desvel2b[i] = desvel(i);
+ for (int i = 0; i < desvel.size(); ++i) desvel2b[i] = desvel(i);
sotDEBUG(15) << "# Out }" << endl;
return desvel2b;
@@ -91,8 +90,7 @@ TaskConti::computeContiDesiredVelocity(VectorMultiBound &desvel2b,
const dynamicgraph::Vector &desvel = errorSOUT(timecurr);
const double &gain = controlGainSIN(timecurr);
desvel2b.resize(desvel.size());
- for (int i = 0; i < desvel.size(); ++i)
- desvel2b[i] = -gain * desvel(i);
+ for (int i = 0; i < desvel.size(); ++i) desvel2b[i] = -gain * desvel(i);
return desvel2b;
}
}
diff --git a/src/task/task-pd.cpp b/src/task/task-pd.cpp
index b47fcdf2..73d2aebb 100644
--- a/src/task/task-pd.cpp
+++ b/src/task/task-pd.cpp
@@ -13,6 +13,7 @@
/* SOT */
#include <dynamic-graph/all-commands.h>
+
#include <sot/core/debug.hh>
#include <sot/core/task-pd.hh>
@@ -29,7 +30,9 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TaskPD, "TaskPD");
/* --------------------------------------------------------------------- */
TaskPD::TaskPD(const std::string &n)
- : Task(n), previousError(), beta(1),
+ : Task(n),
+ previousError(),
+ beta(1),
errorDotSOUT(boost::bind(&TaskPD::computeErrorDot, this, _1, _2),
errorSOUT,
"sotTaskPD(" + n + ")::output(vector)::errorDotOUT"),
diff --git a/src/task/task-unilateral.cpp b/src/task/task-unilateral.cpp
index e8cf9f8f..e09db949 100644
--- a/src/task/task-unilateral.cpp
+++ b/src/task/task-unilateral.cpp
@@ -30,13 +30,14 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TaskUnilateral, "TaskUnilateral");
/* --------------------------------------------------------------------- */
TaskUnilateral::TaskUnilateral(const std::string &n)
- : Task(n), featureList(),
+ : Task(n),
+ featureList(),
positionSIN(NULL,
"sotTaskUnilateral(" + n + ")::input(vector)::position"),
- referenceInfSIN(NULL, "sotTaskUnilateral(" + n +
- ")::input(vector)::referenceInf"),
- referenceSupSIN(NULL, "sotTaskUnilateral(" + n +
- ")::input(vector)::referenceSup"),
+ referenceInfSIN(
+ NULL, "sotTaskUnilateral(" + n + ")::input(vector)::referenceInf"),
+ referenceSupSIN(
+ NULL, "sotTaskUnilateral(" + n + ")::input(vector)::referenceSup"),
dtSIN(NULL, "sotTaskUnilateral(" + n + ")::input(double)::dt") {
taskSOUT.setFunction(
boost::bind(&TaskUnilateral::computeTaskUnilateral, this, _1, _2));
diff --git a/src/task/task.cpp b/src/task/task.cpp
index 80bad033..7bf4f65c 100644
--- a/src/task/task.cpp
+++ b/src/task/task.cpp
@@ -12,12 +12,13 @@
/* --------------------------------------------------------------------- */
/* SOT */
+#include <dynamic-graph/all-commands.h>
+
#include <sot/core/debug.hh>
+#include <sot/core/pool.hh>
#include <sot/core/task.hh>
#include "../src/task/task-command.h"
-#include <dynamic-graph/all-commands.h>
-#include <sot/core/pool.hh>
using namespace std;
using namespace dynamicgraph::sot;
@@ -31,7 +32,9 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Task, "Task");
/* --------------------------------------------------------------------- */
Task::Task(const std::string &n)
- : TaskAbstract(n), featureList(), withDerivative(false),
+ : TaskAbstract(n),
+ featureList(),
+ withDerivative(false),
controlGainSIN(NULL, "sotTask(" + n + ")::input(double)::controlGain"),
dampingGainSINOUT(NULL, "sotTask(" + n + ")::in/output(double)::damping")
// TODO As far as I understand, this is not used in this class.
@@ -69,12 +72,13 @@ void Task::initCommands(void) {
//
std::string docstring;
// AddFeature
- docstring = " \n"
- " Add a feature to the task\n"
- " \n"
- " Input:\n"
- " - name of the feature\n"
- " \n";
+ docstring =
+ " \n"
+ " Add a feature to the task\n"
+ " \n"
+ " Input:\n"
+ " - name of the feature\n"
+ " \n";
addCommand("add",
makeCommandVoid1(*this, &Task::addFeatureFromName, docstring));
@@ -85,16 +89,18 @@ void Task::initCommands(void) {
makeDirectGetter(*this, &withDerivative,
docDirectGetter("withDerivative", "bool")));
// ClearFeatureList
- docstring = " \n"
- " Clear the list of features of the task\n"
- " \n";
+ docstring =
+ " \n"
+ " Clear the list of features of the task\n"
+ " \n";
addCommand("clear",
makeCommandVoid0(*this, &Task::clearFeatureList, docstring));
// List features
- docstring = " \n"
- " Returns the list of features of the task\n"
- " \n";
+ docstring =
+ " \n"
+ " Returns the list of features of the task\n"
+ " \n";
addCommand("list", new command::task::ListFeatures(*this, docstring));
}
@@ -113,7 +119,6 @@ void Task::addFeatureFromName(const std::string &featureName) {
}
void Task::clearFeatureList(void) {
-
for (FeatureList_t::iterator iter = featureList.begin();
iter != featureList.end(); ++iter) {
FeatureAbstract &s = **iter;
@@ -183,7 +188,7 @@ dynamicgraph::Vector &Task::computeError(dynamicgraph::Vector &error,
const dynamicgraph::Vector &partialError = feature.errorSOUT(time);
const dynamicgraph::Vector::Index dim = partialError.size();
- while (cursorError + dim > dimError) // DEBUG It was >=
+ while (cursorError + dim > dimError) // DEBUG It was >=
{
dimError *= 2;
error.resize(dimError);
@@ -206,8 +211,8 @@ dynamicgraph::Vector &Task::computeError(dynamicgraph::Vector &error,
return error;
}
-dynamicgraph::Vector &
-Task::computeErrorTimeDerivative(dynamicgraph::Vector &res, int time) {
+dynamicgraph::Vector &Task::computeErrorTimeDerivative(
+ dynamicgraph::Vector &res, int time) {
res.resize(errorSOUT(time).size());
dynamicgraph::Vector::Index cursor = 0;
@@ -224,8 +229,8 @@ Task::computeErrorTimeDerivative(dynamicgraph::Vector &res, int time) {
return res;
}
-VectorMultiBound &
-Task::computeTaskExponentialDecrease(VectorMultiBound &errorRef, int time) {
+VectorMultiBound &Task::computeTaskExponentialDecrease(
+ VectorMultiBound &errorRef, int time) {
sotDEBUG(15) << "# In {" << endl;
const dynamicgraph::Vector &errSingleBound = errorSOUT(time);
const double &gain = controlGainSIN(time);
diff --git a/src/tools/binary-int-to-uint.cpp b/src/tools/binary-int-to-uint.cpp
index 531fc0cc..6e1f74be 100644
--- a/src/tools/binary-int-to-uint.cpp
+++ b/src/tools/binary-int-to-uint.cpp
@@ -28,10 +28,10 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(BinaryIntToUint, "BinaryIntToUint");
BinaryIntToUint::BinaryIntToUint(const string &fname)
: Entity(fname),
binaryIntSIN(NULL, "BinaryIntToUint(" + name + ")::input(int)::sin"),
- binaryUintSOUT(boost::bind(&BinaryIntToUint::computeOutput, this, _1, _2),
- binaryIntSIN,
- "BinaryIntToUint(" + name +
- ")::output(unsigned int)::sout") {
+ binaryUintSOUT(
+ boost::bind(&BinaryIntToUint::computeOutput, this, _1, _2),
+ binaryIntSIN,
+ "BinaryIntToUint(" + name + ")::output(unsigned int)::sout") {
signalRegistration(binaryIntSIN << binaryUintSOUT);
}
diff --git a/src/tools/clamp-workspace.cpp b/src/tools/clamp-workspace.cpp
index 11a62111..02c73d26 100644
--- a/src/tools/clamp-workspace.cpp
+++ b/src/tools/clamp-workspace.cpp
@@ -8,7 +8,6 @@
*/
#include <cmath>
-
#include <sot/core/clamp-workspace.hh>
using namespace std;
@@ -21,8 +20,9 @@ using namespace dynamicgraph;
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ClampWorkspace, "ClampWorkspace");
ClampWorkspace::ClampWorkspace(const string &fName)
- : Entity(fName), positionrefSIN(NULL, "ClampWorkspace(" + name +
- ")::input(double)::positionref"),
+ : Entity(fName),
+ positionrefSIN(
+ NULL, "ClampWorkspace(" + name + ")::input(double)::positionref"),
positionSIN(NULL, "ClampWorkspace(" + name + ")::input(double)::position")
,
@@ -44,15 +44,23 @@ ClampWorkspace::ClampWorkspace(const string &fName)
timeUpdate(0)
,
- alpha(6, 6), alphabar(6, 6)
+ alpha(6, 6),
+ alphabar(6, 6)
,
pd(3)
,
- beta(1), scale(0), dm_min(0.019), dm_max(0.025), dm_min_yaw(0.019),
- dm_max_yaw(0.119), theta_min(-30. * 3.14159 / 180.),
- theta_max(5. * 3.14159 / 180.), mode(1), frame(FRAME_POINT)
+ beta(1),
+ scale(0),
+ dm_min(0.019),
+ dm_max(0.025),
+ dm_min_yaw(0.019),
+ dm_max_yaw(0.119),
+ theta_min(-30. * 3.14159 / 180.),
+ theta_max(5. * 3.14159 / 180.),
+ mode(1),
+ frame(FRAME_POINT)
{
alpha.setZero();
@@ -94,18 +102,18 @@ void ClampWorkspace::update(int time) {
}
switch (mode) {
- case 0:
- alpha(i, i) = 0;
- alphabar(i, i) = 1;
- break;
- case 1:
- alpha(i, i) = 1;
- alphabar(i, i) = 0;
- break;
- case 2:
- default:
- alpha(i, i) = 0.5 * (1 + tanh(1 / Y - 1 / (1 - Y)));
- alphabar(i, i) = 1 - alpha(i);
+ case 0:
+ alpha(i, i) = 0;
+ alphabar(i, i) = 1;
+ break;
+ case 1:
+ alpha(i, i) = 1;
+ alphabar(i, i) = 0;
+ break;
+ case 2:
+ default:
+ alpha(i, i) = 0.5 * (1 + tanh(1 / Y - 1 / (1 - Y)));
+ alphabar(i, i) = 1 - alpha(i);
}
if (i == 2) {
diff --git a/src/tools/com-freezer.cpp b/src/tools/com-freezer.cpp
index 175ebc06..700e3440 100644
--- a/src/tools/com-freezer.cpp
+++ b/src/tools/com-freezer.cpp
@@ -8,6 +8,7 @@
*/
#include <dynamic-graph/factory.h>
+
#include <sot/core/com-freezer.hh>
#include <sot/core/debug.hh>
@@ -17,7 +18,9 @@ using namespace dynamicgraph::sot;
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(CoMFreezer, "CoMFreezer");
CoMFreezer::CoMFreezer(const std::string &name)
- : Entity(name), m_lastCoM(3), m_previousPGInProcess(false),
+ : Entity(name),
+ m_lastCoM(3),
+ m_previousPGInProcess(false),
m_lastStopTime(-1)
,
@@ -40,9 +43,8 @@ CoMFreezer::~CoMFreezer(void) {
return;
}
-dynamicgraph::Vector &
-CoMFreezer::computeFreezedCoM(dynamicgraph::Vector &freezedCoM,
- const int &time) {
+dynamicgraph::Vector &CoMFreezer::computeFreezedCoM(
+ dynamicgraph::Vector &freezedCoM, const int &time) {
sotDEBUGIN(15);
unsigned PGInProcess = PGInProcessSIN(time);
diff --git a/src/tools/device.cpp b/src/tools/device.cpp
index 262b7bf0..d07de744 100644
--- a/src/tools/device.cpp
+++ b/src/tools/device.cpp
@@ -14,19 +14,20 @@
#define ENABLE_RT_LOG
#include "sot/core/device.hh"
+
#include <iostream>
#include <sot/core/debug.hh>
#include <sot/core/macros.hh>
using namespace std;
-#include <Eigen/Geometry>
#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/real-time-logger.h>
-#include <sot/core/matrix-geometry.hh>
+#include <Eigen/Geometry>
#include <pinocchio/multibody/liegroup/special-euclidean.hpp>
+#include <sot/core/matrix-geometry.hh>
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
@@ -70,7 +71,9 @@ Device::~Device() {
}
Device::Device(const std::string &n)
- : Entity(n), state_(6), sanityCheck_(true),
+ : Entity(n),
+ state_(6),
+ sanityCheck_(true),
controlInputType_(CONTROL_INPUT_ONE_INTEGRATION),
controlSIN(NULL, "Device(" + n + ")::input(double)::control"),
attitudeSIN(NULL, "Device(" + n + ")::input(vector3)::attitudeIN"),
@@ -89,7 +92,8 @@ Device::Device(const std::string &n)
pseudoTorqueSOUT("Device(" + n + ")::output(vector)::ptorque")
,
- ffPose_(), forceZero6(6) {
+ ffPose_(),
+ forceZero6(6) {
forceZero6.fill(0);
/* --- SIGNALS --- */
for (int i = 0; i < 4; ++i) {
@@ -121,20 +125,23 @@ Device::Device(const std::string &n)
{
std::string docstring;
/* Command setStateSize. */
- docstring = "\n"
- " Set size of state vector\n"
- "\n";
+ docstring =
+ "\n"
+ " Set size of state vector\n"
+ "\n";
addCommand("resize", new command::Setter<Device, unsigned int>(
*this, &Device::setStateSize, docstring));
- docstring = "\n"
- " Set state vector value\n"
- "\n";
+ docstring =
+ "\n"
+ " Set state vector value\n"
+ "\n";
addCommand("set", new command::Setter<Device, Vector>(
*this, &Device::setState, docstring));
- docstring = "\n"
- " Set velocity vector value\n"
- "\n";
+ docstring =
+ "\n"
+ " Set velocity vector value\n"
+ "\n";
addCommand("setVelocity", new command::Setter<Device, Vector>(
*this, &Device::setVelocity, docstring));
@@ -145,35 +152,39 @@ Device::Device(const std::string &n)
command::makeCommandVoid1(*this, setRootPtr, docstring));
/* Second Order Integration set. */
- docstring = "\n"
- " Set the position calculous starting from \n"
- " acceleration measure instead of velocity \n"
- "\n";
+ docstring =
+ "\n"
+ " Set the position calculous starting from \n"
+ " acceleration measure instead of velocity \n"
+ "\n";
addCommand("setSecondOrderIntegration",
command::makeCommandVoid0(
*this, &Device::setSecondOrderIntegration, docstring));
/* Display information */
- docstring = "\n"
- " Display information on device \n"
- "\n";
+ docstring =
+ "\n"
+ " Display information on device \n"
+ "\n";
addCommand("display", command::makeCommandVoid0(*this, &Device::cmdDisplay,
docstring));
/* SET of control input type. */
- docstring = "\n"
- " Set the type of control input which can be \n"
- " acceleration, velocity, or position\n"
- "\n";
+ docstring =
+ "\n"
+ " Set the type of control input which can be \n"
+ " acceleration, velocity, or position\n"
+ "\n";
addCommand("setControlInputType",
new command::Setter<Device, string>(
*this, &Device::setControlInputType, docstring));
- docstring = "\n"
- " Enable/Disable sanity checks\n"
- "\n";
+ docstring =
+ "\n"
+ " Enable/Disable sanity checks\n"
+ "\n";
addCommand("setSanityCheck",
new command::Setter<Device, bool>(*this, &Device::setSanityCheck,
docstring));
@@ -233,36 +244,38 @@ void Device::setState(const Vector &st) {
SOT_CORE_DISABLE_WARNING_PUSH
SOT_CORE_DISABLE_WARNING_FALLTHROUGH
switch (controlInputType_) {
- case CONTROL_INPUT_TWO_INTEGRATION:
- dgRTLOG()
- << "Sanity check for this control is not well supported. "
- "In order to make it work, use pinocchio and the contact forces "
- "to estimate the joint torques for the given acceleration.\n";
- if (s != lowerTorque_.size() || s != upperTorque_.size()) {
- std::ostringstream os;
- os << "dynamicgraph::sot::Device::setState: upper and/or lower torque"
- "bounds do not match state size. Input State size = "
- << st.size() << ", lowerTorque_.size() = " << lowerTorque_.size()
- << ", upperTorque_.size() = " << upperTorque_.size()
- << ". Set them first with setTorqueBounds.";
- throw std::invalid_argument(os.str().c_str());
+ case CONTROL_INPUT_TWO_INTEGRATION:
+ dgRTLOG()
+ << "Sanity check for this control is not well supported. "
+ "In order to make it work, use pinocchio and the contact forces "
+ "to estimate the joint torques for the given acceleration.\n";
+ if (s != lowerTorque_.size() || s != upperTorque_.size()) {
+ std::ostringstream os;
+ os << "dynamicgraph::sot::Device::setState: upper and/or lower torque"
+ "bounds do not match state size. Input State size = "
+ << st.size() << ", lowerTorque_.size() = " << lowerTorque_.size()
+ << ", upperTorque_.size() = " << upperTorque_.size()
+ << ". Set them first with setTorqueBounds.";
+ throw std::invalid_argument(os.str().c_str());
+ // fall through
+ }
+ case CONTROL_INPUT_ONE_INTEGRATION:
+ if (s != lowerVelocity_.size() || s != upperVelocity_.size()) {
+ std::ostringstream os;
+ os << "dynamicgraph::sot::Device::setState: upper and/or lower "
+ "velocity"
+ " bounds do not match state size. Input State size = "
+ << st.size()
+ << ", lowerVelocity_.size() = " << lowerVelocity_.size()
+ << ", upperVelocity_.size() = " << upperVelocity_.size()
+ << ". Set them first with setVelocityBounds.";
+ throw std::invalid_argument(os.str().c_str());
+ }
// fall through
- }
- case CONTROL_INPUT_ONE_INTEGRATION:
- if (s != lowerVelocity_.size() || s != upperVelocity_.size()) {
- std::ostringstream os;
- os << "dynamicgraph::sot::Device::setState: upper and/or lower velocity"
- " bounds do not match state size. Input State size = "
- << st.size() << ", lowerVelocity_.size() = " << lowerVelocity_.size()
- << ", upperVelocity_.size() = " << upperVelocity_.size()
- << ". Set them first with setVelocityBounds.";
- throw std::invalid_argument(os.str().c_str());
- }
- // fall through
- case CONTROL_INPUT_NO_INTEGRATION:
- break;
- default:
- throw std::invalid_argument("Invalid control mode type.");
+ case CONTROL_INPUT_NO_INTEGRATION:
+ break;
+ default:
+ throw std::invalid_argument("Invalid control mode type.");
}
SOT_CORE_DISABLE_WARNING_POP
}
@@ -285,9 +298,8 @@ void Device::setRoot(const Matrix &root) {
void Device::setRoot(const MatrixHomogeneous &worldMwaist) {
VectorRollPitchYaw r = (worldMwaist.linear().eulerAngles(2, 1, 0)).reverse();
Vector q = state_;
- q = worldMwaist.translation(); // abusive ... but working.
- for (unsigned int i = 0; i < 3; ++i)
- q(i + 3) = r(i);
+ q = worldMwaist.translation(); // abusive ... but working.
+ for (unsigned int i = 0; i < 3; ++i) q(i + 3) = r(i);
}
void Device::setSecondOrderIntegration() {
@@ -320,30 +332,30 @@ void Device::setSanityCheck(const bool &enableCheck) {
SOT_CORE_DISABLE_WARNING_PUSH
SOT_CORE_DISABLE_WARNING_FALLTHROUGH
switch (controlInputType_) {
- case CONTROL_INPUT_TWO_INTEGRATION:
- dgRTLOG()
- << "Sanity check for this control is not well supported. "
- "In order to make it work, use pinocchio and the contact forces "
- "to estimate the joint torques for the given acceleration.\n";
- if (s != lowerTorque_.size() || s != upperTorque_.size())
- throw std::invalid_argument(
- "Upper and/or lower torque bounds "
- "do not match state size. Set them first with setTorqueBounds");
- // fall through
- case CONTROL_INPUT_ONE_INTEGRATION:
- if (s != lowerVelocity_.size() || s != upperVelocity_.size())
- throw std::invalid_argument(
- "Upper and/or lower velocity bounds "
- "do not match state size. Set them first with setVelocityBounds");
- // fall through
- case CONTROL_INPUT_NO_INTEGRATION:
- if (s != lowerPosition_.size() || s != upperPosition_.size())
- throw std::invalid_argument(
- "Upper and/or lower position bounds "
- "do not match state size. Set them first with setPositionBounds");
- break;
- default:
- throw std::invalid_argument("Invalid control mode type.");
+ case CONTROL_INPUT_TWO_INTEGRATION:
+ dgRTLOG()
+ << "Sanity check for this control is not well supported. "
+ "In order to make it work, use pinocchio and the contact forces "
+ "to estimate the joint torques for the given acceleration.\n";
+ if (s != lowerTorque_.size() || s != upperTorque_.size())
+ throw std::invalid_argument(
+ "Upper and/or lower torque bounds "
+ "do not match state size. Set them first with setTorqueBounds");
+ // fall through
+ case CONTROL_INPUT_ONE_INTEGRATION:
+ if (s != lowerVelocity_.size() || s != upperVelocity_.size())
+ throw std::invalid_argument(
+ "Upper and/or lower velocity bounds "
+ "do not match state size. Set them first with setVelocityBounds");
+ // fall through
+ case CONTROL_INPUT_NO_INTEGRATION:
+ if (s != lowerPosition_.size() || s != upperPosition_.size())
+ throw std::invalid_argument(
+ "Upper and/or lower position bounds "
+ "do not match state size. Set them first with setPositionBounds");
+ break;
+ default:
+ throw std::invalid_argument("Invalid control mode type.");
}
SOT_CORE_DISABLE_WARNING_POP
}
@@ -438,8 +450,7 @@ void Device::increment(const double &dt) {
velocitySOUT.setTime(time + 1);
}
for (int i = 0; i < 4; ++i) {
- if (!withForceSignals[i])
- forcesSOUT[i]->setConstant(forceZero6);
+ if (!withForceSignals[i]) forcesSOUT[i]->setConstant(forceZero6);
}
Vector zmp(3);
zmp.fill(.0);
@@ -479,15 +490,15 @@ inline bool saturateBounds(double &val, const double &lower,
return false;
}
-#define CHECK_BOUNDS(val, lower, upper, what) \
- for (int i = 0; i < val.size(); ++i) { \
- double old = val(i); \
- if (saturateBounds(val(i), lower(i), upper(i))) { \
- std::ostringstream oss; \
- oss << "Robot " what " bound violation at DoF " << i << ": requested " \
- << old << " but set " << val(i) << '\n'; \
- SEND_ERROR_STREAM_MSG(oss.str()); \
- } \
+#define CHECK_BOUNDS(val, lower, upper, what) \
+ for (int i = 0; i < val.size(); ++i) { \
+ double old = val(i); \
+ if (saturateBounds(val(i), lower(i), upper(i))) { \
+ std::ostringstream oss; \
+ oss << "Robot " what " bound violation at DoF " << i << ": requested " \
+ << old << " but set " << val(i) << '\n'; \
+ SEND_ERROR_STREAM_MSG(oss.str()); \
+ } \
}
void Device::integrate(const double &dt) {
@@ -504,8 +515,7 @@ void Device::integrate(const double &dt) {
return;
}
- if (vel_control_.size() == 0)
- vel_control_ = Vector::Zero(controlIN.size());
+ if (vel_control_.size() == 0) vel_control_ = Vector::Zero(controlIN.size());
// If control size is state size - 6, integrate joint angles,
// if control and state are of same size, integrate 6 first degrees of
diff --git a/src/tools/event.cpp b/src/tools/event.cpp
index 9fa7295d..ae388ca2 100644
--- a/src/tools/event.cpp
+++ b/src/tools/event.cpp
@@ -1,10 +1,10 @@
// Copyright (c) 2017, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
-#include <sot/core/event.hh>
-
#include <dynamic-graph/factory.h>
+#include <sot/core/event.hh>
+
namespace dynamicgraph {
namespace sot {
@@ -24,8 +24,8 @@ bool &Event::check(bool &ret, const int &time) {
lastVal_ = val;
}
if (trigger) {
- for (Triggers_t::const_iterator _s = triggers.begin();
- _s != triggers.end(); ++_s)
+ for (Triggers_t::const_iterator _s = triggers.begin(); _s != triggers.end();
+ ++_s)
(*_s)->recompute(time);
timeSinceUp_ = 0;
}
@@ -33,5 +33,5 @@ bool &Event::check(bool &ret, const int &time) {
}
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Event, "Event");
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
diff --git a/src/tools/exp-moving-avg.cpp b/src/tools/exp-moving-avg.cpp
index 47863c81..1efca5ce 100644
--- a/src/tools/exp-moving-avg.cpp
+++ b/src/tools/exp-moving-avg.cpp
@@ -6,11 +6,10 @@
*
*/
-#include <boost/function.hpp>
-
#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>
+#include <boost/function.hpp>
#include <sot/core/exp-moving-avg.hh>
#include <sot/core/factory.hh>
@@ -36,16 +35,18 @@ ExpMovingAvg::ExpMovingAvg(const std::string &n)
averageSOUT(boost::bind(&ExpMovingAvg::update, this, _1, _2),
updateSIN << refresherSINTERN,
"ExpMovingAvg(" + n + ")::output(vector)::average"),
- alpha(0.), init(false) {
+ alpha(0.),
+ init(false) {
// Register signals into the entity.
signalRegistration(updateSIN << averageSOUT);
refresherSINTERN.setDependencyType(TimeDependency<int>::ALWAYS_READY);
std::string docstring;
// setAlpha
- docstring = "\n"
- " Set the alpha used to update the current value."
- "\n";
+ docstring =
+ "\n"
+ " Set the alpha used to update the current value."
+ "\n";
addCommand(std::string("setAlpha"),
new ::dynamicgraph::command::Setter<ExpMovingAvg, double>(
*this, &ExpMovingAvg::setAlpha, docstring));
diff --git a/src/tools/gradient-ascent.cpp b/src/tools/gradient-ascent.cpp
index b8d1f872..b12b4c24 100644
--- a/src/tools/gradient-ascent.cpp
+++ b/src/tools/gradient-ascent.cpp
@@ -6,11 +6,10 @@
*
*/
-#include <boost/function.hpp>
-
#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>
+#include <boost/function.hpp>
#include <sot/core/factory.hh>
#include <sot/core/gradient-ascent.hh>
diff --git a/src/tools/gripper-control.cpp b/src/tools/gripper-control.cpp
index ef947b93..29fb5fb9 100644
--- a/src/tools/gripper-control.cpp
+++ b/src/tools/gripper-control.cpp
@@ -9,14 +9,14 @@
#define ENABLE_RT_LOG
+#include <dynamic-graph/all-commands.h>
+#include <dynamic-graph/real-time-logger.h>
+
#include <sot/core/debug.hh>
#include <sot/core/factory.hh>
#include <sot/core/gripper-control.hh>
#include <sot/core/macros-signal.hh>
-#include <dynamic-graph/all-commands.h>
-#include <dynamic-graph/real-time-logger.h>
-
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
@@ -26,15 +26,15 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(GripperControlPlugin, "GripperControl");
/* --- PLUGIN --------------------------------------------------------------- */
/* --- PLUGIN --------------------------------------------------------------- */
-#define SOT_FULL_TO_REDUCED(sotName) \
- sotName##FullSizeSIN(NULL, "GripperControl(" + name + \
- ")::input(vector)::" + #sotName + "FullIN"), \
- sotName##ReduceSOUT(SOT_INIT_SIGNAL_2(GripperControlPlugin::selector, \
- sotName##FullSizeSIN, \
- dynamicgraph::Vector, \
- selectionSIN, Flags), \
- "GripperControl(" + name + \
- ")::input(vector)::" + #sotName + "ReducedOUT")
+#define SOT_FULL_TO_REDUCED(sotName) \
+ sotName##FullSizeSIN(NULL, "GripperControl(" + name + \
+ ")::input(vector)::" + #sotName + "FullIN"), \
+ sotName##ReduceSOUT( \
+ SOT_INIT_SIGNAL_2(GripperControlPlugin::selector, \
+ sotName##FullSizeSIN, dynamicgraph::Vector, \
+ selectionSIN, Flags), \
+ "GripperControl(" + name + ")::input(vector)::" + #sotName + \
+ "ReducedOUT")
const double GripperControl::OFFSET_DEFAULT = 0.9;
@@ -45,18 +45,20 @@ GripperControl::GripperControl(void)
: offset(GripperControl::OFFSET_DEFAULT), factor() {}
GripperControlPlugin::GripperControlPlugin(const std::string &name)
- : Entity(name), calibrationStarted(false),
+ : Entity(name),
+ calibrationStarted(false),
positionSIN(NULL,
"GripperControl(" + name + ")::input(vector)::position"),
- positionDesSIN(NULL, "GripperControl(" + name +
- ")::input(vector)::positionDes"),
+ positionDesSIN(
+ NULL, "GripperControl(" + name + ")::input(vector)::positionDes"),
torqueSIN(NULL, "GripperControl(" + name + ")::input(vector)::torque"),
- torqueLimitSIN(NULL, "GripperControl(" + name +
- ")::input(vector)::torqueLimit"),
+ torqueLimitSIN(
+ NULL, "GripperControl(" + name + ")::input(vector)::torqueLimit"),
selectionSIN(NULL, "GripperControl(" + name + ")::input(vector)::selec")
,
- SOT_FULL_TO_REDUCED(position), SOT_FULL_TO_REDUCED(torque),
+ SOT_FULL_TO_REDUCED(position),
+ SOT_FULL_TO_REDUCED(torque),
SOT_FULL_TO_REDUCED(torqueLimit),
desiredPositionSOUT(
SOT_MEMBER_SIGNAL_4(GripperControl::computeDesiredPosition,
@@ -127,12 +129,11 @@ void GripperControl::computeIncrement(
}
}
-dynamicgraph::Vector &
-GripperControl::computeDesiredPosition(const dynamicgraph::Vector ¤tPos,
- const dynamicgraph::Vector &desiredPos,
- const dynamicgraph::Vector &torques,
- const dynamicgraph::Vector &torqueLimits,
- dynamicgraph::Vector &referencePos) {
+dynamicgraph::Vector &GripperControl::computeDesiredPosition(
+ const dynamicgraph::Vector ¤tPos,
+ const dynamicgraph::Vector &desiredPos, const dynamicgraph::Vector &torques,
+ const dynamicgraph::Vector &torqueLimits,
+ dynamicgraph::Vector &referencePos) {
const dynamicgraph::Vector::Index SIZE = currentPos.size();
// if( (SIZE==torques.size()) )
// { /* ERROR ... */ }
@@ -156,20 +157,18 @@ GripperControl::computeDesiredPosition(const dynamicgraph::Vector ¤tPos,
return referencePos;
}
-dynamicgraph::Vector &
-GripperControl::selector(const dynamicgraph::Vector &fullsize,
- const Flags &selec, dynamicgraph::Vector &desPos) {
+dynamicgraph::Vector &GripperControl::selector(
+ const dynamicgraph::Vector &fullsize, const Flags &selec,
+ dynamicgraph::Vector &desPos) {
int size = 0;
for (int i = 0; i < fullsize.size(); ++i) {
- if (selec(i))
- size++;
+ if (selec(i)) size++;
}
int curs = 0;
desPos.resize(size);
for (int i = 0; i < fullsize.size(); ++i) {
- if (selec(i))
- desPos(curs++) = fullsize(i);
+ if (selec(i)) desPos(curs++) = fullsize(i);
}
return desPos;
diff --git a/src/tools/joint-limitator.cpp b/src/tools/joint-limitator.cpp
index b4eec1b8..3a0474b0 100644
--- a/src/tools/joint-limitator.cpp
+++ b/src/tools/joint-limitator.cpp
@@ -72,7 +72,7 @@ dynamicgraph::Vector &JointLimitator::computeControl(dynamicgraph::Vector &uOUT,
for (unsigned int i = 0; i < controlSize; ++i) {
double qnext = q(i + offset) + uIN(i) * 0.005;
- if ((i + offset < 6) || // do not take into account of freeflyer
+ if ((i + offset < 6) || // do not take into account of freeflyer
((qnext < UJL(i + offset)) && (qnext > LJL(i + offset)))) {
uOUT(i) = uIN(i);
}
@@ -85,6 +85,5 @@ dynamicgraph::Vector &JointLimitator::computeControl(dynamicgraph::Vector &uOUT,
}
void JointLimitator::display(std::ostream &os) const {
-
os << "JointLimitator <" << name << "> ... TODO";
}
diff --git a/src/tools/joint-trajectory-command.hh b/src/tools/joint-trajectory-command.hh
index c645e59d..b202cc49 100644
--- a/src/tools/joint-trajectory-command.hh
+++ b/src/tools/joint-trajectory-command.hh
@@ -8,12 +8,11 @@
#ifndef JOINT_TRAJECTORY_COMMAND_H_
#define JOINT_TRAJECTORY_COMMAND_H_
-#include <boost/assign/list_of.hpp>
-
#include <dynamic-graph/command-getter.h>
#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/command.h>
+#include <boost/assign/list_of.hpp>
#include <sot/core/joint-trajectory-entity.hh>
namespace dynamicgraph {
@@ -24,7 +23,7 @@ using ::dynamicgraph::command::Command;
using ::dynamicgraph::command::Value;
class SetInitTrajectory : public Command {
-public:
+ public:
virtual ~SetInitTrajectory() {}
/// Set the initial trajectory.
@@ -33,13 +32,12 @@ public:
: Command(entity, boost::assign::list_of(Value::STRING), docstring) {}
virtual Value doExecute() {
-
// return void
return Value();
}
};
-} // namespace classSot
-} // namespace command
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace classSot
+} // namespace command
+} // namespace sot
+} // namespace dynamicgraph
#endif /* JOINT_TRAJECTORY_COMMAND_H_ */
diff --git a/src/tools/joint-trajectory-entity.cpp b/src/tools/joint-trajectory-entity.cpp
index 2bcd6b7f..18317b51 100644
--- a/src/tools/joint-trajectory-entity.cpp
+++ b/src/tools/joint-trajectory-entity.cpp
@@ -11,11 +11,11 @@
#include <sot/core/matrix-geometry.hh>
#ifdef VP_DEBUG
class sotJTE__INIT {
-public:
+ public:
sotJTE__INIT(void) { dynamicgraph::sot::DebugTrace::openFile(); }
};
sotJTE__INIT sotJTE_initiator;
-#endif //#ifdef VP_DEBUG
+#endif //#ifdef VP_DEBUG
#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/command-bind.h>
@@ -34,8 +34,9 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SotJointTrajectoryEntity,
"SotJointTrajectoryEntity");
SotJointTrajectoryEntity::SotJointTrajectoryEntity(const std::string &n)
- : Entity(n), refresherSINTERN("SotJointTrajectoryEntity(" + n +
- ")::intern(dummy)::refresher"),
+ : Entity(n),
+ refresherSINTERN("SotJointTrajectoryEntity(" + n +
+ ")::intern(dummy)::refresher"),
OneStepOfUpdateS(
boost::bind(&SotJointTrajectoryEntity::OneStepOfUpdate, this, _1, _2),
refresherSINTERN << trajectorySIN,
@@ -59,7 +60,10 @@ SotJointTrajectoryEntity::SotJointTrajectoryEntity(const std::string &n)
"SotJointTrajectory(" + n + ")::output(uint)::seqid"),
trajectorySIN(NULL, "SotJointTrajectory(" + n +
")::input(trajectory)::trajectoryIN"),
- index_(0), traj_timestamp_(0, 0), seqid_(0), deque_traj_(0) {
+ index_(0),
+ traj_timestamp_(0, 0),
+ seqid_(0),
+ deque_traj_(0) {
using namespace command;
sotDEBUGIN(5);
@@ -69,12 +73,13 @@ SotJointTrajectoryEntity::SotJointTrajectoryEntity(const std::string &n)
refresherSINTERN.setReady(true);
std::string docstring;
- docstring = " \n"
- " initialize the first trajectory.\n"
- " \n"
- " Input:\n"
- " = a string : .\n"
- " \n";
+ docstring =
+ " \n"
+ " initialize the first trajectory.\n"
+ " \n"
+ " Input:\n"
+ " = a string : .\n"
+ " \n";
addCommand("initTraj",
makeCommandVoid1(*this, &SotJointTrajectoryEntity::setInitTraj,
docCommandVoid1("Set initial trajectory",
@@ -83,12 +88,10 @@ SotJointTrajectoryEntity::SotJointTrajectoryEntity(const std::string &n)
}
void SotJointTrajectoryEntity::UpdatePoint(const JointTrajectoryPoint &aJTP) {
-
sotDEBUGIN(5);
// Posture
std::vector<JointTrajectoryPoint>::size_type possize = aJTP.positions_.size();
- if (possize == 0)
- return;
+ if (possize == 0) return;
pose_.resize(aJTP.positions_.size());
for (std::vector<JointTrajectoryPoint>::size_type i = 0; i < possize - 5;
@@ -169,7 +172,6 @@ void SotJointTrajectoryEntity::UpdateTrajectory(const Trajectory &aTrajectory) {
// Strategy at the end of the trajectory.
if (index_ >= deque_traj_.front().points_.size()) {
-
if (deque_traj_.size() > 1) {
deque_traj_.pop_front();
index_ = 0;
@@ -237,9 +239,8 @@ sot::MatrixHomogeneous SotJointTrajectoryEntity::XYZThetaToMatrixHomogeneous(
return res;
}
-dynamicgraph::Vector &
-SotJointTrajectoryEntity::getNextPosition(dynamicgraph::Vector &pos,
- const int &time) {
+dynamicgraph::Vector &SotJointTrajectoryEntity::getNextPosition(
+ dynamicgraph::Vector &pos, const int &time) {
sotDEBUGIN(5);
OneStepOfUpdateS(time);
pos = pose_;
@@ -248,9 +249,8 @@ SotJointTrajectoryEntity::getNextPosition(dynamicgraph::Vector &pos,
return pos;
}
-dynamicgraph::Vector &
-SotJointTrajectoryEntity::getNextCoM(dynamicgraph::Vector &com,
- const int &time) {
+dynamicgraph::Vector &SotJointTrajectoryEntity::getNextCoM(
+ dynamicgraph::Vector &com, const int &time) {
sotDEBUGIN(5);
OneStepOfUpdateS(time);
com = com_;
@@ -258,9 +258,8 @@ SotJointTrajectoryEntity::getNextCoM(dynamicgraph::Vector &com,
return com;
}
-dynamicgraph::Vector &
-SotJointTrajectoryEntity::getNextCoP(dynamicgraph::Vector &cop,
- const int &time) {
+dynamicgraph::Vector &SotJointTrajectoryEntity::getNextCoP(
+ dynamicgraph::Vector &cop, const int &time) {
sotDEBUGIN(5);
OneStepOfUpdateS(time);
cop = cop_;
@@ -268,9 +267,8 @@ SotJointTrajectoryEntity::getNextCoP(dynamicgraph::Vector &cop,
return cop;
}
-sot::MatrixHomogeneous &
-SotJointTrajectoryEntity::getNextWaist(sot::MatrixHomogeneous &waist,
- const int &time) {
+sot::MatrixHomogeneous &SotJointTrajectoryEntity::getNextWaist(
+ sot::MatrixHomogeneous &waist, const int &time) {
sotDEBUGIN(5);
OneStepOfUpdateS(time);
waist = waist_;
diff --git a/src/tools/kalman.cpp b/src/tools/kalman.cpp
index adc5b936..84defc44 100644
--- a/src/tools/kalman.cpp
+++ b/src/tools/kalman.cpp
@@ -10,14 +10,14 @@
*/
/* --- SOT --- */
+#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/factory.h>
+
#include <sot/core/debug.hh>
#include <sot/core/exception-tools.hh>
#include <sot/core/factory.hh>
#include <sot/core/kalman.hh> /* Header of the class implemented here. */
-#include <dynamic-graph/command-setter.h>
-
namespace dynamicgraph {
using command::Setter;
namespace sot {
@@ -25,7 +25,8 @@ namespace sot {
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Kalman, "Kalman");
Kalman::Kalman(const std::string &name)
- : Entity(name), measureSIN(NULL, "Kalman(" + name + ")::input(vector)::y"),
+ : Entity(name),
+ measureSIN(NULL, "Kalman(" + name + ")::input(vector)::y"),
modelTransitionSIN(NULL, "Kalman(" + name + ")::input(matrix)::F"),
modelMeasureSIN(NULL, "Kalman(" + name + ")::input(matrix)::H"),
noiseTransitionSIN(NULL, "Kalman(" + name + ")::input(matrix)::Q"),
@@ -36,7 +37,8 @@ Kalman::Kalman(const std::string &name)
observationPredictedSIN(0, "Kalman(" + name + ")::input(vector)::y_pred"),
varianceUpdateSOUT("Kalman(" + name + ")::output(vector)::P"),
stateUpdateSOUT("Kalman(" + name + ")::output(vector)::x_est"),
- stateEstimation_(), stateVariance_() {
+ stateEstimation_(),
+ stateVariance_() {
sotDEBUGIN(15);
varianceUpdateSOUT.setFunction(
boost::bind(&Kalman::computeVarianceUpdate, this, _1, _2));
@@ -48,18 +50,20 @@ Kalman::Kalman(const std::string &name)
<< noiseMeasureSIN << statePredictedSIN
<< stateUpdateSOUT << varianceUpdateSOUT);
- std::string docstring = " Set initial state estimation\n"
- "\n"
- " input:\n"
- " - a vector: initial state\n";
+ std::string docstring =
+ " Set initial state estimation\n"
+ "\n"
+ " input:\n"
+ " - a vector: initial state\n";
addCommand("setInitialState",
new Setter<Kalman, Vector>(*this, &Kalman::setStateEstimation,
docstring));
- docstring = " Set variance of initial state estimation\n"
- "\n"
- " input:\n"
- " - a matrix: variance covariance matrix\n";
+ docstring =
+ " Set variance of initial state estimation\n"
+ "\n"
+ " input:\n"
+ " - a matrix: variance covariance matrix\n";
addCommand(
"setInitialVariance",
new Setter<Kalman, Matrix>(*this, &Kalman::setStateVariance, docstring));
@@ -75,7 +79,6 @@ Matrix &Kalman::computeVarianceUpdate(Matrix &Pk_k, const int &time) {
varianceUpdateSOUT.addDependency(noiseTransitionSIN);
varianceUpdateSOUT.addDependency(modelTransitionSIN);
} else {
-
const Matrix &Q = noiseTransitionSIN(time);
const Matrix &R = noiseMeasureSIN(time);
const Matrix &F = modelTransitionSIN(time);
@@ -175,8 +178,8 @@ Vector &Kalman::computeStateUpdate(Vector &x_est, const int &time) {
void Kalman::display(std::ostream &) const {}
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
/*!
\file Kalman.cpp
diff --git a/src/tools/latch.cpp b/src/tools/latch.cpp
index 1cc6369d..1c425a52 100644
--- a/src/tools/latch.cpp
+++ b/src/tools/latch.cpp
@@ -11,5 +11,5 @@
namespace dynamicgraph {
namespace sot {
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Latch, "Latch");
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
diff --git a/src/tools/mailbox-vector.cpp b/src/tools/mailbox-vector.cpp
index f82850e2..7a13fffc 100644
--- a/src/tools/mailbox-vector.cpp
+++ b/src/tools/mailbox-vector.cpp
@@ -9,6 +9,7 @@
/* --- SOT PLUGIN --- */
#include <dynamic-graph/linear-algebra.h>
+
#include <sot/core/debug.hh>
#include <sot/core/factory.hh>
#include <sot/core/mailbox-vector.hh>
diff --git a/src/tools/motion-period.cpp b/src/tools/motion-period.cpp
index 71cf9fad..7c7bc05a 100644
--- a/src/tools/motion-period.cpp
+++ b/src/tools/motion-period.cpp
@@ -12,12 +12,12 @@
/* --------------------------------------------------------------------- */
/* --- SOT --- */
+#include <dynamic-graph/linear-algebra.h>
+
#include <sot/core/debug.hh>
#include <sot/core/exception-feature.hh>
#include <sot/core/factory.hh>
#include <sot/core/motion-period.hh>
-
-#include <dynamic-graph/linear-algebra.h>
using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
@@ -29,7 +29,8 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(MotionPeriod, "MotionPeriod");
/* --------------------------------------------------------------------- */
MotionPeriod::MotionPeriod(const string &fName)
- : Entity(fName), motionParams(0),
+ : Entity(fName),
+ motionParams(0),
motionSOUT(boost::bind(&MotionPeriod::computeMotion, this, _1, _2),
sotNOSIGNAL,
"MotionPeriod(" + name + ")::output(vector)::motion") {
@@ -52,19 +53,19 @@ dynamicgraph::Vector &MotionPeriod::computeMotion(dynamicgraph::Vector &res,
double x = ((((time - p.initPeriod) % p.period) + 0.0) / (p.period + 0.0));
res(i) = p.initAmplitude;
switch (p.motionType) {
- case MOTION_CONSTANT: {
- res(i) += p.amplitude;
- break;
- }
- case MOTION_SIN: {
- res(i) += p.amplitude * sin(M_PI * 2 * x);
- break;
- }
- case MOTION_COS: {
- res(i) += p.amplitude * cos(M_PI * 2 * x);
- break;
- }
- // case MOTION_: {res(i)+= p.amplitude; break}
+ case MOTION_CONSTANT: {
+ res(i) += p.amplitude;
+ break;
+ }
+ case MOTION_SIN: {
+ res(i) += p.amplitude * sin(M_PI * 2 * x);
+ break;
+ }
+ case MOTION_COS: {
+ res(i) += p.amplitude * cos(M_PI * 2 * x);
+ break;
+ }
+ // case MOTION_: {res(i)+= p.amplitude; break}
}
}
@@ -88,19 +89,19 @@ void MotionPeriod::display(std::ostream &os) const {
os << "MotionPeriod <" << name << "> ... TODO";
}
-#define SOT_PARAMS_CONFIG(ARGname, ARGtype) \
- else if (cmdLine == #ARGname) { \
- unsigned int rank; \
- ARGtype period; \
- cmdArgs >> rank >> std::ws; \
- if (rank >= this->size) { \
- os << "!! Error: size size too large." << std::endl; \
- } \
- if (cmdArgs.good()) { \
- cmdArgs >> period; \
- motionParams[rank].ARGname = period; \
- } else { \
- os << #ARGname << "[" << rank << "] = " << motionParams[rank].ARGname \
- << std::endl; \
- } \
+#define SOT_PARAMS_CONFIG(ARGname, ARGtype) \
+ else if (cmdLine == #ARGname) { \
+ unsigned int rank; \
+ ARGtype period; \
+ cmdArgs >> rank >> std::ws; \
+ if (rank >= this->size) { \
+ os << "!! Error: size size too large." << std::endl; \
+ } \
+ if (cmdArgs.good()) { \
+ cmdArgs >> period; \
+ motionParams[rank].ARGname = period; \
+ } else { \
+ os << #ARGname << "[" << rank << "] = " << motionParams[rank].ARGname \
+ << std::endl; \
+ } \
}
diff --git a/src/tools/neck-limitation.cpp b/src/tools/neck-limitation.cpp
index a7539a25..a0f43a3c 100644
--- a/src/tools/neck-limitation.cpp
+++ b/src/tools/neck-limitation.cpp
@@ -8,6 +8,7 @@
*/
#include <dynamic-graph/pool.h>
+
#include <sot/core/debug.hh>
#include <sot/core/exception-tools.hh>
#include <sot/core/factory.hh>
@@ -20,15 +21,18 @@ using namespace dynamicgraph;
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(NeckLimitation, "NeckLimitation");
const double NeckLimitation::COEFF_LINEAR_DEFAULT = -25.0 / 42.0;
-const double NeckLimitation::COEFF_AFFINE_DEFAULT = 0.6981; // 40DG
+const double NeckLimitation::COEFF_AFFINE_DEFAULT = 0.6981; // 40DG
const double NeckLimitation::SIGN_TILT_DEFAULT = 1;
const unsigned int NeckLimitation::PAN_RANK_DEFAULT = 14;
const unsigned int NeckLimitation::TILT_RANK_DEFAULT = 15;
NeckLimitation::NeckLimitation(const std::string &name)
- : Entity(name), panRank(PAN_RANK_DEFAULT), tiltRank(TILT_RANK_DEFAULT),
+ : Entity(name),
+ panRank(PAN_RANK_DEFAULT),
+ tiltRank(TILT_RANK_DEFAULT),
coeffLinearPan(COEFF_LINEAR_DEFAULT),
- coeffAffinePan(COEFF_AFFINE_DEFAULT), signTilt(SIGN_TILT_DEFAULT)
+ coeffAffinePan(COEFF_AFFINE_DEFAULT),
+ signTilt(SIGN_TILT_DEFAULT)
,
jointSIN(NULL, "NeckLimitation(" + name + ")::input(vector)::joint"),
@@ -54,9 +58,8 @@ NeckLimitation::~NeckLimitation(void) {
/* --- SIGNALS -------------------------------------------------------------- */
/* --- SIGNALS -------------------------------------------------------------- */
-dynamicgraph::Vector &
-NeckLimitation::computeJointLimitation(dynamicgraph::Vector &jointLimited,
- const int &timeSpec) {
+dynamicgraph::Vector &NeckLimitation::computeJointLimitation(
+ dynamicgraph::Vector &jointLimited, const int &timeSpec) {
sotDEBUGIN(15);
const dynamicgraph::Vector &joint = jointSIN(timeSpec);
@@ -67,7 +70,7 @@ NeckLimitation::computeJointLimitation(dynamicgraph::Vector &jointLimited,
double &panLimited = jointLimited(panRank);
double &tiltLimited = jointLimited(tiltRank);
- if (fabs(pan) < 1e-3) // pan == 0
+ if (fabs(pan) < 1e-3) // pan == 0
{
sotDEBUG(15) << "Pan = 0" << std::endl;
if (tilt * signTilt > coeffAffinePan) {
@@ -98,13 +101,12 @@ NeckLimitation::computeJointLimitation(dynamicgraph::Vector &jointLimited,
tiltLimited = tilt;
panLimited = pan;
}
- } else // pan<0
+ } else // pan<0
{
sotDEBUG(15) << "Pan < 0" << std::endl;
sotDEBUG(15) << tilt - coeffAffinePan << "<?" << (-1 * pan * coeffLinearPan)
<< std::endl;
if (tilt * signTilt > (-pan * coeffLinearPan + coeffAffinePan)) {
-
// sotDEBUG(15) << "Below" << std::endl;
// if( (tilt-coeffAffinePan)*coeffLinearPan<pan )
// {
diff --git a/src/tools/parameter-server.cpp b/src/tools/parameter-server.cpp
index 677473f1..face65a9 100644
--- a/src/tools/parameter-server.cpp
+++ b/src/tools/parameter-server.cpp
@@ -15,14 +15,13 @@
*/
#include <iostream>
-
#include <pinocchio/fwd.hpp>
// keep pinocchio before boost
-#include <boost/property_tree/ptree.hpp>
-
#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>
+
+#include <boost/property_tree/ptree.hpp>
#include <sot/core/debug.hh>
#include <sot/core/exception-tools.hh>
#include <sot/core/parameter-server.hh>
@@ -35,9 +34,9 @@ using namespace dynamicgraph::command;
using namespace std;
// Size to be aligned "-------------------------------------------------------"
-#define PROFILE_PWM_DESIRED_COMPUTATION \
+#define PROFILE_PWM_DESIRED_COMPUTATION \
"Control manager "
-#define PROFILE_DYNAMIC_GRAPH_PERIOD \
+#define PROFILE_DYNAMIC_GRAPH_PERIOD \
"Control period "
#define INPUT_SIGNALS
@@ -54,10 +53,13 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ParameterServer, "ParameterServer");
/* --- CONSTRUCTION -------------------------------------------------- */
/* ------------------------------------------------------------------- */
ParameterServer::ParameterServer(const std::string &name)
- : Entity(name), m_robot_util(RefVoidRobotUtil()), m_initSucceeded(false),
- m_emergency_stop_triggered(false), m_is_first_iter(true), m_iter(0),
+ : Entity(name),
+ m_robot_util(RefVoidRobotUtil()),
+ m_initSucceeded(false),
+ m_emergency_stop_triggered(false),
+ m_is_first_iter(true),
+ m_iter(0),
m_sleep_time(0.0) {
-
//~ Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS);
/* Commands. */
@@ -74,10 +76,10 @@ ParameterServer::ParameterServer(const std::string &name)
"Time period in seconds (double)")));
addCommand("setNameToId",
- makeCommandVoid2(*this, &ParameterServer::setNameToId,
- docCommandVoid2("Set map for a name to an Id",
- "(string) joint name",
- "(double) joint id")));
+ makeCommandVoid2(
+ *this, &ParameterServer::setNameToId,
+ docCommandVoid2("Set map for a name to an Id",
+ "(string) joint name", "(double) joint id")));
addCommand(
"setForceNameToForceId",
@@ -203,9 +205,7 @@ ParameterServer::ParameterServer(const std::string &name)
}
void ParameterServer::init_simple(const double &dt) {
-
- if (dt <= 0.0)
- return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
+ if (dt <= 0.0) return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
m_dt = dt;
@@ -241,8 +241,7 @@ void ParameterServer::init_simple(const double &dt) {
void ParameterServer::init(const double &dt, const std::string &urdfFile,
const std::string &robotRef) {
- if (dt <= 0.0)
- return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
+ if (dt <= 0.0) return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
m_dt = dt;
m_emergency_stop_triggered = false;
m_initSucceeded = true;
@@ -299,8 +298,9 @@ void ParameterServer::setForceLimitsFromId(const double &jointId,
void ParameterServer::setForceNameToForceId(const std::string &forceName,
const double &forceId) {
if (!m_initSucceeded) {
- SEND_WARNING_STREAM_MSG("Cannot set force sensor name from force sensor id "
- " before initialization!");
+ SEND_WARNING_STREAM_MSG(
+ "Cannot set force sensor name from force sensor id "
+ " before initialization!");
return;
}
@@ -351,7 +351,8 @@ void ParameterServer::setFootFrameName(const std::string &FootName,
SEND_WARNING_STREAM_MSG("Did not understand the foot name !" + FootName);
}
-void ParameterServer::setHandFrameName(const std::string& HandName, const std::string& FrameName) {
+void ParameterServer::setHandFrameName(const std::string &HandName,
+ const std::string &FrameName) {
if (!m_initSucceeded) {
SEND_WARNING_STREAM_MSG("Cannot set hand frame name!");
return;
@@ -361,7 +362,9 @@ void ParameterServer::setHandFrameName(const std::string& HandName, const std::s
else if (HandName == "Right")
m_robot_util->m_hand_util.m_Right_Hand_Frame_Name = FrameName;
else
- SEND_WARNING_STREAM_MSG("Available hand names are 'Left' and 'Right', not '" + HandName + "' !");
+ SEND_WARNING_STREAM_MSG(
+ "Available hand names are 'Left' and 'Right', not '" + HandName +
+ "' !");
}
void ParameterServer::setImuJointName(const std::string &JointName) {
@@ -421,5 +424,5 @@ bool ParameterServer::isJointInRange(unsigned int id, double q) {
void ParameterServer::display(std::ostream &os) const {
os << "ParameterServer " << getName();
}
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
diff --git a/src/tools/periodic-call-entity.cpp b/src/tools/periodic-call-entity.cpp
index dd03de13..56842618 100644
--- a/src/tools/periodic-call-entity.cpp
+++ b/src/tools/periodic-call-entity.cpp
@@ -13,6 +13,7 @@
/* --- SOT --- */
#include <dynamic-graph/pool.h>
+
#include <sot/core/debug.hh>
#include <sot/core/factory.hh>
#include <sot/core/periodic-call-entity.hh>
@@ -27,7 +28,9 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(PeriodicCallEntity, "PeriodicCallEntity");
/* --------------------------------------------------------------------- */
PeriodicCallEntity::PeriodicCallEntity(const string &fName)
- : Entity(fName), PeriodicCall(), triger("Tracer(" + fName + ")::triger"),
+ : Entity(fName),
+ PeriodicCall(),
+ triger("Tracer(" + fName + ")::triger"),
trigerOnce("Tracer(" + fName + ")::trigerOnce") {
signalRegistration(triger << trigerOnce);
diff --git a/src/tools/periodic-call.cpp b/src/tools/periodic-call.cpp
index f39c4698..97908081 100644
--- a/src/tools/periodic-call.cpp
+++ b/src/tools/periodic-call.cpp
@@ -12,10 +12,11 @@
/* --------------------------------------------------------------------- */
/* --- SOT --- */
-#include <algorithm>
#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/exception-factory.h>
#include <dynamic-graph/pool.h>
+
+#include <algorithm>
#include <sot/core/debug.hh>
#include <sot/core/exception-tools.hh>
#include <sot/core/periodic-call.hh>
diff --git a/src/tools/robot-simu.cpp b/src/tools/robot-simu.cpp
index a456953d..4d3edd00 100644
--- a/src/tools/robot-simu.cpp
+++ b/src/tools/robot-simu.cpp
@@ -7,6 +7,7 @@
*/
#include "sot/core/robot-simu.hh"
+
#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>
@@ -18,22 +19,24 @@ RobotSimu::RobotSimu(const std::string &inName) : Device(inName) {
using namespace dynamicgraph::command;
std::string docstring;
/* Command increment. */
- docstring = "\n"
- " Integrate dynamics for time step provided as input\n"
- "\n"
- " take one floating point number as input\n"
- "\n";
+ docstring =
+ "\n"
+ " Integrate dynamics for time step provided as input\n"
+ "\n"
+ " take one floating point number as input\n"
+ "\n";
addCommand("increment", command::makeCommandVoid1(
(Device &)*this, &Device::increment, docstring));
/* Set Time step. */
- docstring = "\n"
- " Set the time step provided\n"
- "\n"
- " take one floating point number as input\n"
- "\n";
+ docstring =
+ "\n"
+ " Set the time step provided\n"
+ "\n"
+ " take one floating point number as input\n"
+ "\n";
addCommand("setTimeStep",
makeDirectSetter(*this, &this->timestep_, docstring));
}
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
diff --git a/src/tools/robot-utils-py.cpp b/src/tools/robot-utils-py.cpp
index a646a33c..d4065b1f 100644
--- a/src/tools/robot-utils-py.cpp
+++ b/src/tools/robot-utils-py.cpp
@@ -11,7 +11,6 @@
#include <boost/property_tree/ptree.hpp>
#include <boost/python.hpp>
#include <boost/python/suite/indexing/map_indexing_suite.hpp>
-
#include <sot/core/robot-utils.hh>
using namespace boost::python;
diff --git a/src/tools/robot-utils.cpp b/src/tools/robot-utils.cpp
index c7701e79..f6288558 100644
--- a/src/tools/robot-utils.cpp
+++ b/src/tools/robot-utils.cpp
@@ -24,6 +24,7 @@
#endif
#include <dynamic-graph/factory.h>
+
#include <iostream>
#include <sot/core/debug.hh>
#include <sot/core/robot-utils.hh>
@@ -68,10 +69,8 @@ void ExtractJointMimics::go_through(pt::ptree &pt, int level, int stage) {
if (pt.empty()) {
/// and this is a name of a joint (stage == 3) update the
/// curret_joint_name_ variable.
- if (stage == 3)
- current_joint_name_ = pt.data();
+ if (stage == 3) current_joint_name_ = pt.data();
} else {
-
/// This is not a leaf
for (auto pos : pt) {
int new_stage = stage;
@@ -83,14 +82,12 @@ void ExtractJointMimics::go_through(pt::ptree &pt, int level, int stage) {
else if (pos.first == "<xmlattr>") {
/// we are exploring the xml attributes of a joint
/// -> continue the exploration
- if (stage == 1)
- new_stage = 2;
+ if (stage == 1) new_stage = 2;
}
/// The xml attribute of the joint is the name
/// next leaf is the name we are possibly looking for
else if (pos.first == "name") {
- if (stage == 2)
- new_stage = 3;
+ if (stage == 2) new_stage = 3;
}
/// The exploration of the tree tracback on the joint
/// and find that this is a mimic joint.
@@ -157,8 +154,7 @@ void ForceUtil::set_force_id_to_limits(const Index &force_id,
Index ForceUtil::get_id_from_name(const std::string &name) {
std::map<std::string, Index>::const_iterator it;
it = m_name_to_force_id.find(name);
- if (it != m_name_to_force_id.end())
- return it->second;
+ if (it != m_name_to_force_id.end()) return it->second;
return -1;
}
@@ -168,8 +164,7 @@ std::string joint_default_rtn("Joint name not found");
const std::string &ForceUtil::get_name_from_id(Index idx) {
std::map<Index, std::string>::const_iterator it;
it = m_force_id_to_name.find(idx);
- if (it != m_force_id_to_name.end())
- return it->second;
+ if (it != m_force_id_to_name.end()) return it->second;
return force_default_rtn;
}
@@ -187,7 +182,7 @@ const ForceLimits &ForceUtil::get_limits_from_id(Index force_id) {
std::map<Index, ForceLimits>::const_iterator iter =
m_force_id_to_limits.find(force_id);
if (iter == m_force_id_to_limits.end())
- return VoidForceLimits; // Returns void instance
+ return VoidForceLimits; // Returns void instance
return iter->second;
}
@@ -195,7 +190,7 @@ ForceLimits ForceUtil::cp_get_limits_from_id(Index force_id) {
std::map<Index, ForceLimits>::const_iterator iter =
m_force_id_to_limits.find(force_id);
if (iter == m_force_id_to_limits.end())
- return VoidForceLimits; // Returns void instance
+ return VoidForceLimits; // Returns void instance
return iter->second;
}
@@ -240,8 +235,7 @@ void RobotUtil::set_joint_limits_for_id(const Index &idx, const double &lq,
const JointLimits &RobotUtil::get_joint_limits_from_id(Index id) {
std::map<Index, JointLimits>::const_iterator iter = m_limits_map.find(id);
- if (iter == m_limits_map.end())
- return VoidJointLimits;
+ if (iter == m_limits_map.end()) return VoidJointLimits;
return iter->second;
}
JointLimits RobotUtil::cp_get_joint_limits_from_id(Index id) {
@@ -263,15 +257,13 @@ void RobotUtil::create_id_to_name_map() {
const Index &RobotUtil::get_id_from_name(const std::string &name) {
std::map<std::string, Index>::const_iterator it = m_name_to_id.find(name);
- if (it == m_name_to_id.end())
- return VoidIndex;
+ if (it == m_name_to_id.end()) return VoidIndex;
return it->second;
}
const std::string &RobotUtil::get_name_from_id(Index id) {
std::map<Index, std::string>::const_iterator iter = m_id_to_name.find(id);
- if (iter == m_id_to_name.end())
- return joint_default_rtn;
+ if (iter == m_id_to_name.end()) return joint_default_rtn;
return iter->second;
}
@@ -386,7 +378,7 @@ bool RobotUtil::base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf) {
const Eigen::AngleAxisd yawAngle(y, Eigen::Vector3d::UnitZ());
const Eigen::Quaternion<double> quat = yawAngle * pitchAngle * rollAngle;
- q_urdf[0] = q_sot[0]; // BASE
+ q_urdf[0] = q_sot[0]; // BASE
q_urdf[1] = q_sot[1];
q_urdf[2] = q_sot[2];
q_urdf[3] = quat.x();
@@ -499,7 +491,7 @@ bool base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf) {
const Eigen::AngleAxisd yawAngle(y, Eigen::Vector3d::UnitZ());
const Eigen::Quaternion<double> quat = yawAngle * pitchAngle * rollAngle;
- q_urdf[0] = q_sot[0]; // BASE
+ q_urdf[0] = q_sot[0]; // BASE
q_urdf[1] = q_sot[1];
q_urdf[2] = q_sot[2];
q_urdf[3] = quat.x();
@@ -529,21 +521,18 @@ std::shared_ptr<std::vector<std::string> > getListOfRobots() {
RobotUtilShrPtr getRobotUtil(std::string &robotName) {
std::map<std::string, RobotUtilShrPtr>::iterator it =
sgl_map_name_to_robot_util.find(robotName);
- if (it != sgl_map_name_to_robot_util.end())
- return it->second;
+ if (it != sgl_map_name_to_robot_util.end()) return it->second;
return RefVoidRobotUtil();
}
bool isNameInRobotUtil(std::string &robotName) {
std::map<std::string, RobotUtilShrPtr>::iterator it =
sgl_map_name_to_robot_util.find(robotName);
- if (it != sgl_map_name_to_robot_util.end())
- return true;
+ if (it != sgl_map_name_to_robot_util.end()) return true;
return false;
}
RobotUtilShrPtr createRobotUtil(std::string &robotName) {
-
std::map<std::string, RobotUtilShrPtr>::iterator it =
sgl_map_name_to_robot_util.find(robotName);
if (it == sgl_map_name_to_robot_util.end()) {
@@ -553,5 +542,5 @@ RobotUtilShrPtr createRobotUtil(std::string &robotName) {
}
return RefVoidRobotUtil();
}
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
diff --git a/src/tools/sequencer.cpp b/src/tools/sequencer.cpp
index ececc599..14de3024 100644
--- a/src/tools/sequencer.cpp
+++ b/src/tools/sequencer.cpp
@@ -9,6 +9,7 @@
#include <dynamic-graph/factory.h>
#include <dynamic-graph/pool.h>
+
#include <sot/core/debug.hh>
#include <sot/core/exception-tools.hh>
#include <sot/core/sequencer.hh>
@@ -20,7 +21,10 @@ using namespace dynamicgraph;
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Sequencer, "Sequencer");
Sequencer::Sequencer(const std::string &name)
- : Entity(name), timeInit(-1), playMode(false), outputStreamPtr(NULL),
+ : Entity(name),
+ timeInit(-1),
+ playMode(false),
+ outputStreamPtr(NULL),
noOutput(false),
triggerSOUT(boost::bind(&Sequencer::trigger, this, _1, _2), sotNOSIGNAL,
"Sequencer(" + name + ")::output(dummy)::trigger") {
@@ -44,11 +48,11 @@ Sequencer::~Sequencer(void) {
/* --- SPECIFIC EVENT ------------------------------------------------------- */
class sotEventTaskBased : public Sequencer::sotEventAbstract {
-protected:
+ protected:
TaskAbstract *taskPtr;
const std::string defaultTaskName;
-public:
+ public:
sotEventTaskBased(const std::string name = "", TaskAbstract *task = NULL)
: sotEventAbstract(name), taskPtr(task), defaultTaskName("NULL") {}
@@ -77,7 +81,7 @@ public:
};
class sotEventAddATask : public sotEventTaskBased {
-public:
+ public:
sotEventAddATask(const std::string name = "", TaskAbstract *task = NULL)
: sotEventTaskBased(name, task) {
eventType = EVENT_ADD;
@@ -87,8 +91,7 @@ public:
sotDEBUGIN(15);
sotDEBUG(45) << "Sot = " << sotptr << ". Task = " << taskPtr << "."
<< std::endl;
- if ((NULL != sotptr) && (NULL != taskPtr))
- sotptr->push(*taskPtr);
+ if ((NULL != sotptr) && (NULL != taskPtr)) sotptr->push(*taskPtr);
sotDEBUGOUT(15);
}
@@ -100,7 +103,7 @@ public:
};
class sotEventRemoveATask : public sotEventTaskBased {
-public:
+ public:
sotEventRemoveATask(const std::string name = "", TaskAbstract *task = NULL)
: sotEventTaskBased(name, task) {
eventType = EVENT_RM;
@@ -110,8 +113,7 @@ public:
sotDEBUGIN(15);
sotDEBUG(45) << "Sot = " << sotptr << ". Task = " << taskPtr << "."
<< std::endl;
- if ((NULL != sotptr) && (NULL != taskPtr))
- sotptr->remove(*taskPtr);
+ if ((NULL != sotptr) && (NULL != taskPtr)) sotptr->remove(*taskPtr);
sotDEBUGOUT(15);
}
@@ -123,10 +125,10 @@ public:
};
class sotEventCmd : public Sequencer::sotEventAbstract {
-protected:
+ protected:
std::string cmd;
-public:
+ public:
sotEventCmd(const std::string cmdLine = "")
: sotEventAbstract(cmdLine + "<cmd>"), cmd(cmdLine) {
eventType = EVENT_CMD;
@@ -177,7 +179,7 @@ void Sequencer::addTask(sotEventAbstract *task, const unsigned int timeSpec) {
void Sequencer::rmTask(int eventType, const std::string &name,
const unsigned int time) {
TaskMap::iterator listKey = taskMap.find(time);
- if (taskMap.end() != listKey) // the time exist
+ if (taskMap.end() != listKey) // the time exist
{
TaskList &tl = listKey->second;
for (TaskList::iterator itL = tl.begin(); itL != tl.end(); ++itL) {
@@ -188,8 +190,7 @@ void Sequencer::rmTask(int eventType, const std::string &name,
}
// remove the list if empty
- if (tl.empty())
- taskMap.erase(listKey);
+ if (tl.empty()) taskMap.erase(listKey);
}
}
@@ -213,10 +214,8 @@ void Sequencer::clearAll() {
int &Sequencer::trigger(int &dummy, const int &timeSpec) {
sotDEBUGIN(15);
- if (!playMode)
- return dummy;
- if (-1 == timeInit)
- timeInit = timeSpec;
+ if (!playMode) return dummy;
+ if (-1 == timeInit) timeInit = timeSpec;
sotDEBUG(15) << "Ref time: " << (timeSpec - timeInit) << std::endl;
TaskMap::iterator listKey = taskMap.find(timeSpec - timeInit);
@@ -245,8 +244,7 @@ int &Sequencer::trigger(int &dummy, const int &timeSpec) {
/* --- PARAMS --------------------------------------------------------------- */
void Sequencer::display(std::ostream &os) const {
- if (noOutput)
- return;
+ if (noOutput) return;
os << "Sequencer " << getName() << "(t0=" << timeInit
<< ",mode=" << ((playMode) ? "play" : "pause") << "): " << std::endl;
diff --git a/src/tools/smooth-reach.cpp b/src/tools/smooth-reach.cpp
index c19d3662..87acebe0 100644
--- a/src/tools/smooth-reach.cpp
+++ b/src/tools/smooth-reach.cpp
@@ -9,6 +9,7 @@
#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>
+
#include <sot/core/debug.hh>
#include <sot/core/smooth-reach.hh>
@@ -21,11 +22,16 @@ SmoothReach::SmoothReach(const std::string &name)
: Entity(name)
,
- start(0u), goal(0u), startTime(-1), lengthTime(-1), isStarted(false),
+ start(0u),
+ goal(0u),
+ startTime(-1),
+ lengthTime(-1),
+ isStarted(false),
isParam(true)
,
- smoothMode(2), smoothParam(1.2)
+ smoothMode(2),
+ smoothParam(1.2)
,
startSIN(NULL, "SmoothReach(" + name + ")::input(vector)::start"),
@@ -54,19 +60,18 @@ void SmoothReach::initCommands(void) {
}
double SmoothReach::smoothFunction(double x) {
-
switch (smoothMode) {
- case 0:
- return x;
-
- case 1: {
- // const double smoothParam = 0.45;
- return tanh(-smoothParam / x + smoothParam / (1 - x)) / 2 + 0.5;
- }
- case 2: {
- // const double smoothParam = 1.5;
- return atan(-smoothParam / x + smoothParam / (1 - x)) / M_PI + 0.5;
- }
+ case 0:
+ return x;
+
+ case 1: {
+ // const double smoothParam = 0.45;
+ return tanh(-smoothParam / x + smoothParam / (1 - x)) / 2 + 0.5;
+ }
+ case 2: {
+ // const double smoothParam = 1.5;
+ return atan(-smoothParam / x + smoothParam / (1 - x)) / M_PI + 0.5;
+ }
}
return 0;
}
@@ -89,8 +94,7 @@ dynamicgraph::Vector &SmoothReach::goalSOUT_function(dynamicgraph::Vector &res,
if (isStarted) {
double x = double(time - startTime) / lengthTime;
- if (x > 1)
- x = 1;
+ if (x > 1) x = 1;
double x1 = smoothFunction(x);
double x0 = 1 - x1;
res = start * x0 + goal * x1;
diff --git a/src/tools/sot-loader.cpp b/src/tools/sot-loader.cpp
index 27ca453a..e94a3646 100644
--- a/src/tools/sot-loader.cpp
+++ b/src/tools/sot-loader.cpp
@@ -104,18 +104,21 @@ bool SotLoader::initialization() {
// python interpreter.
runPythonCommand("import sys, os", result, out, err);
runPythonCommand("print(\"python version:\", sys.version)", result, out, err);
- runPythonCommand("pythonpath = os.environ.get('PYTHONPATH', '')", result, out, err);
+ runPythonCommand("pythonpath = os.environ.get('PYTHONPATH', '')", result, out,
+ err);
runPythonCommand("path = []", result, out, err);
- runPythonCommand("for p in pythonpath.split(':'):\n"
- " if p not in sys.path:\n"
- " path.append(p)",
- result, out, err);
+ runPythonCommand(
+ "for p in pythonpath.split(':'):\n"
+ " if p not in sys.path:\n"
+ " path.append(p)",
+ result, out, err);
runPythonCommand("path.extend(sys.path)", result, out, err);
runPythonCommand("sys.path = path", result, out, err);
// used to be able to invoke rospy
- runPythonCommand("if not hasattr(sys, \'argv\'):\n"
- " sys.argv = ['sot']",
- result, out, err);
+ runPythonCommand(
+ "if not hasattr(sys, \'argv\'):\n"
+ " sys.argv = ['sot']",
+ result, out, err);
// help setting signals
runPythonCommand("import numpy as np", result, out, err);
// Debug print.
diff --git a/src/tools/switch-python-module-py.cc b/src/tools/switch-python-module-py.cc
index 0c9f87c8..fbc6e83e 100644
--- a/src/tools/switch-python-module-py.cc
+++ b/src/tools/switch-python-module-py.cc
@@ -1,11 +1,13 @@
-#include "dynamic-graph/python/module.hh"
#include <sot/core/switch.hh>
+#include "dynamic-graph/python/module.hh"
+
namespace dg = dynamicgraph;
typedef bp::return_value_policy<bp::reference_existing_object>
reference_existing_object;
-template <typename T, typename Time> void exposeSwitch() {
+template <typename T, typename Time>
+void exposeSwitch() {
typedef dg::sot::Switch<T, Time> E_t;
typedef typename E_t::Base B_t;
dg::python::exposeEntity<E_t, bp::bases<dg::Entity>,
diff --git a/src/tools/switch.cpp b/src/tools/switch.cpp
index f77aba0a..2822a8e3 100644
--- a/src/tools/switch.cpp
+++ b/src/tools/switch.cpp
@@ -1,10 +1,10 @@
// Copyright (c) 2017, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
-#include <sot/core/switch.hh>
-
#include <dynamic-graph/factory.h>
+#include <sot/core/switch.hh>
+
#include "type-name-helper.hh"
namespace dynamicgraph {
@@ -21,12 +21,14 @@ std::string VariadicAbstract<Tin, Tout, Time>::getTypeOutName(void) {
template class VariadicAbstract<Vector, Vector, int>;
typedef Switch<Vector, int> SwitchVector;
-template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchVector, "SwitchVector");
+template <>
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchVector, "SwitchVector");
template class VariadicAbstract<bool, bool, int>;
typedef Switch<bool, int> SwitchBool;
-template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchBool, "SwitchBoolean");
+template <>
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchBool, "SwitchBoolean");
template class VariadicAbstract<MatrixHomogeneous, MatrixHomogeneous, int>;
@@ -34,5 +36,5 @@ typedef Switch<MatrixHomogeneous, int> SwitchMatrixHomogeneous;
template <>
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchMatrixHomogeneous,
"SwitchMatrixHomogeneous");
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
diff --git a/src/tools/time-stamp.cpp b/src/tools/time-stamp.cpp
index f548edcd..e80bd339 100644
--- a/src/tools/time-stamp.cpp
+++ b/src/tools/time-stamp.cpp
@@ -13,6 +13,7 @@
/* SOT */
#include <dynamic-graph/factory.h>
+
#include <sot/core/macros-signal.hh>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/time-stamp.hh>
@@ -28,7 +29,9 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TimeStamp, "TimeStamp");
/* --- CONSTRUCTION ---------------------------------------------------- */
TimeStamp::TimeStamp(const std::string &name)
- : Entity(name), offsetValue(0), offsetSet(false),
+ : Entity(name),
+ offsetValue(0),
+ offsetSet(false),
timeSOUT("TimeStamp(" + name + ")::output(vector2)::time"),
timeDoubleSOUT("TimeStamp(" + name + ")::output(double)::timeDouble"),
timeOnceSOUT(boost::bind(&TimeStamp::getTimeStamp, this, _1, _2),
@@ -68,8 +71,7 @@ dynamicgraph::Vector &TimeStamp::getTimeStamp(dynamicgraph::Vector &res,
const int & /*time*/) {
sotDEBUGIN(15);
gettimeofday(&val, NULL);
- if (res.size() != 2)
- res.resize(2);
+ if (res.size() != 2) res.resize(2);
res(0) = static_cast<double>(val.tv_sec);
res(1) = static_cast<double>(val.tv_usec);
diff --git a/src/tools/timer.cpp b/src/tools/timer.cpp
index bb70ba81..f4643b72 100644
--- a/src/tools/timer.cpp
+++ b/src/tools/timer.cpp
@@ -13,6 +13,7 @@
/* SOT */
#include <dynamic-graph/linear-algebra.h>
+
#include <sot/core/factory.hh>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/timer.hh>
@@ -25,17 +26,20 @@ using namespace dynamicgraph;
/* --------------------------------------------------------------------- */
typedef Timer<dynamicgraph::Vector> timevect;
-template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(timevect, "TimerVector");
+template <>
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(timevect, "TimerVector");
typedef Timer<dynamicgraph::Matrix> timematrix;
-template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(timematrix, "TimerMatrix");
+template <>
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(timematrix, "TimerMatrix");
typedef Timer<MatrixHomogeneous> timematrixhomo;
template <>
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(timematrixhomo, "TimerMatrixHomo");
typedef Timer<double> timedouble;
-template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(timedouble, "TimerDouble");
+template <>
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(timedouble, "TimerDouble");
/* --------------------------------------------------------------------- */
void cmdChrono(const std::string &cmdLine, std::istringstream &cmdArg,
diff --git a/src/tools/trajectory.cpp b/src/tools/trajectory.cpp
index 560cb686..3036a5de 100644
--- a/src/tools/trajectory.cpp
+++ b/src/tools/trajectory.cpp
@@ -30,7 +30,8 @@ namespace dynamicgraph {
namespace sot {
RulesJointTrajectory::RulesJointTrajectory(Trajectory &aTrajectoryToFill)
- : TrajectoryToFill_(aTrajectoryToFill), dbg_level(0),
+ : TrajectoryToFill_(aTrajectoryToFill),
+ dbg_level(0),
float_str_re("[-0-9]+\\.[0-9]*"),
// Header Regular Expression
@@ -46,16 +47,21 @@ RulesJointTrajectory::RulesJointTrajectory(Trajectory &aTrajectoryToFill)
// Point
point_value_str_re("(" + float_str_re + "+)|(?:)"),
- list_of_pv_str_re(point_value_str_re + "(\\,|\\))"), bg_pt_str_re("\\("),
- end_pt_str_re("\\)"), comma_pt_str_re("\\,\\("),
+ list_of_pv_str_re(point_value_str_re + "(\\,|\\))"),
+ bg_pt_str_re("\\("),
+ end_pt_str_re("\\)"),
+ comma_pt_str_re("\\,\\("),
// Liste of points
bg_liste_of_pts_str_re("\\,\\("),
// Reg Exps
- header_re(header_str_re), list_of_jn_re(list_of_jn_str_re),
- list_of_pv_re(list_of_pv_str_re), bg_pt_re(bg_pt_str_re),
- end_pt_re(end_pt_str_re), comma_pt_re(comma_pt_str_re),
+ header_re(header_str_re),
+ list_of_jn_re(list_of_jn_str_re),
+ list_of_pv_re(list_of_pv_str_re),
+ bg_pt_re(bg_pt_str_re),
+ end_pt_re(end_pt_str_re),
+ comma_pt_re(comma_pt_str_re),
bg_liste_of_pts_re(bg_liste_of_pts_str_re) {}
bool RulesJointTrajectory::search_exp_sub_string(
@@ -65,7 +71,6 @@ bool RulesJointTrajectory::search_exp_sub_string(
boost::match_flag_type flags = boost::match_extra;
if (boost::regex_search(text, what, e, flags)) {
-
if (dbg_level > 5) {
std::cout << "** Match found **\n Sub-Expressions:" << what.size()
<< std::endl;
@@ -83,12 +88,10 @@ bool RulesJointTrajectory::search_exp_sub_string(
return true;
}
} else {
- if (dbg_level > 5)
- std::cout << "** No Match found **\n";
+ if (dbg_level > 5) std::cout << "** No Match found **\n";
sub_text = text;
nb_failures++;
- if (nb_failures > 100)
- return false;
+ if (nb_failures > 100) return false;
}
return false;
}
@@ -136,8 +139,7 @@ void RulesJointTrajectory::parse_joint_names(
std::string sep_char;
sep_char = what[2];
- if (sep_char == ")")
- joint_names_loop = false;
+ if (sep_char == ")") joint_names_loop = false;
if (dbg_level > 5) {
std::cout << "joint_name:" << joint_name << " " << sep_char
<< std::endl;
@@ -181,8 +183,7 @@ bool RulesJointTrajectory::parse_seq(std::string &trajectory,
} else if (what.size() == 1)
sep_char = what[0];
- if (sep_char == ")")
- joint_seq_loop = false;
+ if (sep_char == ")") joint_seq_loop = false;
} else {
return true;
@@ -203,31 +204,27 @@ bool RulesJointTrajectory::parse_point(std::string &trajectory,
return false;
sub_text2 = sub_text1;
- if (!parse_seq(sub_text2, sub_text1, aJTP.positions_))
- return false;
+ if (!parse_seq(sub_text2, sub_text1, aJTP.positions_)) return false;
sub_text2 = sub_text1;
if (!search_exp_sub_string(sub_text2, what, comma_pt_re, sub_text1))
return false;
sub_text2 = sub_text1;
- if (!parse_seq(sub_text2, sub_text1, aJTP.velocities_))
- return false;
+ if (!parse_seq(sub_text2, sub_text1, aJTP.velocities_)) return false;
sub_text2 = sub_text1;
if (!search_exp_sub_string(sub_text2, what, comma_pt_re, sub_text1))
return false;
sub_text2 = sub_text1;
- if (!parse_seq(sub_text2, sub_text1, aJTP.accelerations_))
- return false;
+ if (!parse_seq(sub_text2, sub_text1, aJTP.accelerations_)) return false;
sub_text2 = sub_text1;
if (!search_exp_sub_string(sub_text2, what, comma_pt_re, sub_text1))
return false;
sub_text2 = sub_text1;
- if (!parse_seq(sub_text2, sub_text1, aJTP.efforts_))
- return false;
+ if (!parse_seq(sub_text2, sub_text1, aJTP.efforts_)) return false;
TrajectoryToFill_.points_.push_back(aJTP);
return true;
@@ -248,8 +245,7 @@ bool RulesJointTrajectory::parse_points(std::string &trajectory,
return false;
sub_text2 = sub_text1;
- if (!parse_point(sub_text2, sub_text1))
- return false;
+ if (!parse_point(sub_text2, sub_text1)) return false;
sub_text2 = sub_text1;
if (!search_exp_sub_string(sub_text2, what, end_pt_re, sub_text1))
@@ -262,8 +258,7 @@ bool RulesJointTrajectory::parse_points(std::string &trajectory,
std::string sep_char;
sep_char = what[1];
- if (sep_char == ")")
- joint_points_loop = false;
+ if (sep_char == ")") joint_points_loop = false;
} while (joint_points_loop);
@@ -320,5 +315,5 @@ void Trajectory::display(std::ostream &os) const {
}
}
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace sot
+} // namespace dynamicgraph
diff --git a/src/tools/type-name-helper.hh b/src/tools/type-name-helper.hh
index 61c6308f..22b10ba9 100644
--- a/src/tools/type-name-helper.hh
+++ b/src/tools/type-name-helper.hh
@@ -13,7 +13,8 @@
namespace dynamicgraph {
namespace sot {
-template <typename TypeRef> struct TypeNameHelper {
+template <typename TypeRef>
+struct TypeNameHelper {
static inline std::string typeName();
};
template <typename TypeRef>
@@ -21,9 +22,10 @@ inline std::string TypeNameHelper<TypeRef>::typeName() {
return "unspecified";
}
-#define ADD_KNOWN_TYPE(typeid) \
- template <> inline std::string TypeNameHelper<typeid>::typeName() { \
- return #typeid; \
+#define ADD_KNOWN_TYPE(typeid) \
+ template <> \
+ inline std::string TypeNameHelper<typeid>::typeName() { \
+ return #typeid; \
}
ADD_KNOWN_TYPE(bool)
diff --git a/src/tools/utils-windows.cpp b/src/tools/utils-windows.cpp
index aef60693..da4189a0 100644
--- a/src/tools/utils-windows.cpp
+++ b/src/tools/utils-windows.cpp
@@ -8,9 +8,8 @@
*/
#ifdef WIN32
-#include <sot/core/utils-windows.hh>
-
#include < Windows.h >
+#include <sot/core/utils-windows.hh>
#if defined(_MSC_VER) || defined(_MSC_EXTENSIONS)
#define DELTA_EPOCH_IN_MICROSECS 11644473600000000Ui64
#else
diff --git a/src/traces/reader.cpp b/src/traces/reader.cpp
index 21b18aec..def28065 100644
--- a/src/traces/reader.cpp
+++ b/src/traces/reader.cpp
@@ -12,12 +12,12 @@
/* --------------------------------------------------------------------- */
/* SOT */
-#include <sot/core/debug.hh>
-#include <sot/core/reader.hh>
-
-#include <boost/bind.hpp>
#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>
+
+#include <boost/bind.hpp>
+#include <sot/core/debug.hh>
+#include <sot/core/reader.hh>
#include <sstream>
using namespace dynamicgraph;
@@ -31,12 +31,17 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(sotReader, "Reader");
/* --------------------------------------------------------------------- */
sotReader::sotReader(const std::string n)
- : Entity(n), selectionSIN(NULL, "Reader(" + n + ")::input(flag)::selec"),
+ : Entity(n),
+ selectionSIN(NULL, "Reader(" + n + ")::input(flag)::selec"),
vectorSOUT(boost::bind(&sotReader::getNextData, this, _1, _2),
sotNOSIGNAL, "Reader(" + n + ")::vector"),
matrixSOUT(boost::bind(&sotReader::getNextMatrix, this, _1, _2),
vectorSOUT, "Reader(" + n + ")::matrix"),
- dataSet(), currentData(), iteratorSet(false), rows(0), cols(0) {
+ dataSet(),
+ currentData(),
+ iteratorSet(false),
+ rows(0),
+ cols(0) {
signalRegistration(selectionSIN << vectorSOUT << matrixSOUT);
selectionSIN = true;
vectorSOUT.setNeedUpdateFromAllChildren(true);
@@ -72,8 +77,7 @@ void sotReader::load(const string &filename) {
break;
sotDEBUG(45) << "New data = " << x << std::endl;
}
- if (newline.size() > 0)
- dataSet.push_back(newline);
+ if (newline.size() > 0) dataSet.push_back(newline);
}
sotDEBUGOUT(15);
@@ -116,14 +120,12 @@ dynamicgraph::Vector &sotReader::getNextData(dynamicgraph::Vector &res,
unsigned int dim = 0;
for (unsigned int i = 0; i < curr.size(); ++i)
- if (selection(i))
- dim++;
+ if (selection(i)) dim++;
res.resize(dim);
int cursor = 0;
for (unsigned int i = 0; i < curr.size(); ++i)
- if (selection(i))
- res(cursor++) = curr[i];
+ if (selection(i)) res(cursor++) = curr[i];
sotDEBUGOUT(15);
return res;
@@ -133,13 +135,11 @@ dynamicgraph::Matrix &sotReader::getNextMatrix(dynamicgraph::Matrix &res,
const unsigned int time) {
sotDEBUGIN(15);
const dynamicgraph::Vector &vect = vectorSOUT(time);
- if (vect.size() < rows * cols)
- return res;
+ if (vect.size() < rows * cols) return res;
res.resize(rows, cols);
for (int i = 0; i < rows; ++i)
- for (int j = 0; j < cols; ++j)
- res(i, j) = vect(i * cols + j);
+ for (int j = 0; j < cols; ++j) res(i, j) = vect(i * cols + j);
sotDEBUGOUT(15);
return res;
diff --git a/src/utils/stop-watch.cpp b/src/utils/stop-watch.cpp
index 01f4080f..0157fce9 100644
--- a/src/utils/stop-watch.cpp
+++ b/src/utils/stop-watch.cpp
@@ -28,11 +28,13 @@ WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
#include <sys/time.h>
#else
#include <Windows.h>
+
#include <iomanip>
#endif
+#include <iomanip> // std::setprecision
+
#include "sot/core/stop-watch.hh"
-#include <iomanip> // std::setprecision
using std::map;
using std::ostringstream;
@@ -42,7 +44,7 @@ using std::string;
//#define STOP_PROFILER(name) getProfiler().stop(name)
Stopwatch &getProfiler() {
- static Stopwatch s(REAL_TIME); // alternatives are CPU_TIME and REAL_TIME
+ static Stopwatch s(REAL_TIME); // alternatives are CPU_TIME and REAL_TIME
return s;
}
@@ -60,12 +62,10 @@ bool Stopwatch::performance_exists(string perf_name) {
long double Stopwatch::take_time() {
if (mode == CPU_TIME) {
-
// Use ctime
return clock();
} else if (mode == REAL_TIME) {
-
// Query operating system
#ifdef WIN32
@@ -80,8 +80,8 @@ long double Stopwatch::take_time() {
intervals.HighPart = ft.dwHighDateTime;
long double measure = intervals.QuadPart;
- measure -= 116444736000000000.0; // Convert to UNIX epoch time
- measure /= 10000000.0; // Convert to seconds
+ measure -= 116444736000000000.0; // Convert to UNIX epoch time
+ measure /= 10000000.0; // Convert to seconds
return measure;
#else
@@ -90,8 +90,8 @@ long double Stopwatch::take_time() {
gettimeofday(&tv, NULL);
long double measure = tv.tv_usec;
- measure /= 1000000.0; // Convert to seconds
- measure += tv.tv_sec; // Add seconds part
+ measure /= 1000000.0; // Convert to seconds
+ measure += tv.tv_sec; // Add seconds part
return measure;
#endif
@@ -103,8 +103,7 @@ long double Stopwatch::take_time() {
}
void Stopwatch::start(string perf_name) {
- if (!active)
- return;
+ if (!active) return;
// Just works if not already present
records_of->insert(make_pair(perf_name, PerformanceData()));
@@ -122,8 +121,7 @@ void Stopwatch::start(string perf_name) {
}
void Stopwatch::stop(string perf_name) {
- if (!active)
- return;
+ if (!active) return;
long double clock_end = take_time();
@@ -134,21 +132,18 @@ void Stopwatch::stop(string perf_name) {
PerformanceData &perf_info = records_of->find(perf_name)->second;
// check whether the performance has been reset
- if (perf_info.clock_start == 0)
- return;
+ if (perf_info.clock_start == 0) return;
perf_info.stops++;
long double lapse = clock_end - perf_info.clock_start;
- if (mode == CPU_TIME)
- lapse /= (double)CLOCKS_PER_SEC;
+ if (mode == CPU_TIME) lapse /= (double)CLOCKS_PER_SEC;
// Update last time
perf_info.last_time = lapse;
// Update min/max time
- if (lapse >= perf_info.max_time)
- perf_info.max_time = lapse;
+ if (lapse >= perf_info.max_time) perf_info.max_time = lapse;
if (lapse <= perf_info.min_time || perf_info.min_time == 0)
perf_info.min_time = lapse;
@@ -157,8 +152,7 @@ void Stopwatch::stop(string perf_name) {
}
void Stopwatch::pause(string perf_name) {
- if (!active)
- return;
+ if (!active) return;
long double clock_end = clock();
@@ -169,8 +163,7 @@ void Stopwatch::pause(string perf_name) {
PerformanceData &perf_info = records_of->find(perf_name)->second;
// check whether the performance has been reset
- if (perf_info.clock_start == 0)
- return;
+ if (perf_info.clock_start == 0) return;
long double lapse = clock_end - perf_info.clock_start;
@@ -180,8 +173,7 @@ void Stopwatch::pause(string perf_name) {
}
void Stopwatch::reset_all() {
- if (!active)
- return;
+ if (!active) return;
map<string, PerformanceData>::iterator it;
@@ -191,8 +183,7 @@ void Stopwatch::reset_all() {
}
void Stopwatch::report_all(int precision, std::ostream &output) {
- if (!active)
- return;
+ if (!active) return;
output << "\n*** PROFILING RESULTS [ms] (min - avg - max - lastTime - "
"nSamples - totalTime) ***\n";
@@ -203,8 +194,7 @@ void Stopwatch::report_all(int precision, std::ostream &output) {
}
void Stopwatch::reset(string perf_name) {
- if (!active)
- return;
+ if (!active) return;
// Try to recover performance data
if (!performance_exists(perf_name))
@@ -232,8 +222,7 @@ void Stopwatch::turn_off() {
}
void Stopwatch::report(string perf_name, int precision, std::ostream &output) {
- if (!active)
- return;
+ if (!active) return;
// Try to recover performance data
if (!performance_exists(perf_name))
@@ -268,8 +257,7 @@ long double Stopwatch::get_time_so_far(string perf_name) {
long double lapse =
(take_time() - (records_of->find(perf_name)->second).clock_start);
- if (mode == CPU_TIME)
- lapse /= (double)CLOCKS_PER_SEC;
+ if (mode == CPU_TIME) lapse /= (double)CLOCKS_PER_SEC;
return lapse;
}
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 0253e94f..197a2459 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -120,7 +120,7 @@ FOREACH(path ${tests})
TARGET_LINK_LIBRARIES(${test} PRIVATE ${PROJECT_NAME}
Boost::unit_test_framework
${TEST_${test}_LIBS} ${TEST_${test}_EXT_LIBS})
-
+
# add some preprocessor variable
TARGET_COMPILE_DEFINITIONS(
${test}
diff --git a/tests/control/test_control_admittance.cpp b/tests/control/test_control_admittance.cpp
index 9e1e1b5d..198225f2 100644
--- a/tests/control/test_control_admittance.cpp
+++ b/tests/control/test_control_admittance.cpp
@@ -16,6 +16,7 @@ using namespace std;
#include <dynamic-graph/entity.h>
#include <dynamic-graph/factory.h>
+
#include <sot/core/admittance-control-op-point.hh>
#include <sstream>
diff --git a/tests/control/test_control_pd.cpp b/tests/control/test_control_pd.cpp
index 2baa3a14..087b7b01 100644
--- a/tests/control/test_control_pd.cpp
+++ b/tests/control/test_control_pd.cpp
@@ -17,6 +17,7 @@ using namespace std;
#include <dynamic-graph/entity.h>
#include <dynamic-graph/factory.h>
+
#include <sot/core/control-pd.hh>
#include <sstream>
diff --git a/tests/factory/test_factory.cpp b/tests/factory/test_factory.cpp
index 03209df7..f2376e77 100644
--- a/tests/factory/test_factory.cpp
+++ b/tests/factory/test_factory.cpp
@@ -10,16 +10,17 @@
/* -------------------------------------------------------------------------- */
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
-#include <iostream>
-#include <string>
-
-#include "../test-paths.h"
#include <dynamic-graph/entity.h>
#include <dynamic-graph/linear-algebra.h>
+
+#include <iostream>
#include <sot/core/debug.hh>
#include <sot/core/exception-feature.hh>
#include <sot/core/factory.hh>
#include <sot/core/feature-visual-point.hh>
+#include <string>
+
+#include "../test-paths.h"
using namespace std;
using namespace dynamicgraph::sot;
using namespace dg;
@@ -37,7 +38,7 @@ typedef void *sotPluginKey;
#endif
class TestFeature : public FeatureAbstract {
-public:
+ public:
TestFeature(void) : FeatureAbstract("") {}
virtual ~TestFeature(void) {}
virtual unsigned int &getDimension(unsigned int &res, int /*time*/) {
@@ -56,7 +57,6 @@ public:
};
int main() {
-
sotDEBUG(0) << "# In {" << endl;
// Entity test("");
// ExceptionFeature t2(ExceptionFeature::BAD_INIT);
diff --git a/tests/features/test_feature_generic.cpp b/tests/features/test_feature_generic.cpp
index 87651211..463feaf8 100644
--- a/tests/features/test_feature_generic.cpp
+++ b/tests/features/test_feature_generic.cpp
@@ -11,23 +11,21 @@
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
#include <iostream>
-
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/algorithm/joint-configuration.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/multibody/data.hpp>
+#include <pinocchio/multibody/liegroup/liegroup.hpp>
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/parsers/sample-models.hpp>
-#include <pinocchio/multibody/liegroup/liegroup.hpp>
-
#define BOOST_TEST_MODULE features
-#include <boost/test/unit_test.hpp>
-
-#include <Eigen/SVD>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/linear-algebra.h>
+
+#include <Eigen/SVD>
+#include <boost/test/unit_test.hpp>
#include <sot/core/debug.hh>
#include <sot/core/feature-abstract.hh>
#include <sot/core/feature-generic.hh>
@@ -47,33 +45,36 @@ typedef pinocchio::CartesianProductOperation<
typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t;
namespace internal {
-template <Representation_t representation> struct LG_t {
+template <Representation_t representation>
+struct LG_t {
typedef typename boost::mpl::if_c<representation == SE3Representation, SE3_t,
R3xSO3_t>::type type;
};
-} // namespace internal
-} // namespace sot
-} // namespace dynamicgraph
+} // namespace internal
+} // namespace sot
+} // namespace dynamicgraph
using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
-#define EIGEN_VECTOR_IS_APPROX(Va, Vb, precision) \
- BOOST_CHECK_MESSAGE((Va).isApprox(Vb, precision), \
- "check " #Va ".isApprox(" #Vb ") failed " \
- "[\n" \
- << (Va).transpose() << "\n!=\n" \
+#define EIGEN_VECTOR_IS_APPROX(Va, Vb, precision) \
+ BOOST_CHECK_MESSAGE((Va).isApprox(Vb, precision), \
+ "check " #Va ".isApprox(" #Vb \
+ ") failed " \
+ "[\n" \
+ << (Va).transpose() << "\n!=\n" \
<< (Vb).transpose() << "\n]")
-#define EIGEN_MATRIX_IS_APPROX(Va, Vb, precision) \
- BOOST_CHECK_MESSAGE((Va).isApprox(Vb, precision), \
- "check " #Va ".isApprox(" #Vb ") failed " \
- "[\n" \
- << (Va) << "\n!=\n" \
- << (Vb) << "\n]")
+#define EIGEN_MATRIX_IS_APPROX(Va, Vb, precision) \
+ BOOST_CHECK_MESSAGE((Va).isApprox(Vb, precision), "check " #Va \
+ ".isApprox(" #Vb \
+ ") failed " \
+ "[\n" \
+ << (Va) << "\n!=\n" \
+ << (Vb) << "\n]")
class FeatureTestBase {
-public:
+ public:
Task task_;
int time_;
dynamicgraph::Vector expectedTaskOutput_;
@@ -161,13 +162,15 @@ public:
};
class TestFeatureGeneric : public FeatureTestBase {
-public:
+ public:
FeatureGeneric feature_, featureDes_;
int dim_;
TestFeatureGeneric(unsigned dim, const std::string &name)
- : FeatureTestBase(dim, name), feature_("feature" + name),
- featureDes_("featureDes" + name), dim_(dim) {
+ : FeatureTestBase(dim, name),
+ feature_("feature" + name),
+ featureDes_("featureDes" + name),
+ dim_(dim) {
feature_.setReference(&featureDes_);
feature_.selectionSIN = Flags(true);
@@ -185,32 +188,32 @@ public:
double gain;
switch (time_) {
- case 0:
- BOOST_TEST_MESSAGE(" ----- Test Velocity -----");
- s.setZero();
- sd.setZero();
- vd.setZero();
- gain = 0.0;
- break;
- case 1:
- BOOST_TEST_MESSAGE(" ----- Test Position -----");
- s.setZero();
- sd.setConstant(2.);
- vd.setZero();
- gain = 1.0;
- break;
- case 2:
- BOOST_TEST_MESSAGE(" ----- Test both -----");
- s.setZero();
- sd.setConstant(2.);
- vd.setConstant(1.);
- gain = 3.0;
- break;
- default:
- s.setRandom();
- sd.setRandom();
- vd.setRandom();
- gain = 1.0;
+ case 0:
+ BOOST_TEST_MESSAGE(" ----- Test Velocity -----");
+ s.setZero();
+ sd.setZero();
+ vd.setZero();
+ gain = 0.0;
+ break;
+ case 1:
+ BOOST_TEST_MESSAGE(" ----- Test Position -----");
+ s.setZero();
+ sd.setConstant(2.);
+ vd.setZero();
+ gain = 1.0;
+ break;
+ case 2:
+ BOOST_TEST_MESSAGE(" ----- Test both -----");
+ s.setZero();
+ sd.setConstant(2.);
+ vd.setConstant(1.);
+ gain = 3.0;
+ break;
+ default:
+ s.setRandom();
+ sd.setRandom();
+ vd.setRandom();
+ gain = 1.0;
}
feature_.errorSIN = s;
@@ -275,11 +278,10 @@ BOOST_AUTO_TEST_CASE(check_value) {
TestFeatureGeneric testFeatureGeneric(dim, srobot);
- for (int i = 0; i < 10; ++i)
- testFeatureGeneric.checkValue();
+ for (int i = 0; i < 10; ++i) testFeatureGeneric.checkValue();
}
-BOOST_AUTO_TEST_SUITE_END() // feature_generic
+BOOST_AUTO_TEST_SUITE_END() // feature_generic
MatrixHomogeneous randomM() {
MatrixHomogeneous M;
@@ -300,14 +302,13 @@ Vector7 toVector(const pinocchio::SE3 &M) {
Vector toVector(const std::vector<MultiBound> &in) {
Vector out(in.size());
- for (int i = 0; i < (int)in.size(); ++i)
- out[i] = in[i].getSingleBound();
+ for (int i = 0; i < (int)in.size(); ++i) out[i] = in[i].getSingleBound();
return out;
}
template <Representation_t representation>
class TestFeaturePose : public FeatureTestBase {
-public:
+ public:
typedef typename dg::sot::internal::LG_t<representation>::type LieGroup_t;
FeaturePose<representation> feature_;
bool relative_;
@@ -317,9 +318,11 @@ public:
pinocchio::FrameIndex fa_, fb_;
TestFeaturePose(bool relative, const std::string &name)
- : FeatureTestBase(6, name), feature_("feature" + name),
- relative_(relative), data_(model_) {
- pinocchio::buildModels::humanoid(model_, true); // use freeflyer
+ : FeatureTestBase(6, name),
+ feature_("feature" + name),
+ relative_(relative),
+ data_(model_) {
+ pinocchio::buildModels::humanoid(model_, true); // use freeflyer
model_.lowerPositionLimit.head<3>().setConstant(-1.);
model_.upperPositionLimit.head<3>().setConstant(1.);
jb_ = model_.getJointId("rarm_wrist2_joint");
@@ -399,8 +402,7 @@ public:
double gain = 0;
// if (time_ % 5 != 0)
// gain = 2 * (double)rand() / RAND_MAX;
- if (time_ % 2 != 0)
- gain = 1;
+ if (time_ % 2 != 0) gain = 1;
setGain(gain);
}
@@ -600,12 +602,9 @@ template <typename TestClass>
void runTest(TestClass &runner, int N = 2)
// int N = 10)
{
- for (int i = 0; i < N; ++i)
- runner.checkValue();
- for (int i = 0; i < N; ++i)
- runner.checkJacobian();
- for (int i = 0; i < N; ++i)
- runner.checkFeedForward();
+ for (int i = 0; i < N; ++i) runner.checkValue();
+ for (int i = 0; i < N; ++i) runner.checkJacobian();
+ for (int i = 0; i < N; ++i) runner.checkFeedForward();
}
BOOST_AUTO_TEST_SUITE(feature_pose)
@@ -631,7 +630,7 @@ BOOST_AUTO_TEST_CASE(se3) {
feature_pose_absolute_tpl<SE3Representation>("SE3");
}
-BOOST_AUTO_TEST_SUITE_END() // absolute
+BOOST_AUTO_TEST_SUITE_END() // absolute
template <Representation_t representation>
void feature_pose_relative_tpl(const std::string &repr) {
@@ -653,6 +652,6 @@ BOOST_AUTO_TEST_CASE(se3) {
feature_pose_relative_tpl<SE3Representation>("SE3");
}
-BOOST_AUTO_TEST_SUITE_END() // relative
+BOOST_AUTO_TEST_SUITE_END() // relative
-BOOST_AUTO_TEST_SUITE_END() // feature_pose
+BOOST_AUTO_TEST_SUITE_END() // feature_pose
diff --git a/tests/features/test_feature_point6d.cpp b/tests/features/test_feature_point6d.cpp
index 3e66c60b..b5f6abe3 100644
--- a/tests/features/test_feature_point6d.cpp
+++ b/tests/features/test_feature_point6d.cpp
@@ -10,9 +10,9 @@
/* -------------------------------------------------------------------------- */
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
-#include <iostream>
-
#include <dynamic-graph/linear-algebra.h>
+
+#include <iostream>
#include <sot/core/debug.hh>
#include <sot/core/feature-abstract.hh>
#include <sot/core/feature-point6d.hh>
@@ -24,7 +24,7 @@ using namespace std;
using namespace dynamicgraph::sot;
class TestPoint6d {
-public:
+ public:
SOT_CORE_DISABLE_WARNING_PUSH
SOT_CORE_DISABLE_WARNING_DEPRECATED
FeaturePoint6d feature_, featureDes_;
@@ -35,8 +35,10 @@ public:
dynamicgraph::Vector manual_;
TestPoint6d(unsigned dim, std::string &name)
- : feature_("feature" + name), featureDes_("featureDes" + name),
- task_("task" + name), time_(0)
+ : feature_("feature" + name),
+ featureDes_("featureDes" + name),
+ task_("task" + name),
+ time_(0)
{
feature_.computationFrame("desired");
@@ -87,7 +89,6 @@ public:
}
int recompute() {
-
feature_.errorSOUT.recompute(time_);
feature_.errordotSOUT.recompute(time_);
task_.taskSOUT.recompute(time_);
@@ -117,16 +118,14 @@ public:
aM(2, 0) = -vd(4);
aM(2, 1) = vd(3);
aM(2, 2) = 0.0;
- for (unsigned int i = 0; i < 3; i++)
- aV(i) = sd(i, 3) - s(i, 3);
+ for (unsigned int i = 0; i < 3; i++) aV(i) = sd(i, 3) - s(i, 3);
aV = aM * aV;
/// Recompute error_th.
for (unsigned int i = 0; i < 3; i++) {
manual_[i] = -gain * (s(i, 3) - sd(i, 3)) - (aV(i) - vd(i));
- if (manual_[i] != taskTaskSOUT[i].getSingleBound())
- return -1;
+ if (manual_[i] != taskTaskSOUT[i].getSingleBound()) return -1;
}
return 0;
}
diff --git a/tests/filters/test_filter_differentiator.cpp b/tests/filters/test_filter_differentiator.cpp
index e658800f..fcbf30da 100644
--- a/tests/filters/test_filter_differentiator.cpp
+++ b/tests/filters/test_filter_differentiator.cpp
@@ -17,6 +17,7 @@ using namespace std;
#include <dynamic-graph/entity.h>
#include <dynamic-graph/factory.h>
+
#include <sot/core/filter-differentiator.hh>
#include <sstream>
@@ -58,8 +59,7 @@ BOOST_AUTO_TEST_CASE(test_filter_differentiator) {
srand(0);
dynamicgraph::Vector aVec(16);
- for (unsigned int i = 0; i < 16; i++)
- aVec(i) = (double)(i + rand() % 100);
+ for (unsigned int i = 0; i < 16; i++) aVec(i) = (double)(i + rand() % 100);
aFilterDiff->m_xSIN = aVec;
aFilterDiff->m_x_filteredSOUT.recompute(0);
output_test_stream output;
@@ -67,20 +67,21 @@ BOOST_AUTO_TEST_CASE(test_filter_differentiator) {
dynamicgraph::Vector outVec;
aFilterDiff->m_x_filteredSOUT.get(output);
- BOOST_CHECK(output.is_equal("82.5614 "
- "86.5403 "
- "78.5826 "
- "17.9049 "
- "96.4874 "
- "39.7886 "
- "91.5139 "
- "98.4769 "
- "56.6988 "
- "29.8415 "
- "71.6195 "
- "37.7992 "
- "101.461 "
- "71.6195 "
- "76.5931 "
- "40.7834"));
+ BOOST_CHECK(
+ output.is_equal("82.5614 "
+ "86.5403 "
+ "78.5826 "
+ "17.9049 "
+ "96.4874 "
+ "39.7886 "
+ "91.5139 "
+ "98.4769 "
+ "56.6988 "
+ "29.8415 "
+ "71.6195 "
+ "37.7992 "
+ "101.461 "
+ "71.6195 "
+ "76.5931 "
+ "40.7834"));
}
diff --git a/tests/filters/test_madgwick_ahrs.cpp b/tests/filters/test_madgwick_ahrs.cpp
index 82272398..d4eeeac6 100644
--- a/tests/filters/test_madgwick_ahrs.cpp
+++ b/tests/filters/test_madgwick_ahrs.cpp
@@ -17,6 +17,7 @@ using namespace std;
#include <dynamic-graph/entity.h>
#include <dynamic-graph/factory.h>
+
#include <sot/core/madgwickahrs.hh>
#include <sstream>
@@ -54,8 +55,9 @@ BOOST_AUTO_TEST_CASE(test_filter_differentiator) {
aFilter->m_imu_quatSOUT.get(output);
aFilter->m_imu_quatSOUT.get(anoss);
- BOOST_CHECK(output.is_equal("1 "
- "5.5547e-05 "
- "-5.83205e-05 "
- "0.00015"));
+ BOOST_CHECK(
+ output.is_equal("1 "
+ "5.5547e-05 "
+ "-5.83205e-05 "
+ "0.00015"));
}
diff --git a/tests/filters/test_madgwick_arhs.cpp b/tests/filters/test_madgwick_arhs.cpp
index 5c8ffdf3..e3ea72ce 100644
--- a/tests/filters/test_madgwick_arhs.cpp
+++ b/tests/filters/test_madgwick_arhs.cpp
@@ -17,6 +17,7 @@ using namespace std;
#include <dynamic-graph/entity.h>
#include <dynamic-graph/factory.h>
+
#include <sot/core/madgwickahrs.hh>
#include <sstream>
@@ -54,20 +55,21 @@ BOOST_AUTO_TEST_CASE(test_filter_differentiator) {
aFilter->m_imu_quatSOUT.get(output);
aFilter->m_imu_quatSOUT.get(anoss);
std::cout << "anoss:" << anoss << std::endl;
- BOOST_CHECK(output.is_equal("82.5614\n"
- "86.5403\n"
- "78.5826\n"
- "17.9049\n"
- "96.4874\n"
- "39.7886\n"
- "91.5139\n"
- "98.4769\n"
- "56.6988\n"
- "29.8415\n"
- "71.6195\n"
- "37.7992\n"
- "101.461\n"
- "71.6195\n"
- "76.5931\n"
- "40.7834\n"));
+ BOOST_CHECK(
+ output.is_equal("82.5614\n"
+ "86.5403\n"
+ "78.5826\n"
+ "17.9049\n"
+ "96.4874\n"
+ "39.7886\n"
+ "91.5139\n"
+ "98.4769\n"
+ "56.6988\n"
+ "29.8415\n"
+ "71.6195\n"
+ "37.7992\n"
+ "101.461\n"
+ "71.6195\n"
+ "76.5931\n"
+ "40.7834\n"));
}
diff --git a/tests/math/matrix-homogeneous.cpp b/tests/math/matrix-homogeneous.cpp
index 08e4b99e..0637a1e9 100644
--- a/tests/math/matrix-homogeneous.cpp
+++ b/tests/math/matrix-homogeneous.cpp
@@ -15,38 +15,38 @@ using boost::test_tools::output_test_stream;
namespace dg = dynamicgraph;
-#define MATRIX_BOOST_REQUIRE_CLOSE(N, M, LEFT, RIGHT, TOLERANCE) \
- for (unsigned i = 0; i < N; ++i) \
- for (unsigned j = 0; j < M; ++j) \
+#define MATRIX_BOOST_REQUIRE_CLOSE(N, M, LEFT, RIGHT, TOLERANCE) \
+ for (unsigned i = 0; i < N; ++i) \
+ for (unsigned j = 0; j < M; ++j) \
BOOST_REQUIRE_CLOSE(LEFT(i, j), RIGHT(i, j), TOLERANCE)
-#define MATRIX_4x4_BOOST_REQUIRE_CLOSE(LEFT, RIGHT, TOLERANCE) \
+#define MATRIX_4x4_BOOST_REQUIRE_CLOSE(LEFT, RIGHT, TOLERANCE) \
MATRIX_BOOST_REQUIRE_CLOSE(4, 4, LEFT, RIGHT, TOLERANCE)
-#define MATRIX_IDENTITY_4x4_REQUIRE_CLOSE(M, TOLERANCE) \
- for (unsigned i = 0; i < 4; ++i) \
- for (unsigned j = 0; j < 4; ++j) \
- if (i == j) \
- BOOST_REQUIRE_CLOSE(M(i, j), 1., TOLERANCE); \
- else \
+#define MATRIX_IDENTITY_4x4_REQUIRE_CLOSE(M, TOLERANCE) \
+ for (unsigned i = 0; i < 4; ++i) \
+ for (unsigned j = 0; j < 4; ++j) \
+ if (i == j) \
+ BOOST_REQUIRE_CLOSE(M(i, j), 1., TOLERANCE); \
+ else \
BOOST_CHECK_SMALL(M(i, j), .01 * TOLERANCE)
-#define MATRIX_HOMO_INIT(M, tx, ty, tz, roll, pitch, yaw) \
- M(0, 0) = cos(pitch) * cos(yaw); \
- M(0, 1) = sin(roll) * sin(pitch) * cos(yaw) - cos(roll) * sin(yaw); \
- M(0, 2) = cos(roll) * sin(pitch) * cos(yaw) + sin(roll) * sin(yaw); \
- M(0, 3) = tx; \
- M(1, 0) = cos(pitch) * sin(yaw); \
- M(1, 1) = sin(roll) * sin(pitch) * sin(yaw) + cos(roll) * cos(yaw); \
- M(1, 2) = cos(roll) * sin(pitch) * sin(yaw) - sin(roll) * cos(yaw); \
- M(1, 3) = ty; \
- M(2, 0) = -sin(pitch); \
- M(2, 1) = sin(roll) * cos(pitch); \
- M(2, 2) = cos(roll) * cos(pitch); \
- M(2, 3) = tz; \
- M(3, 0) = 0.; \
- M(3, 1) = 0.; \
- M(3, 2) = 0.; \
+#define MATRIX_HOMO_INIT(M, tx, ty, tz, roll, pitch, yaw) \
+ M(0, 0) = cos(pitch) * cos(yaw); \
+ M(0, 1) = sin(roll) * sin(pitch) * cos(yaw) - cos(roll) * sin(yaw); \
+ M(0, 2) = cos(roll) * sin(pitch) * cos(yaw) + sin(roll) * sin(yaw); \
+ M(0, 3) = tx; \
+ M(1, 0) = cos(pitch) * sin(yaw); \
+ M(1, 1) = sin(roll) * sin(pitch) * sin(yaw) + cos(roll) * cos(yaw); \
+ M(1, 2) = cos(roll) * sin(pitch) * sin(yaw) - sin(roll) * cos(yaw); \
+ M(1, 3) = ty; \
+ M(2, 0) = -sin(pitch); \
+ M(2, 1) = sin(roll) * cos(pitch); \
+ M(2, 2) = cos(roll) * cos(pitch); \
+ M(2, 3) = tz; \
+ M(3, 0) = 0.; \
+ M(3, 1) = 0.; \
+ M(3, 2) = 0.; \
M(3, 3) = 1.
BOOST_AUTO_TEST_CASE(product) {
diff --git a/tests/math/matrix-twist.cpp b/tests/math/matrix-twist.cpp
index 436e9de3..af0a9fce 100644
--- a/tests/math/matrix-twist.cpp
+++ b/tests/math/matrix-twist.cpp
@@ -7,24 +7,23 @@
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/output_test_stream.hpp>
#include <boost/test/unit_test.hpp>
-
#include <sot/core/matrix-geometry.hh>
using boost::test_tools::output_test_stream;
-#define MATRIX_BOOST_REQUIRE_CLOSE(N, M, LEFT, RIGHT, TOLERANCE) \
- for (unsigned i = 0; i < N; ++i) \
- for (unsigned j = 0; j < M; ++j) \
+#define MATRIX_BOOST_REQUIRE_CLOSE(N, M, LEFT, RIGHT, TOLERANCE) \
+ for (unsigned i = 0; i < N; ++i) \
+ for (unsigned j = 0; j < M; ++j) \
BOOST_REQUIRE_CLOSE(LEFT(i, j), RIGHT(i, j), TOLERANCE)
-#define MATRIX_6x6_BOOST_REQUIRE_CLOSE(LEFT, RIGHT, TOLERANCE) \
+#define MATRIX_6x6_BOOST_REQUIRE_CLOSE(LEFT, RIGHT, TOLERANCE) \
MATRIX_BOOST_REQUIRE_CLOSE(6, 6, LEFT, RIGHT, TOLERANCE)
-#define MATRIX_4x4_INIT(M, A00, A01, A02, A03, A10, A11, A12, A13, A20, A21, \
- A22, A23, A30, A31, A32, A33) \
- M(0, 0) = A00, M(0, 1) = A01, M(0, 2) = A02, M(0, 3) = A03; \
- M(1, 0) = A10, M(1, 1) = A11, M(1, 2) = A12, M(1, 3) = A13; \
- M(2, 0) = A20, M(2, 1) = A21, M(2, 2) = A22, M(2, 3) = A23; \
+#define MATRIX_4x4_INIT(M, A00, A01, A02, A03, A10, A11, A12, A13, A20, A21, \
+ A22, A23, A30, A31, A32, A33) \
+ M(0, 0) = A00, M(0, 1) = A01, M(0, 2) = A02, M(0, 3) = A03; \
+ M(1, 0) = A10, M(1, 1) = A11, M(1, 2) = A12, M(1, 3) = A13; \
+ M(2, 0) = A20, M(2, 1) = A21, M(2, 2) = A22, M(2, 3) = A23; \
M(3, 0) = A30, M(3, 1) = A31, M(3, 2) = A32, M(3, 3) = A33
#define MATRIX_6x6_INIT(M, A00, A01, A02, A03, A04, A05, A10, A11, A12, A13, \
@@ -67,8 +66,7 @@ BOOST_AUTO_TEST_CASE(constructor_trivial_identity) {
dynamicgraph::Matrix twistRef(6, 6);
for (unsigned i = 0; i < 6; ++i)
- for (unsigned j = 0; j < 6; ++j)
- twistRef(i, j) = (i == j) ? 1. : 0.;
+ for (unsigned j = 0; j < 6; ++j) twistRef(i, j) = (i == j) ? 1. : 0.;
MATRIX_6x6_BOOST_REQUIRE_CLOSE(twist, twistRef, 0.001);
}
diff --git a/tests/matrix/test_operator.cpp b/tests/matrix/test_operator.cpp
index eaa9d6f4..49201a86 100644
--- a/tests/matrix/test_operator.cpp
+++ b/tests/matrix/test_operator.cpp
@@ -1,8 +1,9 @@
/// Copyright CNRS 2019
/// author: O. Stasse
-#include "../../src/matrix/operator.cpp"
#include <iostream>
+#include "../../src/matrix/operator.cpp"
+
namespace dg = ::dynamicgraph;
using namespace dynamicgraph::sot;
@@ -26,12 +27,12 @@ BOOST_AUTO_TEST_CASE(test_vector_selecter) {
BOOST_CHECK(output.is_equal("Vector"));
output << aSelec_of_vector.getDocString();
- BOOST_CHECK(output.is_equal("Undocumented unary operator\n"
- " - input Vector\n"
- " - output Vector\n"));
+ BOOST_CHECK(
+ output.is_equal("Undocumented unary operator\n"
+ " - input Vector\n"
+ " - output Vector\n"));
dg::Vector vIn(10), vOut(10);
- for (unsigned int i = 0; i < 10; i++)
- vIn(i) = i;
+ for (unsigned int i = 0; i < 10; i++) vIn(i) = i;
aSelec_of_vector.setBounds(3, 5);
aSelec_of_vector.addBounds(7, 10);
@@ -56,9 +57,10 @@ BOOST_AUTO_TEST_CASE(test_vector_selecter) {
BOOST_CHECK(output.is_equal("Selec_of_vector"));
output << aVectorSelecter->getDocString();
- BOOST_CHECK(output.is_equal("Undocumented unary operator\n"
- " - input Vector\n"
- " - output Vector\n"));
+ BOOST_CHECK(
+ output.is_equal("Undocumented unary operator\n"
+ " - input Vector\n"
+ " - output Vector\n"));
}
BOOST_AUTO_TEST_CASE(test_vector_component) {
@@ -69,17 +71,17 @@ BOOST_AUTO_TEST_CASE(test_vector_component) {
aComponent_of_vector.setIndex(1);
dg::Vector vIn(3);
- for (unsigned int i = 0; i < 3; i++)
- vIn(i) = i;
+ for (unsigned int i = 0; i < 3; i++) vIn(i) = i;
double res;
aComponent_of_vector(vIn, res);
BOOST_CHECK(res == 1.0);
output << aComponent_of_vector.getDocString();
- BOOST_CHECK(output.is_equal("Select a component of a vector\n"
- " - input vector\n"
- " - output double"));
+ BOOST_CHECK(
+ output.is_equal("Select a component of a vector\n"
+ " - input vector\n"
+ " - output double"));
output << aComponent_of_vector.nameTypeIn();
BOOST_CHECK(output.is_equal("Vector"));
@@ -100,9 +102,10 @@ BOOST_AUTO_TEST_CASE(test_vector_component) {
BOOST_CHECK(output.is_equal("Component_of_vector"));
output << aVectorSelecter->getDocString();
- BOOST_CHECK(output.is_equal("Select a component of a vector\n"
- " - input vector\n"
- " - output double"));
+ BOOST_CHECK(
+ output.is_equal("Select a component of a vector\n"
+ " - input vector\n"
+ " - output double"));
}
BOOST_AUTO_TEST_CASE(test_matrix_selector) {
@@ -114,8 +117,7 @@ BOOST_AUTO_TEST_CASE(test_matrix_selector) {
dg::Matrix aMatrix(5, 5);
for (unsigned int i = 0; i < 5; i++)
- for (unsigned int j = 0; j < 5; j++)
- aMatrix(i, j) = i * 5 + j;
+ for (unsigned int j = 0; j < 5; j++) aMatrix(i, j) = i * 5 + j;
dg::Matrix resMatrix(2, 2);
aSelec_of_matrix(aMatrix, resMatrix);
@@ -144,24 +146,30 @@ BOOST_AUTO_TEST_CASE(test_matrix_selector) {
BOOST_AUTO_TEST_SUITE(test_rotation_conversions)
-template <typename type> type random();
-template <> VectorRollPitchYaw random<VectorRollPitchYaw>() {
+template <typename type>
+type random();
+template <>
+VectorRollPitchYaw random<VectorRollPitchYaw>() {
return VectorRollPitchYaw::Random();
}
-template <> VectorQuaternion random<VectorQuaternion>() {
+template <>
+VectorQuaternion random<VectorQuaternion>() {
return VectorQuaternion(Eigen::Vector4d::Random().normalized());
}
-template <> MatrixRotation random<MatrixRotation>() {
+template <>
+MatrixRotation random<MatrixRotation>() {
return MatrixRotation(random<VectorQuaternion>());
}
-template <> MatrixHomogeneous random<MatrixHomogeneous>() {
+template <>
+MatrixHomogeneous random<MatrixHomogeneous>() {
MatrixHomogeneous matrix_homo;
matrix_homo.translation() = Eigen::Vector3d::Random();
matrix_homo.linear() = random<MatrixRotation>();
return matrix_homo;
}
-template <typename type> bool compare(const type &a, const type &b) {
+template <typename type>
+bool compare(const type &a, const type &b) {
return a.isApprox(b);
}
template <>
@@ -170,7 +178,8 @@ bool compare<VectorQuaternion>(const VectorQuaternion &a,
return a.isApprox(b) || a.coeffs().isApprox(-b.coeffs());
}
-template <typename AtoB, typename BtoA> void test_impl() {
+template <typename AtoB, typename BtoA>
+void test_impl() {
typedef typename AtoB::Tin A;
typedef typename AtoB::Tout B;
diff --git a/tests/python/parameter_server_conf.py b/tests/python/parameter_server_conf.py
index d299e0fd..a6b70815 100644
--- a/tests/python/parameter_server_conf.py
+++ b/tests/python/parameter_server_conf.py
@@ -1,13 +1,15 @@
NJ = 2
-model_path = ['/integration_tests/openrobots/share/']
-urdfFileName = "/integration_tests/openrobots/share/" + \
- "simple_humanoid_description/urdf/" + \
- "simple_humanoid.urdf"
+model_path = ["/integration_tests/openrobots/share/"]
+urdfFileName = (
+ "/integration_tests/openrobots/share/"
+ + "simple_humanoid_description/urdf/"
+ + "simple_humanoid.urdf"
+)
ImuJointName = "imu_joint"
mapJointNameToID = {
- 'j1': 0,
- 'j2': 1,
+ "j1": 0,
+ "j2": 1,
}
mapJointLimits = {
diff --git a/tests/python/test-imports.py b/tests/python/test-imports.py
index bfd25440..d7eb0eb2 100755
--- a/tests/python/test-imports.py
+++ b/tests/python/test-imports.py
@@ -95,5 +95,5 @@ class PythonImportTest(unittest.TestCase):
self.fail(str(ie))
-if __name__ == '__main__':
+if __name__ == "__main__":
unittest.main()
diff --git a/tests/python/test-initialize-euler.py b/tests/python/test-initialize-euler.py
index 3a8e1576..6f91d0f9 100755
--- a/tests/python/test-initialize-euler.py
+++ b/tests/python/test-initialize-euler.py
@@ -10,8 +10,10 @@ class OpPointModifierTest(unittest.TestCase):
ent = ie.IntegratorEulerVectorDouble("ie")
with self.assertRaises(RuntimeError) as cm:
ent.initialize()
- self.assertEqual(str(cm.exception), 'The numerator or the denominator is empty.')
+ self.assertEqual(
+ str(cm.exception), "The numerator or the denominator is empty."
+ )
-if __name__ == '__main__':
+if __name__ == "__main__":
unittest.main()
diff --git a/tests/python/test-matrix-util.py b/tests/python/test-matrix-util.py
index 40ebdf4c..32110484 100644
--- a/tests/python/test-matrix-util.py
+++ b/tests/python/test-matrix-util.py
@@ -40,9 +40,21 @@ class MatrixUtilTest(unittest.TestCase):
def test_rotate(self):
for axis, angle, mat in [
- ('x', np.pi, ((1, 0, 0, 0), (0, -1, 0, 0), (0, 0, -1, 0), (0, 0, 0, 1))),
- ('y', np.pi, ((-1, 0, 0, 0), (0, 1, 0, 0), (0, 0, -1, 0), (0, 0, 0, 1))),
- ('z', np.pi, ((-1, 0, 0, 0), (0, -1, 0, 0), (0, 0, 1, 0), (0, 0, 0, 1))),
+ (
+ "x",
+ np.pi,
+ ((1, 0, 0, 0), (0, -1, 0, 0), (0, 0, -1, 0), (0, 0, 0, 1)),
+ ),
+ (
+ "y",
+ np.pi,
+ ((-1, 0, 0, 0), (0, 1, 0, 0), (0, 0, -1, 0), (0, 0, 0, 1)),
+ ),
+ (
+ "z",
+ np.pi,
+ ((-1, 0, 0, 0), (0, -1, 0, 0), (0, 0, 1, 0), (0, 0, 0, 1)),
+ ),
]:
self.assertEqual(mat, mod.rotate(axis, angle))
@@ -55,5 +67,5 @@ class MatrixUtilTest(unittest.TestCase):
self.assertEqual(mat, mod.quaternionToMatrix(quat))
-if __name__ == '__main__':
+if __name__ == "__main__":
unittest.main()
diff --git a/tests/python/test-op-point-modifier.py b/tests/python/test-op-point-modifier.py
index ce0a41bd..640815ac 100755
--- a/tests/python/test-op-point-modifier.py
+++ b/tests/python/test-op-point-modifier.py
@@ -7,21 +7,54 @@ import unittest
import numpy as np
from dynamic_graph.sot.core.op_point_modifier import OpPointModifier
-gaze = np.array((((1.0, 0.0, 0.0, 0.025), (0.0, 1.0, 0.0, 0.0), (0.0, 0.0, 1.0, 0.648), (0.0, 0.0, 0.0, 1.0))))
+gaze = np.array(
+ (
+ (
+ (1.0, 0.0, 0.0, 0.025),
+ (0.0, 1.0, 0.0, 0.0),
+ (0.0, 0.0, 1.0, 0.648),
+ (0.0, 0.0, 0.0, 1.0),
+ )
+ )
+)
Jgaze = np.array(
- (((1.0, 0.0, 0.0, 0.0, 0.648, 0.0), (0.0, 1.0, 0.0, -0.648, 0.0, 0.025), (0.0, 0.0, 1.0, 0.0, -0.025, 0.0),
- (0.0, 0.0, 0.0, 1.0, 0.0, 0.0), (0.0, 0.0, 0.0, 0.0, 1.0, 0.0), (0.0, 0.0, 0.0, 0.0, 0.0, 1.0))))
-
-I4 = np.array(((1., 0., 0., 0.), (0., 1., 0., 0.), (0., 0., 1., 0.), (0., 0., 0., 1.)))
-
-I6 = np.array(((1., 0., 0., 0., 0., 0.), (0., 1., 0., 0., 0., 0.), (0., 0., 1., 0., 0., 0.), (0., 0., 0., 1., 0., 0.),
- (0., 0., 0., 0., 1., 0.), (0., 0., 0., 0., 0., 1.)))
+ (
+ (
+ (1.0, 0.0, 0.0, 0.0, 0.648, 0.0),
+ (0.0, 1.0, 0.0, -0.648, 0.0, 0.025),
+ (0.0, 0.0, 1.0, 0.0, -0.025, 0.0),
+ (0.0, 0.0, 0.0, 1.0, 0.0, 0.0),
+ (0.0, 0.0, 0.0, 0.0, 1.0, 0.0),
+ (0.0, 0.0, 0.0, 0.0, 0.0, 1.0),
+ )
+ )
+)
+
+I4 = np.array(
+ (
+ (1.0, 0.0, 0.0, 0.0),
+ (0.0, 1.0, 0.0, 0.0),
+ (0.0, 0.0, 1.0, 0.0),
+ (0.0, 0.0, 0.0, 1.0),
+ )
+)
+
+I6 = np.array(
+ (
+ (1.0, 0.0, 0.0, 0.0, 0.0, 0.0),
+ (0.0, 1.0, 0.0, 0.0, 0.0, 0.0),
+ (0.0, 0.0, 1.0, 0.0, 0.0, 0.0),
+ (0.0, 0.0, 0.0, 1.0, 0.0, 0.0),
+ (0.0, 0.0, 0.0, 0.0, 1.0, 0.0),
+ (0.0, 0.0, 0.0, 0.0, 0.0, 1.0),
+ )
+)
class OpPointModifierTest(unittest.TestCase):
def test_simple(self):
- op = OpPointModifier('op')
+ op = OpPointModifier("op")
op.setTransformation(I4)
op.positionIN.value = I4
op.jacobianIN.value = I6
@@ -33,13 +66,20 @@ class OpPointModifierTest(unittest.TestCase):
self.assertTrue((op.jacobian.value == I6).all())
def test_translation(self):
- tx = 11.
- ty = 22.
- tz = 33.
-
- T = np.array(((1., 0., 0., tx), (0., 1., 0., ty), (0., 0., 1., tz), (0., 0., 0., 1.)))
-
- op = OpPointModifier('op2')
+ tx = 11.0
+ ty = 22.0
+ tz = 33.0
+
+ T = np.array(
+ (
+ (1.0, 0.0, 0.0, tx),
+ (0.0, 1.0, 0.0, ty),
+ (0.0, 0.0, 1.0, tz),
+ (0.0, 0.0, 0.0, 1.0),
+ )
+ )
+
+ op = OpPointModifier("op2")
op.setTransformation(T)
op.positionIN.value = gaze
op.jacobianIN.value = Jgaze
@@ -57,8 +97,16 @@ class OpPointModifierTest(unittest.TestCase):
# Check w_M_s == w_M_s_ref
self.assertTrue((w_M_s == w_M_s_ref).all())
- twist = np.array([[1., 0., 0., 0., tz, -ty], [0., 1., 0., -tz, 0., tx], [0., 0., 1., ty, -tx, 0.],
- [0., 0., 0., 1., 0., 0.], [0., 0., 0., 0., 1., 0.], [0., 0., 0., 0., 0., 1.]])
+ twist = np.array(
+ [
+ [1.0, 0.0, 0.0, 0.0, tz, -ty],
+ [0.0, 1.0, 0.0, -tz, 0.0, tx],
+ [0.0, 0.0, 1.0, ty, -tx, 0.0],
+ [0.0, 0.0, 0.0, 1.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 1.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
+ ]
+ )
J = op.jacobian.value
J_ref = twist.dot(Jgaze)
@@ -67,9 +115,16 @@ class OpPointModifierTest(unittest.TestCase):
self.assertTrue((J == J_ref).all())
def test_rotation(self):
- T = np.array(((0., 0., 1., 0.), (0., -1., 0., 0.), (1., 0., 0., 0.), (0., 0., 0., 1.)))
-
- op = OpPointModifier('op3')
+ T = np.array(
+ (
+ (0.0, 0.0, 1.0, 0.0),
+ (0.0, -1.0, 0.0, 0.0),
+ (1.0, 0.0, 0.0, 0.0),
+ (0.0, 0.0, 0.0, 1.0),
+ )
+ )
+
+ op = OpPointModifier("op3")
op.setTransformation(T)
op.positionIN.value = gaze
op.jacobianIN.value = Jgaze
@@ -87,8 +142,16 @@ class OpPointModifierTest(unittest.TestCase):
# Check w_M_s == w_M_s_ref
self.assertTrue((w_M_s == w_M_s_ref).all())
- twist = np.array([[0., 0., 1., 0., 0., 0.], [0., -1., 0., 0., 0., 0.], [1., 0., 0., 0., 0., 0.],
- [0., 0., 0., 0., 0., 1.], [0., 0., 0., 0., -1., 0.], [0., 0., 0., 1., 0., 0.]])
+ twist = np.array(
+ [
+ [0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
+ [0.0, -1.0, 0.0, 0.0, 0.0, 0.0],
+ [1.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
+ [0.0, 0.0, 0.0, 0.0, -1.0, 0.0],
+ [0.0, 0.0, 0.0, 1.0, 0.0, 0.0],
+ ]
+ )
J = op.jacobian.value
J_ref = twist.dot(Jgaze)
@@ -97,13 +160,20 @@ class OpPointModifierTest(unittest.TestCase):
self.assertTrue((J == J_ref).all())
def test_rotation_translation(self):
- tx = 11.
- ty = 22.
- tz = 33.
-
- T = np.array(((0., 0., 1., tx), (0., -1., 0., ty), (1., 0., 0., tz), (0., 0., 0., 1.)))
-
- op = OpPointModifier('op4')
+ tx = 11.0
+ ty = 22.0
+ tz = 33.0
+
+ T = np.array(
+ (
+ (0.0, 0.0, 1.0, tx),
+ (0.0, -1.0, 0.0, ty),
+ (1.0, 0.0, 0.0, tz),
+ (0.0, 0.0, 0.0, 1.0),
+ )
+ )
+
+ op = OpPointModifier("op4")
op.setTransformation(T)
op.positionIN.value = gaze
op.jacobianIN.value = Jgaze
@@ -121,8 +191,16 @@ class OpPointModifierTest(unittest.TestCase):
# Check w_M_s == w_M_s_ref
self.assertTrue((w_M_s == w_M_s_ref).all())
- twist = np.array([[0., 0., 1., ty, -tx, 0.], [0., -1., 0., tz, 0., -tx], [1., 0., 0., 0., tz, -ty],
- [0., 0., 0., 0., 0., 1.], [0., 0., 0., 0., -1., 0.], [0., 0., 0., 1., 0., 0.]])
+ twist = np.array(
+ [
+ [0.0, 0.0, 1.0, ty, -tx, 0.0],
+ [0.0, -1.0, 0.0, tz, 0.0, -tx],
+ [1.0, 0.0, 0.0, 0.0, tz, -ty],
+ [0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
+ [0.0, 0.0, 0.0, 0.0, -1.0, 0.0],
+ [0.0, 0.0, 0.0, 1.0, 0.0, 0.0],
+ ]
+ )
J = op.jacobian.value
J_ref = twist.dot(Jgaze)
@@ -131,5 +209,5 @@ class OpPointModifierTest(unittest.TestCase):
self.assertTrue((J == J_ref).all())
-if __name__ == '__main__':
+if __name__ == "__main__":
unittest.main()
diff --git a/tests/python/test-parameter-server.py b/tests/python/test-parameter-server.py
index 695b5a21..728a2168 100644
--- a/tests/python/test-parameter-server.py
+++ b/tests/python/test-parameter-server.py
@@ -12,7 +12,7 @@ param_server.init(0.001, "talos.urdf", "talos")
# Control time interval
dt = 0.001
-robot_name = 'robot'
+robot_name = "robot"
urdfPath = param_server_conf.urdfFileName
urdfDir = param_server_conf.model_path
@@ -21,10 +21,10 @@ urdfDir = param_server_conf.model_path
class TestParameterServer(unittest.TestCase):
def test_set_parameter(self):
# Read talos model
- path = join(dirname(dirname(abspath(__file__))), 'models', 'others', 'python')
+ path = join(dirname(dirname(abspath(__file__))), "models", "others", "python")
sys.path.append(path)
- _, _, urdf_file_name, _ = load_full('talos')
+ _, _, urdf_file_name, _ = load_full("talos")
with open(urdf_file_name) as fs:
urdf_rrbot_model_string = fs.read()
@@ -39,5 +39,5 @@ class TestParameterServer(unittest.TestCase):
self.assertEqual(aValue, a2Value)
-if __name__ == '__main__':
+if __name__ == "__main__":
unittest.main()
diff --git a/tests/python/test-smooth-reach.py b/tests/python/test-smooth-reach.py
index f2a23eb9..29a74136 100755
--- a/tests/python/test-smooth-reach.py
+++ b/tests/python/test-smooth-reach.py
@@ -11,5 +11,5 @@ class SmoothReachTest(unittest.TestCase):
self.assertIn("output(vector)::goal (Type Fun)", str(sr.goal))
-if __name__ == '__main__':
+if __name__ == "__main__":
unittest.main()
diff --git a/tests/signal/test_dep.cpp b/tests/signal/test_dep.cpp
index 9d863c26..5cb1b5b7 100644
--- a/tests/signal/test_dep.cpp
+++ b/tests/signal/test_dep.cpp
@@ -12,13 +12,14 @@
/* -------------------------------------------------------------------------- */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/linear-algebra.h>
+
#include <iostream>
using namespace std;
using namespace dynamicgraph;
-template <class Res = double> class DummyClass {
-
-public:
+template <class Res = double>
+class DummyClass {
+ public:
DummyClass(void) : res(), appel(0), timedata(0) {}
Res &fun(Res &res, int t) {
@@ -57,9 +58,13 @@ public:
int timedata;
};
-template <class Res> Res DummyClass<Res>::operator()(void) { return this->res; }
+template <class Res>
+Res DummyClass<Res>::operator()(void) {
+ return this->res;
+}
-template <> double DummyClass<double>::operator()(void) {
+template <>
+double DummyClass<double>::operator()(void) {
res = appel * timedata;
return res;
}
diff --git a/tests/signal/test_depend.cpp b/tests/signal/test_depend.cpp
index 2d2bef0f..188433bd 100644
--- a/tests/signal/test_depend.cpp
+++ b/tests/signal/test_depend.cpp
@@ -12,20 +12,21 @@
/* -------------------------------------------------------------------------- */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/linear-algebra.h>
+
#include <iostream>
#include <sot/core/debug.hh>
using namespace std;
using namespace dynamicgraph;
-template <class Res = double> class DummyClass {
-
-public:
+template <class Res = double>
+class DummyClass {
+ public:
std::string proname;
list<SignalTimeDependent<double, int> *> inputsig;
list<SignalTimeDependent<dynamicgraph::Vector, int> *> inputsigV;
-public:
+ public:
DummyClass(const std::string &n) : proname(n), res(), appel(0), timedata(0) {}
Res &fun(Res &res, int t) {
@@ -61,9 +62,13 @@ public:
int timedata;
};
-template <class Res> Res DummyClass<Res>::operator()(void) { return this->res; }
+template <class Res>
+Res DummyClass<Res>::operator()(void) {
+ return this->res;
+}
-template <> double DummyClass<double>::operator()(void) {
+template <>
+double DummyClass<double>::operator()(void) {
res = appel * timedata;
return res;
}
diff --git a/tests/signal/test_ptr.cpp b/tests/signal/test_ptr.cpp
index a6a17cd7..16bb21c8 100644
--- a/tests/signal/test_ptr.cpp
+++ b/tests/signal/test_ptr.cpp
@@ -12,6 +12,7 @@
/* -------------------------------------------------------------------------- */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/linear-algebra.h>
+
#include <iostream>
#include <sot/core/debug.hh>
#include <sot/core/exception-abstract.hh>
@@ -21,9 +22,9 @@ using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
-template <class Res = double> class DummyClass {
-
-public:
+template <class Res = double>
+class DummyClass {
+ public:
DummyClass(void) : res(), appel(0), timedata(0) {}
Res &fun(Res &res, int t) {
@@ -62,9 +63,13 @@ public:
int timedata;
};
-template <class Res> Res DummyClass<Res>::operator()(void) { return this->res; }
+template <class Res>
+Res DummyClass<Res>::operator()(void) {
+ return this->res;
+}
-template <> double DummyClass<double>::operator()(void) {
+template <>
+double DummyClass<double>::operator()(void) {
res = appel * timedata;
return res;
}
@@ -74,7 +79,8 @@ dynamicgraph::Vector DummyClass<dynamicgraph::Vector>::operator()(void) {
res.fill(appel * timedata);
return res;
}
-template <> VectorUTheta DummyClass<VectorUTheta>::operator()(void) {
+template <>
+VectorUTheta DummyClass<VectorUTheta>::operator()(void) {
res.angle() = 0.26;
res.axis() = Eigen::Vector3d::UnitX();
return res;
diff --git a/tests/signal/test_ptrcast.cpp b/tests/signal/test_ptrcast.cpp
index e64ef95c..bc6f891a 100644
--- a/tests/signal/test_ptrcast.cpp
+++ b/tests/signal/test_ptrcast.cpp
@@ -12,6 +12,7 @@
/* -------------------------------------------------------------------------- */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/linear-algebra.h>
+
#include <iostream>
#include <sot/core/matrix-geometry.hh>
using namespace std;
diff --git a/tests/signal/test_signal.cpp b/tests/signal/test_signal.cpp
index 6a15ef2f..334ece24 100644
--- a/tests/signal/test_signal.cpp
+++ b/tests/signal/test_signal.cpp
@@ -12,13 +12,13 @@
/* -------------------------------------------------------------------------- */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/linear-algebra.h>
+
#include <iostream>
using namespace std;
using namespace dynamicgraph;
class DummyClass {
-
-public:
+ public:
dynamicgraph::Vector &fun(dynamicgraph::Vector &res, double j) {
res.resize(3);
res.fill(j);
diff --git a/tests/sot/test_solverSoth.cpp b/tests/sot/test_solverSoth.cpp
index 5f9dd228..01531189 100644
--- a/tests/sot/test_solverSoth.cpp
+++ b/tests/sot/test_solverSoth.cpp
@@ -79,8 +79,7 @@ void parseTest(const std::string filename) {
ee.resize(me);
if (me > 0)
for (int i = 0; i < me; ++i) {
- for (int j = 0; j < nJ; ++j)
- off >> Je(i, j);
+ for (int j = 0; j < nJ; ++j) off >> Je(i, j);
off >> ee(i);
}
@@ -97,8 +96,7 @@ void parseTest(const std::string filename) {
eiBoundSide.resize(mi);
if (mi > 0)
for (int i = 0; i < mi; ++i) {
- for (int j = 0; j < nJ; ++j)
- off >> Ji(i, j);
+ for (int j = 0; j < nJ; ++j) off >> Ji(i, j);
std::string number;
eiBoundSide[i] = ConstraintMem::BOUND_VOID;
off >> number;
@@ -257,17 +255,14 @@ void deparse(std::vector<bubMatrix> Jes, std::vector<bubVector> ees,
<< "equalities " << ee.size() << endl;
if (ee.size() > 0)
for (unsigned int i = 0; i < ee.size(); ++i) {
- for (unsigned int j = 0; j < Je.size2(); ++j)
- cout << Je(i, j) << " ";
+ for (unsigned int j = 0; j < Je.size2(); ++j) cout << Je(i, j) << " ";
cout << "\t" << ee(i) << endl;
}
unsigned int nbIneq = 0;
for (unsigned int i = 0; i < boundSide.size(); ++i) {
- if (boundSide[i] & ConstraintMem::BOUND_INF)
- nbIneq++;
- if (boundSide[i] & ConstraintMem::BOUND_SUP)
- nbIneq++;
+ if (boundSide[i] & ConstraintMem::BOUND_INF) nbIneq++;
+ if (boundSide[i] & ConstraintMem::BOUND_SUP) nbIneq++;
}
cout << endl << "inequalities " << nbIneq << endl;
@@ -280,8 +275,7 @@ void deparse(std::vector<bubMatrix> Jes, std::vector<bubVector> ees,
<< " X " << -eiInf(i) << endl;
}
if (boundSide[i] & ConstraintMem::BOUND_SUP) {
- for (unsigned int j = 0; j < Ji.size2(); ++j)
- cout << Ji(i, j) << " ";
+ for (unsigned int j = 0; j < Ji.size2(); ++j) cout << Ji(i, j) << " ";
cout << "\t"
<< " X " << eiSup(i) << endl;
}
@@ -339,8 +333,7 @@ void convertDoubleToSingle(const std::string filename) {
ee.resize(me);
if (me > 0)
for (int i = 0; i < me; ++i) {
- for (int j = 0; j < nJ; ++j)
- off >> Je(i, j);
+ for (int j = 0; j < nJ; ++j) off >> Je(i, j);
off >> ee(i);
}
@@ -356,11 +349,10 @@ void convertDoubleToSingle(const std::string filename) {
eiBoundSide.resize(mi);
if (mi > 0)
for (int i = 0; i < mi; ++i) {
- for (int j = 0; j < nJ; ++j)
- off >> Ji(i, j);
+ for (int j = 0; j < nJ; ++j) off >> Ji(i, j);
std::string number;
eiBoundSide[i] = ConstraintMem::BOUND_VOID;
- off >> number; // std::cout << "toto '" << number << "'" << std::endl;
+ off >> number; // std::cout << "toto '" << number << "'" << std::endl;
if (number != "X") {
eiBoundSide[i] = (ConstraintMem::BoundSideType)(
eiBoundSide[i] | ConstraintMem::BOUND_INF);
@@ -426,7 +418,7 @@ void randTest(const unsigned int nJ, const bool enableSolve[]) {
sotDEBUG(15) << "eiSup1 = " << (MATLAB)eiSup1 << std::endl;
randBound(bound1, 3);
randMatrix(Je1, 3,
- nJ); // sotDEBUG(15) << "Je1 = " << (MATLAB)Je1 << std::endl;
+ nJ); // sotDEBUG(15) << "Je1 = " << (MATLAB)Je1 << std::endl;
randMatrix(Ji1, 3, nJ);
sotDEBUG(15) << "Ji1 = " << (MATLAB)Ji1 << std::endl;
bubMatrix xhi;
@@ -536,40 +528,35 @@ void randTest(const unsigned int nJ, const bool enableSolve[]) {
/* ---------------------------------------------------------- */
- if (enableSolve[0])
- solver.solve(Je0, ee0, Ji0, eiInf0, eiSup0, bound0);
+ if (enableSolve[0]) solver.solve(Je0, ee0, Ji0, eiInf0, eiSup0, bound0);
sotDEBUG(15) << "/* ----------------------------------------------------- */"
<< std::endl;
sotDEBUG(15) << "/* ----------------------------------------------------- */"
<< std::endl;
sotDEBUG(15) << "/* ----------------------------------------------------- */"
<< std::endl;
- if (enableSolve[1])
- solver.solve(Je1, ee1, Ji1, eiInf1, eiSup1, bound1);
+ if (enableSolve[1]) solver.solve(Je1, ee1, Ji1, eiInf1, eiSup1, bound1);
sotDEBUG(15) << "/* ----------------------------------------------------- */"
<< std::endl;
sotDEBUG(15) << "/* ----------------------------------------------------- */"
<< std::endl;
sotDEBUG(15) << "/* ----------------------------------------------------- */"
<< std::endl;
- if (enableSolve[2])
- solver.solve(Je2, ee2, Ji2, eiInf2, eiSup2, bound2);
+ if (enableSolve[2]) solver.solve(Je2, ee2, Ji2, eiInf2, eiSup2, bound2);
sotDEBUG(15) << "/* ----------------------------------------------------- */"
<< std::endl;
sotDEBUG(15) << "/* ----------------------------------------------------- */"
<< std::endl;
sotDEBUG(15) << "/* ----------------------------------------------------- */"
<< std::endl;
- if (enableSolve[3])
- solver.solve(Je3, ee3, Ji3, eiInf3, eiSup3, bound3);
+ if (enableSolve[3]) solver.solve(Je3, ee3, Ji3, eiInf3, eiSup3, bound3);
sotDEBUG(15) << "/* ----------------------------------------------------- */"
<< std::endl;
sotDEBUG(15) << "/* ----------------------------------------------------- */"
<< std::endl;
sotDEBUG(15) << "/* ----------------------------------------------------- */"
<< std::endl;
- if (enableSolve[4])
- solver.solve(Je4, ee4, Ji4, eiInf4, eiSup4, bound4);
+ if (enableSolve[4]) solver.solve(Je4, ee4, Ji4, eiInf4, eiSup4, bound4);
/* ---------------------------------------------------------- */
@@ -580,7 +567,6 @@ void randTest(const unsigned int nJ, const bool enableSolve[]) {
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
int main(void) {
-
// convertDoubleToSingle("/home/nmansard/src/StackOfTasks/tests/tools/testFR.txt");
// exit(0);
diff --git a/tests/sot/tsot.cpp b/tests/sot/tsot.cpp
index 6f035940..03bf9522 100644
--- a/tests/sot/tsot.cpp
+++ b/tests/sot/tsot.cpp
@@ -13,6 +13,7 @@
#include <iostream>
//#include <sot/core/sot-h.hh>
#include <dynamic-graph/linear-algebra.h>
+
#include <sot/core/debug.hh>
#include <sot/core/feature-abstract.hh>
#include <sot/core/feature-visual-point.hh>
@@ -25,8 +26,7 @@ using namespace dynamicgraph::sot;
double drand(void) { return 2 * ((double)rand()) / RAND_MAX - 1; }
dynamicgraph::Matrix &mrand(dynamicgraph::Matrix &J) {
for (int i = 0; i < J.rows(); ++i)
- for (int j = 0; j < J.cols(); ++j)
- J(i, j) = drand();
+ for (int j = 0; j < J.cols(); ++j) J(i, j) = drand();
return J;
}
diff --git a/tests/task/test_flags.cpp b/tests/task/test_flags.cpp
index f6661af5..5a26781c 100644
--- a/tests/task/test_flags.cpp
+++ b/tests/task/test_flags.cpp
@@ -68,8 +68,7 @@ int main(void) {
cout << "f1 byte per byte:";
for (int i = 0; i < 16; ++i) {
- if (!(i % 8))
- cout << " ";
+ if (!(i % 8)) cout << " ";
cout << f1(i);
}
cout << endl;
diff --git a/tests/task/test_gain.cpp b/tests/task/test_gain.cpp
index da3c7933..1e9dec6a 100644
--- a/tests/task/test_gain.cpp
+++ b/tests/task/test_gain.cpp
@@ -13,6 +13,7 @@
#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/signal.h>
+
#include <iostream>
#include <sot/core/gain-adaptive.hh>
using namespace std;
@@ -20,7 +21,7 @@ using namespace dynamicgraph::sot;
using namespace dynamicgraph;
class DummyClass {
-public:
+ public:
dynamicgraph::Vector err;
dynamicgraph::Vector &getError(dynamicgraph::Vector &res, int t) {
diff --git a/tests/task/test_task.cpp b/tests/task/test_task.cpp
index b356db5f..96d017b2 100644
--- a/tests/task/test_task.cpp
+++ b/tests/task/test_task.cpp
@@ -10,9 +10,9 @@
/* -------------------------------------------------------------------------- */
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
-#include <iostream>
-
#include <dynamic-graph/linear-algebra.h>
+
+#include <iostream>
#include <sot/core/debug.hh>
#include <sot/core/feature-abstract.hh>
#include <sot/core/feature-visual-point.hh>
@@ -25,8 +25,7 @@ using namespace dynamicgraph::sot;
double drand(void) { return 2 * ((double)rand()) / RAND_MAX - 1; }
dynamicgraph::Matrix &mrand(dynamicgraph::Matrix &J) {
for (int i = 0; i < J.rows(); ++i)
- for (int j = 0; j < J.cols(); ++j)
- J(i, j) = drand();
+ for (int j = 0; j < J.cols(); ++j) J(i, j) = drand();
return J;
}
diff --git a/tests/tools/dummy-sot-external-interface.cc b/tests/tools/dummy-sot-external-interface.cc
index 3386add6..de11b205 100644
--- a/tests/tools/dummy-sot-external-interface.cc
+++ b/tests/tools/dummy-sot-external-interface.cc
@@ -9,11 +9,11 @@
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
+#include "dummy-sot-external-interface.hh"
+
#include <iostream>
#include <sot/core/debug.hh>
-#include "dummy-sot-external-interface.hh"
-
using namespace std;
using namespace dynamicgraph::sot;
@@ -63,4 +63,4 @@ void destroySotExternalInterface(
dynamicgraph::sot::AbstractSotExternalInterface *p) {
delete p;
}
-}
\ No newline at end of file
+}
diff --git a/tests/tools/dummy-sot-external-interface.hh b/tests/tools/dummy-sot-external-interface.hh
index 8ad1ddd7..ae32ba83 100644
--- a/tests/tools/dummy-sot-external-interface.hh
+++ b/tests/tools/dummy-sot-external-interface.hh
@@ -16,7 +16,7 @@
class DummySotExternalInterface
: public dynamicgraph::sot::AbstractSotExternalInterface {
-public:
+ public:
DummySotExternalInterface(){};
virtual ~DummySotExternalInterface(){};
@@ -35,7 +35,7 @@ public:
virtual void setSecondOrderIntegration(void);
virtual void setNoIntegration(void);
-public:
+ public:
bool second_integration_;
};
diff --git a/tests/tools/plugin.cc b/tests/tools/plugin.cc
index 18db7fc5..fc20ef52 100644
--- a/tests/tools/plugin.cc
+++ b/tests/tools/plugin.cc
@@ -14,19 +14,19 @@
// POSIX.1-2001
#include <dlfcn.h>
-#include "plugin.hh"
#include <sot/core/abstract-sot-external-interface.hh>
#include <sot/core/debug.hh>
+#include "plugin.hh"
+
using namespace std;
using namespace dynamicgraph::sot;
class Plugin : public PluginAbstract {
-
-protected:
+ protected:
AbstractSotExternalInterface *sotController_;
-public:
+ public:
Plugin(){};
~Plugin(){};
diff --git a/tests/tools/plugin.hh b/tests/tools/plugin.hh
index 8c6b819c..ce5bb4e3 100644
--- a/tests/tools/plugin.hh
+++ b/tests/tools/plugin.hh
@@ -2,7 +2,7 @@
#define _PLUGIN_HH_
class PluginAbstract {
-public:
+ public:
PluginAbstract(){};
virtual ~PluginAbstract(){};
virtual void Initialization(std::string &astr) = 0;
diff --git a/tests/tools/test_abstract_interface.cpp b/tests/tools/test_abstract_interface.cpp
index 7b85553e..c9337eb1 100644
--- a/tests/tools/test_abstract_interface.cpp
+++ b/tests/tools/test_abstract_interface.cpp
@@ -15,22 +15,21 @@
#include <dlfcn.h>
#include <boost/program_options.hpp>
+#include <sot/core/debug.hh>
#include "plugin.hh"
-#include <sot/core/debug.hh>
using namespace std;
using namespace dynamicgraph::sot;
namespace po = boost::program_options;
class PluginLoader {
-
-protected:
+ protected:
PluginAbstract *sotController_;
po::variables_map vm_;
std::string dynamicLibraryName_;
-public:
+ public:
PluginLoader(){};
~PluginLoader(){};
@@ -57,7 +56,6 @@ public:
}
void Initialization() {
-
// Load the SotRobotBipedController library.
void *SotRobotControllerLibrary =
dlopen("libpluginabstract.so", RTLD_LAZY | RTLD_LOCAL);
diff --git a/tests/tools/test_boost.cpp b/tests/tools/test_boost.cpp
index 85e2e771..00888355 100644
--- a/tests/tools/test_boost.cpp
+++ b/tests/tools/test_boost.cpp
@@ -11,6 +11,7 @@
#include <sys/time.h>
#endif
#include <dynamic-graph/linear-algebra.h>
+
#include <iostream>
#include <sot/core/debug.hh>
#include <sot/core/matrix-geometry.hh>
@@ -27,12 +28,12 @@
using namespace dynamicgraph::sot;
using namespace std;
-#define INIT_CHRONO(name) \
- struct timeval t0##_##name, t1##_##name; \
+#define INIT_CHRONO(name) \
+ struct timeval t0##_##name, t1##_##name; \
double dt##_##name
-#define START_CHRONO(name) \
- gettimeofday(&t0##_##name, NULL); \
- sotDEBUG(25) << "t0 " << #name << ": " << t0##_##name.tv_sec << " - " \
+#define START_CHRONO(name) \
+ gettimeofday(&t0##_##name, NULL); \
+ sotDEBUG(25) << "t0 " << #name << ": " << t0##_##name.tv_sec << " - " \
<< t0##_##name.tv_usec << std::endl
#define STOP_CHRONO(name, commentaire) \
gettimeofday(&t1##_##name, NULL); \
@@ -124,17 +125,14 @@ double timerCounter;
/* ----------------------------------------------------------------------- */
int main(int argc, char **argv) {
- if (sotDEBUG_ENABLE(1))
- DebugTrace::openFile();
+ if (sotDEBUG_ENABLE(1)) DebugTrace::openFile();
// const unsigned int r=1;
// const unsigned int c=30;
unsigned int r = 1;
- if (argc > 1)
- r = atoi(argv[1]);
+ if (argc > 1) r = atoi(argv[1]);
unsigned int c = 30;
- if (argc > 2)
- c = atoi(argv[2]);
+ if (argc > 2) c = atoi(argv[2]);
static const int BENCH = 100;
dynamicgraph::Matrix M(r, c);
@@ -146,8 +144,7 @@ int main(int argc, char **argv) {
unsigned int nbzeros = 0;
for (unsigned int j = 0; j < c; ++j) {
if ((rand() + 1.) / RAND_MAX > .8) {
- for (unsigned int i = 0; i < r; ++i)
- M(i, j) = 0.;
+ for (unsigned int i = 0; i < r; ++i) M(i, j) = 0.;
nbzeros++;
} else
for (unsigned int i = 0; i < r; ++i)
@@ -155,7 +152,7 @@ int main(int argc, char **argv) {
}
for (unsigned int i = 0; i < r; ++i)
for (unsigned int j = 0; j < c; ++j)
- M1(i, j) = M(i, j); //+ ((rand()+1.) / RAND_MAX*2-1) * 1e-28 ;
+ M1(i, j) = M(i, j); //+ ((rand()+1.) / RAND_MAX*2-1) * 1e-28 ;
// sotDEBUG(15) << dynamicgraph::MATLAB <<"M = "<< M <<endl;
sotDEBUG(15) << "M1 = " << M1 << endl;
@@ -164,14 +161,12 @@ int main(int argc, char **argv) {
INIT_CHRONO(inv);
START_CHRONO(inv);
- for (int i = 0; i < BENCH; ++i)
- dynamicgraph::pseudoInverse(M, Minv);
+ for (int i = 0; i < BENCH; ++i) dynamicgraph::pseudoInverse(M, Minv);
STOP_CHRONO(inv, "init");
sotDEBUG(15) << "Minv = " << Minv << endl;
START_CHRONO(inv);
- for (int i = 0; i < BENCH; ++i)
- dynamicgraph::pseudoInverse(M, Minv);
+ for (int i = 0; i < BENCH; ++i) dynamicgraph::pseudoInverse(M, Minv);
STOP_CHRONO(inv, "M+standard");
cout << dt_inv << endl;
@@ -201,15 +196,13 @@ int main(int argc, char **argv) {
dynamicgraph::Matrix Mcreuse;
dynamicgraph::Matrix Mcreuseinv;
for (int ib = 0; ib < BENCH; ++ib) {
-
double sumsq;
unsigned int parc = 0;
if (!ib) {
nonzeros.clear();
for (unsigned int j = 0; j < c; ++j) {
sumsq = 0.;
- for (unsigned int i = 0; i < r; ++i)
- sumsq += M(i, j) * M(i, j);
+ for (unsigned int i = 0; i < r; ++i) sumsq += M(i, j) * M(i, j);
if (sumsq > 1e-6) {
nonzeros.push_back(j);
parc++;
@@ -237,8 +230,7 @@ int main(int argc, char **argv) {
Minv.fill(0.);
for (std::list<unsigned int>::iterator iter = nonzeros.begin();
iter != nonzeros.end(); ++iter) {
- for (unsigned int i = 0; i < r; ++i)
- Minv(*iter, i) = Mcreuseinv(parc, i);
+ for (unsigned int i = 0; i < r; ++i) Minv(*iter, i) = Mcreuseinv(parc, i);
parc++;
}
@@ -254,14 +246,12 @@ int main(int argc, char **argv) {
// sotDEBUG(15) << dynamicgraph::MATLAB <<"Minv = "<< Minv <<endl;
{
-
double sumsq;
nonzeros.clear();
unsigned int parc = 0;
for (unsigned int j = 0; j < c; ++j) {
sumsq = 0.;
- for (unsigned int i = 0; i < r; ++i)
- sumsq += M(i, j) * M(i, j);
+ for (unsigned int i = 0; i < r; ++i) sumsq += M(i, j) * M(i, j);
if (sumsq > 1e-6) {
nonzeros.push_back(j);
parc++;
@@ -289,8 +279,7 @@ int main(int argc, char **argv) {
Minv.fill(0.);
for (std::list<unsigned int>::iterator iter = nonzeros.begin();
iter != nonzeros.end(); ++iter) {
- for (unsigned int i = 0; i < r; ++i)
- Minv(*iter, i) = Mcreuseinv(parc, i);
+ for (unsigned int i = 0; i < r; ++i) Minv(*iter, i) = Mcreuseinv(parc, i);
parc++;
}
diff --git a/tests/tools/test_control_pd.cpp b/tests/tools/test_control_pd.cpp
index ef26fb6f..1f20f3db 100644
--- a/tests/tools/test_control_pd.cpp
+++ b/tests/tools/test_control_pd.cpp
@@ -17,6 +17,7 @@ using namespace std;
#include <dynamic-graph/entity.h>
#include <dynamic-graph/factory.h>
+
#include <sot/core/control-pd.hh>
#include <sstream>
diff --git a/tests/tools/test_device.cpp b/tests/tools/test_device.cpp
index a2edb35b..2562d789 100644
--- a/tests/tools/test_device.cpp
+++ b/tests/tools/test_device.cpp
@@ -6,9 +6,8 @@
*
*/
-#include <pinocchio/multibody/liegroup/special-euclidean.hpp>
-
#include <iostream>
+#include <pinocchio/multibody/liegroup/special-euclidean.hpp>
#include <sot/core/debug.hh>
#ifndef WIN32
@@ -17,9 +16,10 @@
using namespace std;
-#include <boost/test/unit_test.hpp>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/factory.h>
+
+#include <boost/test/unit_test.hpp>
#include <sot/core/device.hh>
#include <sstream>
@@ -28,7 +28,7 @@ using namespace dynamicgraph::sot;
namespace dg = dynamicgraph;
class TestDevice : public dg::sot::Device {
-public:
+ public:
TestDevice(const std::string &RobotName) : Device(RobotName) {
timestep_ = 0.001;
}
@@ -36,7 +36,6 @@ public:
};
BOOST_AUTO_TEST_CASE(test_device) {
-
TestDevice aDevice(std::string("simple_humanoid"));
/// Fix constant vector for the control entry in position
@@ -62,7 +61,7 @@ BOOST_AUTO_TEST_CASE(test_device) {
aControlVector(i) = 0.1;
}
- dg::Vector expected = aStateVector; // backup initial state vector
+ dg::Vector expected = aStateVector; // backup initial state vector
/// Specify state size
aDevice.setStateSize(38);
@@ -75,7 +74,7 @@ BOOST_AUTO_TEST_CASE(test_device) {
/// Specify velocity bounds
aDevice.setVelocityBounds(aLowerVelBound, anUpperVelBound);
/// Specify current state value
- aDevice.setState(aStateVector); // entry signal in position
+ aDevice.setState(aStateVector); // entry signal in position
/// Specify constant control value
aDevice.controlSIN.setConstant(aControlVector);
@@ -118,8 +117,7 @@ BOOST_AUTO_TEST_CASE(test_device) {
.toRotationMatrix()
.eulerAngles(2, 1, 0)
.reverse();
- for (int i = 6; i < expected.size(); i++)
- expected[i] = 0.3;
+ for (int i = 6; i < expected.size(); i++) expected[i] = 0.3;
std::cout << expected.transpose() << std::endl;
std::cout << aDevice.stateSOUT(N).transpose() << std::endl;
diff --git a/tests/tools/test_mailbox.cpp b/tests/tools/test_mailbox.cpp
index 256c51ea..3c059f22 100644
--- a/tests/tools/test_mailbox.cpp
+++ b/tests/tools/test_mailbox.cpp
@@ -18,6 +18,7 @@ using namespace std;
#include <dynamic-graph/entity.h>
#include <dynamic-graph/factory.h>
+
#include <sot/core/feature-abstract.hh>
#include <sot/core/mailbox-vector.hh>
#include <sstream>
@@ -34,8 +35,7 @@ void f(void) {
Vector vect2(25);
for (int i = 0; i < 250; ++i) {
std::cout << " iter " << i << std::endl;
- for (int j = 0; j < 25; ++j)
- vect(j) = j + i * 10;
+ for (int j = 0; j < 25; ++j) vect(j) = j + i * 10;
mailbox->post(vect);
Vector V = mailbox->getObject(vect2, 1);
std::cout << vect2 << std::endl;
diff --git a/tests/tools/test_matrix.cpp b/tests/tools/test_matrix.cpp
index bcfefb9a..7f6e8c70 100644
--- a/tests/tools/test_matrix.cpp
+++ b/tests/tools/test_matrix.cpp
@@ -12,10 +12,10 @@
/* -------------------------------------------------------------------------- */
#include <dynamic-graph/linear-algebra.h>
-#include <sot/core/debug.hh>
-#include <sot/core/feature-abstract.hh>
#include <iostream>
+#include <sot/core/debug.hh>
+#include <sot/core/feature-abstract.hh>
using namespace std;
#include <sot/core/sot.hh>
@@ -26,10 +26,10 @@ using namespace std;
#include <sot/core/utils-windows.hh>
#endif /*WIN32*/
-#define sotCHRONO1 \
- gettimeofday(&t1, NULL); \
- dt = ((t1.tv_sec - t0.tv_sec) * 1000. + \
- (t1.tv_usec - t0.tv_usec + 0.) / 1000.); \
+#define sotCHRONO1 \
+ gettimeofday(&t1, NULL); \
+ dt = ((t1.tv_sec - t0.tv_sec) * 1000. + \
+ (t1.tv_usec - t0.tv_usec + 0.) / 1000.); \
cout << "dt: " << dt
int main(int, char **) {
@@ -42,11 +42,9 @@ int main(int, char **) {
dynamicgraph::Matrix J(6, 40);
dynamicgraph::Matrix JK(6, 40);
for (int i = 0; i < 40; ++i)
- for (int j = 0; j < 40; ++j)
- P(i, j) = (rand() + 1.) / RAND_MAX;
+ for (int j = 0; j < 40; ++j) P(i, j) = (rand() + 1.) / RAND_MAX;
for (int i = 0; i < J.rows(); ++i)
- for (int j = 0; j < J.cols(); ++j)
- J(i, j) = (rand() + 1.) / RAND_MAX;
+ for (int j = 0; j < J.cols(); ++j) J(i, j) = (rand() + 1.) / RAND_MAX;
int nbIter = 100000;
dt = 0;
diff --git a/tests/tools/test_robot_utils.cpp b/tests/tools/test_robot_utils.cpp
index 47169e8f..eceb81e3 100644
--- a/tests/tools/test_robot_utils.cpp
+++ b/tests/tools/test_robot_utils.cpp
@@ -10,6 +10,7 @@
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
#include <dynamic-graph/factory.h>
+
#include <iostream>
#include <sot/core/debug.hh>
#include <sot/core/robot-utils.hh>
diff --git a/tests/tools/test_sot_loader.cpp b/tests/tools/test_sot_loader.cpp
index b388da22..994f0e8c 100644
--- a/tests/tools/test_sot_loader.cpp
+++ b/tests/tools/test_sot_loader.cpp
@@ -8,15 +8,14 @@
*/
#define BOOST_TEST_MODULE test_sot_loader
-#include <boost/test/included/unit_test.hpp>
-
-#include <fstream>
-#include <iostream>
-
#include <dynamic-graph/entity.h>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/pool.h>
+#include <boost/test/included/unit_test.hpp>
+#include <fstream>
+#include <iostream>
+
#include "sot/core/debug.hh"
#include "sot/core/device.hh"
#include "sot/core/sot-loader.hh"
@@ -26,7 +25,7 @@ using namespace dynamicgraph::sot;
namespace dg = dynamicgraph;
class TestDevice : public dg::sot::Device {
-public:
+ public:
TestDevice(const std::string &RobotName) : Device(RobotName) {
timestep_ = 0.001;
}
@@ -162,9 +161,10 @@ BOOST_FIXTURE_TEST_CASE(test_run_python_command_error, TestFixture) {
std::cout << std::quoted(err) << std::endl;
BOOST_CHECK_EQUAL(res, "");
BOOST_CHECK_EQUAL(out, "");
- BOOST_CHECK_EQUAL(err, "Traceback (most recent call last):\n"
- " File \"<string>\", line 1, in <module>\n"
- "NameError: name 'a' is not defined\n");
+ BOOST_CHECK_EQUAL(err,
+ "Traceback (most recent call last):\n"
+ " File \"<string>\", line 1, in <module>\n"
+ "NameError: name 'a' is not defined\n");
}
BOOST_FIXTURE_TEST_CASE(test_run_python_scripts, TestFixture) {
@@ -182,12 +182,13 @@ BOOST_FIXTURE_TEST_CASE(test_run_python_scripts_failure, TestFixture) {
sot_loader.initialization();
std::string err;
sot_loader.runPythonFile(bad_python_script_, err);
- BOOST_CHECK_EQUAL(err, "Traceback (most recent call last):\n"
- " File \"" +
- bad_python_script_ +
- "\", line 2, in <module>\n"
- " print(b)\n"
- "NameError: name 'b' is not defined\n");
+ BOOST_CHECK_EQUAL(err,
+ "Traceback (most recent call last):\n"
+ " File \"" +
+ bad_python_script_ +
+ "\", line 2, in <module>\n"
+ " print(b)\n"
+ "NameError: name 'b' is not defined\n");
}
BOOST_FIXTURE_TEST_CASE(test_load_device_in_python, TestFixture) {
diff --git a/tests/traces/files.cpp b/tests/traces/files.cpp
index 9e6bb176..d3f88659 100644
--- a/tests/traces/files.cpp
+++ b/tests/traces/files.cpp
@@ -7,16 +7,14 @@
*
*/
-#include <iostream>
-#include <sot/core/debug.hh>
-
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/path.hpp>
+#include <iostream>
+#include <sot/core/debug.hh>
using namespace std;
int main() {
-
boost::filesystem::create_directory("foobar");
ofstream file("foobar/cheeze");
file << "tastes good!\n";
diff --git a/tests/traces/test_traces.cpp b/tests/traces/test_traces.cpp
index 74edac2b..1917da71 100644
--- a/tests/traces/test_traces.cpp
+++ b/tests/traces/test_traces.cpp
@@ -7,13 +7,13 @@
*
*/
-#include <iostream>
-
#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/signal-base.h>
#include <dynamic-graph/signal-time-dependent.h>
#include <dynamic-graph/signal.h>
#include <dynamic-graph/tracer.h>
+
+#include <iostream>
#include <sot/core/debug.hh>
using namespace std;
--
GitLab